eaglesfe
/
robotcode
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Goog

This commit is contained in:
sihan 2024-03-09 15:04:01 -06:00
parent 066d693d91
commit dbf6d85e9f
8 changed files with 92 additions and 107 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

112
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Drive.java
View File

@ -1,56 +1,56 @@
package org.firstinspires.ftc.teamcode.hardware;
import static org.firstinspires.ftc.teamcode.util.Constants.WHEEL_BACK_LEFT;
import static org.firstinspires.ftc.teamcode.util.Constants.WHEEL_BACK_RIGHT;
import static org.firstinspires.ftc.teamcode.util.Constants.WHEEL_FRONT_LEFT;
import static org.firstinspires.ftc.teamcode.util.Constants.WHEEL_FRONT_RIGHT;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.HardwareMap;
public class Drive {
private DcMotor frontLeft;
private DcMotor frontRight;
private DcMotor backLeft;
private DcMotor backRight;
public Drive(HardwareMap hardwareMap) {
// Drive
this.frontLeft = hardwareMap.get(DcMotor.class, WHEEL_FRONT_LEFT);
this.frontRight = hardwareMap.get(DcMotor.class, WHEEL_FRONT_RIGHT);
this.backLeft = hardwareMap.get(DcMotor.class, WHEEL_BACK_LEFT);
this.backRight = hardwareMap.get(DcMotor.class, WHEEL_BACK_RIGHT);
this.frontLeft.setDirection(DcMotor.Direction.REVERSE);
this.frontRight.setDirection(DcMotor.Direction.FORWARD);
this.backLeft.setDirection(DcMotor.Direction.REVERSE);
this.backRight.setDirection(DcMotor.Direction.FORWARD);
this.frontLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
this.frontRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
this.backLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
this.backRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
}
public void setInput(double x, double y, double z) {
// instantiate motor power variables
double flPower, frPower, blPower, brPower;
flPower = x + y + z;
frPower = -x + y - z;
blPower = -x + y + z;
brPower = x + y - z;
double max = Math.max(Math.max(flPower, frPower), Math.max(blPower, brPower));
if (max > 1) {
flPower /= max;
frPower /= max;
blPower /= max;
brPower /= max;
}
// actually set the motor powers
frontLeft.setPower(flPower);
frontRight.setPower(frPower);
backLeft.setPower(blPower);
backRight.setPower(brPower);
}
}
//package org.firstinspires.ftc.teamcode.hardware;
//
//import static org.firstinspires.ftc.teamcode.util.Constants.WHEEL_BACK_LEFT;
//import static org.firstinspires.ftc.teamcode.util.Constants.WHEEL_BACK_RIGHT;
//import static org.firstinspires.ftc.teamcode.util.Constants.WHEEL_FRONT_LEFT;
//import static org.firstinspires.ftc.teamcode.util.Constants.WHEEL_FRONT_RIGHT;
//
//import com.qualcomm.robotcore.hardware.DcMotor;
//import com.qualcomm.robotcore.hardware.HardwareMap;
//
//public class Drive {
// private DcMotor frontLeft;
// private DcMotor frontRight;
// private DcMotor backLeft;
// private DcMotor backRight;
//
// public Drive(HardwareMap hardwareMap) {
// // Drive
// this.frontLeft = hardwareMap.get(DcMotor.class, WHEEL_FRONT_LEFT);
// this.frontRight = hardwareMap.get(DcMotor.class, WHEEL_FRONT_RIGHT);
// this.backLeft = hardwareMap.get(DcMotor.class, WHEEL_BACK_LEFT);
// this.backRight = hardwareMap.get(DcMotor.class, WHEEL_BACK_RIGHT);
// this.frontLeft.setDirection(DcMotor.Direction.REVERSE);
// this.frontRight.setDirection(DcMotor.Direction.FORWARD);
// this.backLeft.setDirection(DcMotor.Direction.REVERSE);
// this.backRight.setDirection(DcMotor.Direction.FORWARD);
// this.frontLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
// this.frontRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
// this.backLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
// this.backRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
// }
//
// public void setInput(double x, double y, double z) {
// // instantiate motor power variables
// double flPower, frPower, blPower, brPower;
//
// flPower = x + y + z;
// frPower = -x + y - z;
// blPower = -x + y + z;
// brPower = x + y - z;
//
// double max = Math.max(Math.max(flPower, frPower), Math.max(blPower, brPower));
// if (max > 1) {
// flPower /= max;
// frPower /= max;
// blPower /= max;
// brPower /= max;
// }
//
// // actually set the motor powers
// frontLeft.setPower(flPower);
// frontRight.setPower(frPower);
// backLeft.setPower(blPower);
// backRight.setPower(brPower);
// }
//}

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java
View File

@ -60,7 +60,7 @@ public class Robot {
this.arm = new Arm().init(hardwareMap);
this.wrist = new Wrist().init(hardwareMap);
this.claw = new Claw().init(hardwareMap);
// this.camera = new Camera(hardwareMap);
this.camera = new Camera(hardwareMap);
this.plane = new Plane().init(hardwareMap);
this.slides = new Slides().init(hardwareMap);
// this.led = new Led().init(hardwareMap);

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/DriveConstants.java
View File

@ -71,7 +71,7 @@ public class DriveConstants {
public static double MAX_VEL = 80;
public static double MAX_ACCEL = 80;
public static double MAX_ANG_VEL = Math.toRadians(360);
public static double MAX_ANG_ACCEL = Math.toRadians(660);
public static double MAX_ANG_ACCEL = Math.toRadians(360);
/*
* Adjust the orientations here to match your robot. See the FTC SDK documentation for details.

12
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/MecanumDrive.java
View File

@ -59,9 +59,9 @@ import java.util.List;
* Simple mecanum drive hardware implementation for REV hardware.
*/
@Config
public class MecanumDrive extends com.acmerobotics.roadrunner.drive.MecanumDrive {
public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(20, 0, 1.5);
public static PIDCoefficients HEADING_PID = new PIDCoefficients(10, 0, 2);
public class MecanumDrive extends com.acmerobotics.roadrunner.drive.MecanumDrive {
public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(3, 0, 2.5);
public static PIDCoefficients HEADING_PID = new PIDCoefficients(12, 0, .5);
public static double LATERAL_MULTIPLIER = 1;
@ -113,8 +113,8 @@ public class MecanumDrive extends com.acmerobotics.roadrunner.drive.MecanumDr
this.leftFront.setDirection(DcMotor.Direction.REVERSE);
this.rightFront.setDirection(DcMotor.Direction.FORWARD);
this.leftRear.setDirection(DcMotor.Direction.REVERSE);
this.rightFront.setDirection(DcMotor.Direction.FORWARD);
this.rightRear.setDirection(DcMotor.Direction.FORWARD);
this.leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
this.rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
@ -332,8 +332,8 @@ public class MecanumDrive extends com.acmerobotics.roadrunner.drive.MecanumDr
this.setWeightedDrivePower(
new Pose2d(
gamepad1.left_stick_y * speedScale,
gamepad1.left_stick_x * speedScale,
-gamepad1.left_stick_y * speedScale,
-gamepad1.left_stick_x * speedScale,
-gamepad1.right_stick_x * turnScale
));
}

6
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/TwoWheelTrackingLocalizer.java
View File

@ -62,11 +62,11 @@ public class TwoWheelTrackingLocalizer extends TwoTrackingWheelLocalizer {
this.drive = drive;
parallelEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "rightRear"));
perpendicularEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "leftFront"));
parallelEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "leftFront"));
perpendicularEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "rightRear"));
// TODO: reverse any encoders using Encoder.setDirection(Encoder.Direction.REVERSE)
parallelEncoder.setDirection(Encoder.Direction.REVERSE);
// parallelEncoder.setDirection(Encoder.Direction.REVERSE);
}
public static double encoderTicksToInches(double ticks) {

13
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/TurnTest.java
View File

@ -36,13 +36,12 @@ public class TurnTest extends LinearOpMode {
if (controller.wasJustPressed(GamepadKeys.Button.B)) {
TrajectoryBuilder builder = drive.trajectoryBuilder(drive.getPoseEstimate());
if (zig) {
zig = false;
builder.forward(DISTANCE);
} else {
zig = true;
builder.forward(-DISTANCE);
}
builder.forward(DISTANCE);
drive.followTrajectory(builder.build());
}
if (controller.wasJustPressed(GamepadKeys.Button.Y)) {
TrajectoryBuilder builder = drive.trajectoryBuilder(drive.getPoseEstimate());
builder.back(DISTANCE);
drive.followTrajectory(builder.build());
}
}

40
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRed.java
View File

@ -7,16 +7,14 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.teamcode.hardware.Robot;
import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants;
import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive;
import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
@Autonomous(name = "autoRed")
public class AutoRed extends LinearOpMode {
protected Pose2d initialPosition;
private Robot robot;
private String randomization;
private String parkLocation;
private String randomization = "CENTER";
private String parkLocation = "LEFT";
//Pose2ds
//Preloads
@ -56,19 +54,13 @@ public class AutoRed extends LinearOpMode {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
switch (randomization) {
case "LEFT":
builder.lineToLinearHeading(LEFT_BOARD,
MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(20));
builder.lineToLinearHeading(LEFT_BOARD);
break;
case "CENTER":
builder.lineToLinearHeading(CENTER_BOARD,
MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(20));
builder.lineToLinearHeading(CENTER_BOARD);
break;
case "RIGHT":
builder.lineToLinearHeading(RIGHT_BOARD,
MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(20));
builder.lineToLinearHeading(RIGHT_BOARD);
break;
}
builder.addTemporalMarker(.2, robot.getArm()::armScore);
@ -114,18 +106,22 @@ public class AutoRed extends LinearOpMode {
// Do super fancy chinese shit
while (!this.isStarted()) {
this.telemetry.addData("Starting Position", this.robot.getCamera().getStartingPosition());
randomization = String.valueOf(this.robot.getCamera().getStartingPosition());
parkLocation();
// randomization = String.valueOf(this.robot.getCamera().getStartingPosition());
// parkLocation();
randomization = "CENTER";
this.telemetry.addData("Park Position", parkLocation);
this.telemetry.update();
}
scorePreloadOne();
boardScore();
sleep(250);
this.robot.getClaw().open();
sleep(250);
park();
while (!isStopRequested()) {
robot.update();
scorePreloadOne();
boardScore();
sleep(250);
this.robot.getClaw().open();
sleep(250);
park();
stop();
}
}
}

12
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Constants.java
View File

@ -37,18 +37,8 @@ public class Constants {
public static final Detection INVALID_DETECTION = new Detection(new Size(0, 0), 0);
// Hardware Name Constants
public static final String WHEEL_FRONT_LEFT = "frontLeft";
public static final String WHEEL_FRONT_RIGHT = "frontRight";
public static final String WHEEL_BACK_LEFT = "backLeft";
public static final String WHEEL_BACK_RIGHT = "backRight";
public static final String ARM = "wobbler";
public static final String CLAW = "claw";
public static final String INTAKE = "intake";
public static final String INTAKE_SECONDARY = "secondary";
public static final String INTAKE_SHIELD = "shield";
public static final String SHOOTER = "wheel";
public static final String PUSHER = "pusher";
public static final String STACK_WEBCAM = "Stack Webcam";
public static final String TARGETING_WEBCAM = "Targeting Webcam";
public static final String IMU_SENSOR = "imu";
public static final String LEFTHANG = "leftHang";