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; //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_LEFT;
import static org.firstinspires.ftc.teamcode.util.Constants.WHEEL_BACK_RIGHT; //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_LEFT;
import static org.firstinspires.ftc.teamcode.util.Constants.WHEEL_FRONT_RIGHT; //import static org.firstinspires.ftc.teamcode.util.Constants.WHEEL_FRONT_RIGHT;
//
import com.qualcomm.robotcore.hardware.DcMotor; //import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.HardwareMap; //import com.qualcomm.robotcore.hardware.HardwareMap;
//
public class Drive { //public class Drive {
private DcMotor frontLeft; // private DcMotor frontLeft;
private DcMotor frontRight; // private DcMotor frontRight;
private DcMotor backLeft; // private DcMotor backLeft;
private DcMotor backRight; // private DcMotor backRight;
//
public Drive(HardwareMap hardwareMap) { // public Drive(HardwareMap hardwareMap) {
// Drive // // Drive
this.frontLeft = hardwareMap.get(DcMotor.class, WHEEL_FRONT_LEFT); // this.frontLeft = hardwareMap.get(DcMotor.class, WHEEL_FRONT_LEFT);
this.frontRight = hardwareMap.get(DcMotor.class, WHEEL_FRONT_RIGHT); // this.frontRight = hardwareMap.get(DcMotor.class, WHEEL_FRONT_RIGHT);
this.backLeft = hardwareMap.get(DcMotor.class, WHEEL_BACK_LEFT); // this.backLeft = hardwareMap.get(DcMotor.class, WHEEL_BACK_LEFT);
this.backRight = hardwareMap.get(DcMotor.class, WHEEL_BACK_RIGHT); // this.backRight = hardwareMap.get(DcMotor.class, WHEEL_BACK_RIGHT);
this.frontLeft.setDirection(DcMotor.Direction.REVERSE); // this.frontLeft.setDirection(DcMotor.Direction.REVERSE);
this.frontRight.setDirection(DcMotor.Direction.FORWARD); // this.frontRight.setDirection(DcMotor.Direction.FORWARD);
this.backLeft.setDirection(DcMotor.Direction.REVERSE); // this.backLeft.setDirection(DcMotor.Direction.REVERSE);
this.backRight.setDirection(DcMotor.Direction.FORWARD); // this.backRight.setDirection(DcMotor.Direction.FORWARD);
this.frontLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); // this.frontLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
this.frontRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); // this.frontRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
this.backLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); // this.backLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
this.backRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); // this.backRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
} // }
//
public void setInput(double x, double y, double z) { // public void setInput(double x, double y, double z) {
// instantiate motor power variables // // instantiate motor power variables
double flPower, frPower, blPower, brPower; // double flPower, frPower, blPower, brPower;
//
flPower = x + y + z; // flPower = x + y + z;
frPower = -x + y - z; // frPower = -x + y - z;
blPower = -x + y + z; // blPower = -x + y + z;
brPower = x + y - z; // brPower = x + y - z;
//
double max = Math.max(Math.max(flPower, frPower), Math.max(blPower, brPower)); // double max = Math.max(Math.max(flPower, frPower), Math.max(blPower, brPower));
if (max > 1) { // if (max > 1) {
flPower /= max; // flPower /= max;
frPower /= max; // frPower /= max;
blPower /= max; // blPower /= max;
brPower /= max; // brPower /= max;
} // }
//
// actually set the motor powers // // actually set the motor powers
frontLeft.setPower(flPower); // frontLeft.setPower(flPower);
frontRight.setPower(frPower); // frontRight.setPower(frPower);
backLeft.setPower(blPower); // backLeft.setPower(blPower);
backRight.setPower(brPower); // 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.arm = new Arm().init(hardwareMap);
this.wrist = new Wrist().init(hardwareMap); this.wrist = new Wrist().init(hardwareMap);
this.claw = new Claw().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.plane = new Plane().init(hardwareMap);
this.slides = new Slides().init(hardwareMap); this.slides = new Slides().init(hardwareMap);
// this.led = new Led().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_VEL = 80;
public static double MAX_ACCEL = 80; public static double MAX_ACCEL = 80;
public static double MAX_ANG_VEL = Math.toRadians(360); 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. * Adjust the orientations here to match your robot. See the FTC SDK documentation for details.

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

@ -60,8 +60,8 @@ import java.util.List;
*/ */
@Config @Config
public class MecanumDrive extends com.acmerobotics.roadrunner.drive.MecanumDrive { public class MecanumDrive extends com.acmerobotics.roadrunner.drive.MecanumDrive {
public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(20, 0, 1.5); public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(3, 0, 2.5);
public static PIDCoefficients HEADING_PID = new PIDCoefficients(10, 0, 2); public static PIDCoefficients HEADING_PID = new PIDCoefficients(12, 0, .5);
public static double LATERAL_MULTIPLIER = 1; 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.leftFront.setDirection(DcMotor.Direction.REVERSE);
this.rightFront.setDirection(DcMotor.Direction.FORWARD);
this.leftRear.setDirection(DcMotor.Direction.REVERSE); this.leftRear.setDirection(DcMotor.Direction.REVERSE);
this.rightFront.setDirection(DcMotor.Direction.FORWARD);
this.rightRear.setDirection(DcMotor.Direction.FORWARD); this.rightRear.setDirection(DcMotor.Direction.FORWARD);
this.leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); this.leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
this.rightFront.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( this.setWeightedDrivePower(
new Pose2d( new Pose2d(
gamepad1.left_stick_y * speedScale, -gamepad1.left_stick_y * speedScale,
gamepad1.left_stick_x * speedScale, -gamepad1.left_stick_x * speedScale,
-gamepad1.right_stick_x * turnScale -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; this.drive = drive;
parallelEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "rightRear")); parallelEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "leftFront"));
perpendicularEncoder = 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) // 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) { public static double encoderTicksToInches(double ticks) {

9
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)) { if (controller.wasJustPressed(GamepadKeys.Button.B)) {
TrajectoryBuilder builder = drive.trajectoryBuilder(drive.getPoseEstimate()); TrajectoryBuilder builder = drive.trajectoryBuilder(drive.getPoseEstimate());
if (zig) {
zig = false;
builder.forward(DISTANCE); builder.forward(DISTANCE);
} else { drive.followTrajectory(builder.build());
zig = true;
builder.forward(-DISTANCE);
} }
if (controller.wasJustPressed(GamepadKeys.Button.Y)) {
TrajectoryBuilder builder = drive.trajectoryBuilder(drive.getPoseEstimate());
builder.back(DISTANCE);
drive.followTrajectory(builder.build()); drive.followTrajectory(builder.build());
} }
} }

28
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 com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.teamcode.hardware.Robot; 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; import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
@Autonomous(name = "autoRed") @Autonomous(name = "autoRed")
public class AutoRed extends LinearOpMode { public class AutoRed extends LinearOpMode {
protected Pose2d initialPosition; protected Pose2d initialPosition;
private Robot robot; private Robot robot;
private String randomization; private String randomization = "CENTER";
private String parkLocation; private String parkLocation = "LEFT";
//Pose2ds //Pose2ds
//Preloads //Preloads
@ -56,19 +54,13 @@ public class AutoRed extends LinearOpMode {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
switch (randomization) { switch (randomization) {
case "LEFT": case "LEFT":
builder.lineToLinearHeading(LEFT_BOARD, builder.lineToLinearHeading(LEFT_BOARD);
MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(20));
break; break;
case "CENTER": case "CENTER":
builder.lineToLinearHeading(CENTER_BOARD, builder.lineToLinearHeading(CENTER_BOARD);
MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(20));
break; break;
case "RIGHT": case "RIGHT":
builder.lineToLinearHeading(RIGHT_BOARD, builder.lineToLinearHeading(RIGHT_BOARD);
MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(20));
break; break;
} }
builder.addTemporalMarker(.2, robot.getArm()::armScore); builder.addTemporalMarker(.2, robot.getArm()::armScore);
@ -114,18 +106,22 @@ public class AutoRed extends LinearOpMode {
// Do super fancy chinese shit // Do super fancy chinese shit
while (!this.isStarted()) { while (!this.isStarted()) {
this.telemetry.addData("Starting Position", this.robot.getCamera().getStartingPosition()); this.telemetry.addData("Starting Position", this.robot.getCamera().getStartingPosition());
randomization = String.valueOf(this.robot.getCamera().getStartingPosition()); // randomization = String.valueOf(this.robot.getCamera().getStartingPosition());
parkLocation(); // parkLocation();
randomization = "CENTER";
this.telemetry.addData("Park Position", parkLocation); this.telemetry.addData("Park Position", parkLocation);
this.telemetry.update(); this.telemetry.update();
} }
while (!isStopRequested()) {
robot.update();
scorePreloadOne(); scorePreloadOne();
boardScore(); boardScore();
sleep(250); sleep(250);
this.robot.getClaw().open(); this.robot.getClaw().open();
sleep(250); sleep(250);
park(); 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); public static final Detection INVALID_DETECTION = new Detection(new Size(0, 0), 0);
// Hardware Name Constants // 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 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 TARGETING_WEBCAM = "Targeting Webcam";
public static final String IMU_SENSOR = "imu"; public static final String IMU_SENSOR = "imu";
public static final String LEFTHANG = "leftHang"; public static final String LEFTHANG = "leftHang";