ripark2/FTC-Code
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

update

Browse Source
This commit is contained in:
Anirudh Narayanan
2026-02-09 17:15:24 -06:00
parent e7d4ce2d00
commit 28d98c41d9
28 changed files with 2583 additions and 603 deletions
Download Patch File Download Diff File Expand all files Collapse all files

353
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auton/blueDepot.java
View File

@@ -1,138 +1,215 @@
package org.firstinspires.ftc.teamcode.Auton;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryVelocityConstraint;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.DcMotor;
import org.firstinspires.ftc.teamcode.Hware;
import org.firstinspires.ftc.teamcode.drive.DriveConstants;
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
import org.firstinspires.ftc.teamcode.drive.advanced.PoseStorage;
import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence;
@Config
@Autonomous(name = "Blue Depot Cycle")
public class blueDepot extends LinearOpMode {
Hware robot = new Hware();
double TICKS_PER_DEGREE = (384.5 * 9.26) / 360;
public void turretPower(double power) {
robot.turret.setPower(power);
}
public void turretPos(int position) {
turretPower(0);
robot.turret.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
robot.turret.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.turret.setTargetPosition(position * (int) TICKS_PER_DEGREE);
robot.turret.setMode(DcMotor.RunMode.RUN_TO_POSITION);
turretPower(1);
}
public void spintakeStart(){
robot.intake.set(1);
}
public void spintakeEnd(){
robot.intake.set(0);
}
public void shoot(){
robot.topFly.set(1);
robot.bottomFly.set(1);
robot.activeTransfer.setPosition(1);
}
public void reset(){
robot.intake.set(0);
robot.topFly.set(0);
robot.bottomFly.set(0);
robot.launchHood.setPosition(0.3);
robot.activeTransfer.setPosition(0);
}
public void runOpMode() throws InterruptedException {
//webcam1 = hardwareMap.get(WebcamName.class, "Webcam 2");
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
Pose2d startPose = new Pose2d(0, 0, Math.toRadians(0));
drive.setPoseEstimate(startPose);
TrajectorySequence oneCycle = drive.trajectorySequenceBuilder(startPose)
.strafeLeft(25)
.UNSTABLE_addTemporalMarkerOffset(.3, () -> {
shoot();
})
.waitSeconds(1)
.UNSTABLE_addTemporalMarkerOffset(.1, () -> {
reset();
})
.lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0)))
.UNSTABLE_addTemporalMarkerOffset(.1, () -> {
spintakeStart();
})
.forward(30, SampleMecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
.UNSTABLE_addTemporalMarkerOffset(.1, () -> {
spintakeEnd();
})
.lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0)))
.UNSTABLE_addTemporalMarkerOffset(.1, () -> {
shoot();
})
.waitSeconds(1)
.UNSTABLE_addTemporalMarkerOffset(.1, () -> {
reset();
})
.lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0)))
.forward(10)
.UNSTABLE_addTemporalMarkerOffset(.1, () -> {
spintakeStart();
})
.strafeLeft(10)
.forward(3)
.UNSTABLE_addTemporalMarkerOffset(.1, () -> {
spintakeEnd();
})
.lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0)))
.UNSTABLE_addTemporalMarkerOffset(.1, () -> {
shoot();
})
.waitSeconds(1)
.lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0)))
.UNSTABLE_addTemporalMarkerOffset(.1, () -> {
spintakeStart();
})
.forward(30, SampleMecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
.UNSTABLE_addTemporalMarkerOffset(.1, () -> {
spintakeEnd();
})
.lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0)))
.UNSTABLE_addTemporalMarkerOffset(.1, () -> {
shoot();
})
.waitSeconds(1)
.UNSTABLE_addTemporalMarkerOffset(.1, () -> {
reset();
})
.lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0)))
.build();
waitForStart();
if (opModeIsActive() && !isStopRequested()) {
// while (opModeIsActive()) {
drive.followTrajectorySequence(oneCycle);
PoseStorage.currentPose = drive.getPoseEstimate();
// }
}
}
}
package org.firstinspires.ftc.teamcode.Auton;//package org.firstinspires.ftc.teamcode.Auton;
//
//import com.acmerobotics.dashboard.config.Config;
//import com.acmerobotics.roadrunner.geometry.Pose2d;
//import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
//import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
//import com.qualcomm.robotcore.hardware.DcMotor;
//
//import org.firstinspires.ftc.teamcode.OUTDATED.Hware;
//import org.firstinspires.ftc.teamcode.drive.DriveConstants;
//import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
//import org.firstinspires.ftc.teamcode.drive.advanced.PoseStorage;
//import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence;
//
//@Config
//@Autonomous(name = "Blue Depot Cycle")
//public class blueDepot extends LinearOpMode {
//
// Hware robot = new Hware();
// public static double TICKS_PER_DEGREE = 9.738888;
//
// public void turretPower(double power) {
// robot.turret.setPower(power);
// }
//
// public void turretPos(int position) {
// turretPower(0);
// robot.turret.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
// robot.turret.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
// robot.turret.setTargetPosition(position * (int) TICKS_PER_DEGREE);
// robot.turret.setMode(DcMotor.RunMode.RUN_TO_POSITION);
// turretPower(1);
// }
//
// public void spintakeStart(){
// robot.intake.set(1);
// }
//
// public void spintakeEnd(){
// robot.intake.set(0);
// }
//
// public void shoot(){
// robot.topFly.set(0.85);
// robot.bottomFly.set(0.85);
// robot.launchHood.setPosition(0.4);
// }
//
// public void reset(){
// robot.intake.set(0);
//// robot.topFly.set(0);
//// robot.bottomFly.set(0);
// robot.launchHood.setPosition(0.35);
// robot.activeTransfer.setPosition(0.8);
// }
//
//
// public void runOpMode() throws InterruptedException {
//
// //webcam1 = hardwareMap.get(WebcamName.class, "Webcam 2");
// SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
// robot.initialize(hardwareMap);
//
// Pose2d startPose = new Pose2d(0, 0, Math.toRadians(0));
//
// drive.setPoseEstimate(startPose);
//
// TrajectorySequence threeBall = drive.trajectorySequenceBuilder(startPose)
// .UNSTABLE_addTemporalMarkerOffset(.3, () -> {
// reset();
// turretPos(5);
// })
// .strafeLeft(35)
// .UNSTABLE_addTemporalMarkerOffset(.3, () -> {
// shoot();
// })
// .UNSTABLE_addTemporalMarkerOffset(1.5, () -> {
// robot.intake.set(0.9);
// })
// .UNSTABLE_addTemporalMarkerOffset(2, () -> {
// robot.activeTransfer.setPosition(1);
// })
//// .UNSTABLE_addTemporalMarkerOffset(2.2, () -> {
//// robot.launchHood.setPosition(0.5);
//// })
// .waitSeconds(3)
// .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
// reset();
// })
// .lineToLinearHeading(new Pose2d(12,50, Math.toRadians(-45)))
// .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
// spintakeStart();
// })
// .forward(35, SampleMecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
// SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
// .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
// spintakeEnd();
// })
// .lineToLinearHeading(new Pose2d(0,35, Math.toRadians(0)))
// .UNSTABLE_addTemporalMarkerOffset(.3, () -> {
// shoot();
// })
// .UNSTABLE_addTemporalMarkerOffset(1.5, () -> {
// robot.intake.set(0.9);
// })
// .UNSTABLE_addTemporalMarkerOffset(2, () -> {
// robot.activeTransfer.setPosition(1);
// })
//// .UNSTABLE_addTemporalMarkerOffset(2.2, () -> {
//// robot.launchHood.setPosition(0.5);
//// })
// .waitSeconds(3)
// .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
// reset();
// })
// .lineToLinearHeading(new Pose2d(35,60, Math.toRadians(-45)))
// .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
// spintakeStart();
// })
// .forward(35, SampleMecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
// SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
// .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
// spintakeEnd();
// })
// .lineToLinearHeading(new Pose2d(0,35, Math.toRadians(0)))
// .UNSTABLE_addTemporalMarkerOffset(.3, () -> {
// shoot();
// })
// .UNSTABLE_addTemporalMarkerOffset(1.5, () -> {
// robot.intake.set(0.9);
// })
// .UNSTABLE_addTemporalMarkerOffset(2, () -> {
// robot.activeTransfer.setPosition(1);
// })
//// .UNSTABLE_addTemporalMarkerOffset(2.2, () -> {
//// robot.launchHood.setPosition(0.5);
//// })
// .waitSeconds(3)
// .UNSTABLE_addTemporalMarkerOffset(0.5, () -> {
// turretPos(84);
// })
// .lineToLinearHeading(new Pose2d(35,60, Math.toRadians(-45)))
// // .lineToLinearHeading(new Pose2d(55.5,68, Math.toRadians(-45)))
// .build();
//
//// TrajectorySequence oneCycle = drive.trajectorySequenceBuilder(startPose)
//// .strafeLeft(25)
//// .UNSTABLE_addTemporalMarkerOffset(.3, () -> {
//// shoot();
//// })
//// .waitSeconds(1)
//// .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
//// reset();
//// })
//// .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0)))
//// .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
//// spintakeStart();
//// })
//// .forward(30, SampleMecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
//// SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
//// .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
//// spintakeEnd();
//// })
//// .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0)))
//// .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
//// shoot();
//// })
//// .waitSeconds(1)
//// .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
//// reset();
//// })
//// .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0)))
//// .forward(10)
//// .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
//// spintakeStart();
//// })
//// .strafeLeft(10)
//// .forward(3)
//// .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
//// spintakeEnd();
//// })
//// .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0)))
//// .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
//// shoot();
//// })
//// .waitSeconds(1)
//// .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0)))
//// .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
//// spintakeStart();
//// })
//// .forward(30, SampleMecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
//// SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
//// .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
//// spintakeEnd();
//// })
//// .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0)))
//// .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
//// shoot();
//// })
//// .waitSeconds(1)
//// .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
//// reset();
//// })
//// .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0)))
//// .build();
//
// waitForStart();
// if (opModeIsActive() && !isStopRequested()) {
//// while (opModeIsActive()) {
// drive.followTrajectorySequence(threeBall);
// PoseStorage.currentPose = drive.getPoseEstimate();
//// }
// }
// }
//}

80
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auton/blueDepotFar.java Normal file
View File

@@ -0,0 +1,80 @@
package org.firstinspires.ftc.teamcode.Auton;//package org.firstinspires.ftc.teamcode.Auton;
//
//import com.acmerobotics.dashboard.config.Config;
//import com.acmerobotics.roadrunner.geometry.Pose2d;
//import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
//import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
//import com.qualcomm.robotcore.hardware.DcMotor;
//
//import org.firstinspires.ftc.teamcode.OUTDATED.Hware;
//import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
//import org.firstinspires.ftc.teamcode.drive.advanced.PoseStorage;
//import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence;
//
//@Config
//@Autonomous(name = "Blue Depot Far NO USE")
//public class blueDepotFar extends LinearOpMode {
//
// Hware robot = new Hware();
// public static double TICKS_PER_DEGREE = 9.738888;
//
// public void turretPower(double power) {
// robot.turret.setPower(power);
// }
//
// public void turretPos(int position) {
// turretPower(0);
// robot.turret.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
// robot.turret.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
// robot.turret.setTargetPosition(position * (int) TICKS_PER_DEGREE);
// robot.turret.setMode(DcMotor.RunMode.RUN_TO_POSITION);
// turretPower(1);
// }
//
// public void spintakeStart(){
// robot.intake.set(1);
// }
//
// public void spintakeEnd(){
// robot.intake.set(0);
// }
//
// public void shoot(){
// robot.topFly.set(1);
// robot.bottomFly.set(1);
// robot.launchHood.setPosition(0.58);
// }
//
// public void reset(){
// robot.intake.set(0);
//// robot.topFly.set(0);
//// robot.bottomFly.set(0);
// robot.launchHood.setPosition(0.35);
// robot.activeTransfer.setPosition(0.8);
// }
//
//
// public void runOpMode() throws InterruptedException {
//
// //webcam1 = hardwareMap.get(WebcamName.class, "Webcam 2");
// SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
// robot.initialize(hardwareMap);
//
// Pose2d startPose = new Pose2d(0, 0, Math.toRadians(0));
//
// drive.setPoseEstimate(startPose);
//
// TrajectorySequence threeBall = drive.trajectorySequenceBuilder(startPose)
// .waitSeconds(3)
// .forward(5)
// .build();
//
// waitForStart();
// if (opModeIsActive() && !isStopRequested()) {
//// while (opModeIsActive()) {
// drive.followTrajectorySequence(threeBall);
// PoseStorage.currentPose = drive.getPoseEstimate();
//// }
// }
// }
//}

215
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auton/redDepot.java Normal file
View File

@@ -0,0 +1,215 @@
package org.firstinspires.ftc.teamcode.Auton;//package org.firstinspires.ftc.teamcode.Auton;
//
//import com.acmerobotics.dashboard.config.Config;
//import com.acmerobotics.roadrunner.geometry.Pose2d;
//import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
//import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
//import com.qualcomm.robotcore.hardware.DcMotor;
//
//import org.firstinspires.ftc.teamcode.OUTDATED.Hware;
//import org.firstinspires.ftc.teamcode.drive.DriveConstants;
//import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
//import org.firstinspires.ftc.teamcode.drive.advanced.PoseStorage;
//import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence;
//
//@Config
//@Autonomous(name = "Red Depot Cycle")
//public class redDepot extends LinearOpMode {
//
// Hware robot = new Hware();
// public static double TICKS_PER_DEGREE = 9.738888;
//
// public void turretPower(double power) {
// robot.turret.setPower(power);
// }
//
// public void turretPos(int position) {
// turretPower(0);
// robot.turret.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
// robot.turret.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
// robot.turret.setTargetPosition(position * (int) TICKS_PER_DEGREE);
// robot.turret.setMode(DcMotor.RunMode.RUN_TO_POSITION);
// turretPower(1);
// }
//
// public void spintakeStart(){
// robot.intake.set(1);
// }
//
// public void spintakeEnd(){
// robot.intake.set(0);
// }
//
// public void shoot(){
// robot.topFly.set(0.85);
// robot.bottomFly.set(0.85);
// robot.launchHood.setPosition(0.4);
// }
//
// public void reset(){
// robot.intake.set(0);
//// robot.topFly.set(0);
//// robot.bottomFly.set(0);
// robot.launchHood.setPosition(0.35);
// robot.activeTransfer.setPosition(0.8);
// }
//
//
// public void runOpMode() throws InterruptedException {
//
// //webcam1 = hardwareMap.get(WebcamName.class, "Webcam 2");
// SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
// robot.initialize(hardwareMap);
//
// Pose2d startPose = new Pose2d(0, 0, Math.toRadians(0));
//
// drive.setPoseEstimate(startPose);
//
// TrajectorySequence threeBall = drive.trajectorySequenceBuilder(startPose)
// .UNSTABLE_addTemporalMarkerOffset(.3, () -> {
// reset();
// turretPos(-10);
// })
// .strafeRight(35)
// .UNSTABLE_addTemporalMarkerOffset(.3, () -> {
// shoot();
// })
// .UNSTABLE_addTemporalMarkerOffset(1.5, () -> {
// robot.intake.set(0.9);
// })
// .UNSTABLE_addTemporalMarkerOffset(2, () -> {
// robot.activeTransfer.setPosition(1);
// })
//// .UNSTABLE_addTemporalMarkerOffset(2.2, () -> {
//// robot.launchHood.setPosition(0.5);
//// })
// .waitSeconds(3)
// .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
// reset();
// })
// .lineToLinearHeading(new Pose2d(12,-50, Math.toRadians(45)))
// .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
// spintakeStart();
// })
// .forward(35, SampleMecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
// SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
// .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
// spintakeEnd();
// })
// .lineToLinearHeading(new Pose2d(0,-35, Math.toRadians(0)))
// .UNSTABLE_addTemporalMarkerOffset(.3, () -> {
// shoot();
// })
// .UNSTABLE_addTemporalMarkerOffset(1.5, () -> {
// robot.intake.set(0.9);
// })
// .UNSTABLE_addTemporalMarkerOffset(2, () -> {
// robot.activeTransfer.setPosition(1);
// })
//// .UNSTABLE_addTemporalMarkerOffset(2.2, () -> {
//// robot.launchHood.setPosition(0.5);
//// })
// .waitSeconds(3)
// .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
// reset();
// })
// .lineToLinearHeading(new Pose2d(35,-60, Math.toRadians(45)))
// .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
// spintakeStart();
// })
// .forward(35, SampleMecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
// SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
// .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
// spintakeEnd();
// })
// .lineToLinearHeading(new Pose2d(0,-35, Math.toRadians(0)))
// .UNSTABLE_addTemporalMarkerOffset(.3, () -> {
// shoot();
// })
// .UNSTABLE_addTemporalMarkerOffset(1.5, () -> {
// robot.intake.set(0.9);
// })
// .UNSTABLE_addTemporalMarkerOffset(2, () -> {
// robot.activeTransfer.setPosition(1);
// })
//// .UNSTABLE_addTemporalMarkerOffset(2.2, () -> {
//// robot.launchHood.setPosition(0.5);
//// })
// .waitSeconds(3)
// .UNSTABLE_addTemporalMarkerOffset(0.5, () -> {
// turretPos(-84);
// })
// .lineToLinearHeading(new Pose2d(35,-60, Math.toRadians(45)))
// // .lineToLinearHeading(new Pose2d(55.5,68, Math.toRadians(-45)))
// .build();
//
//// TrajectorySequence oneCycle = drive.trajectorySequenceBuilder(startPose)
//// .strafeLeft(25)
//// .UNSTABLE_addTemporalMarkerOffset(.3, () -> {
//// shoot();
//// })
//// .waitSeconds(1)
//// .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
//// reset();
//// })
//// .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0)))
//// .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
//// spintakeStart();
//// })
//// .forward(30, SampleMecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
//// SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
//// .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
//// spintakeEnd();
//// })
//// .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0)))
//// .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
//// shoot();
//// })
//// .waitSeconds(1)
//// .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
//// reset();
//// })
//// .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0)))
//// .forward(10)
//// .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
//// spintakeStart();
//// })
//// .strafeLeft(10)
//// .forward(3)
//// .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
//// spintakeEnd();
//// })
//// .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0)))
//// .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
//// shoot();
//// })
//// .waitSeconds(1)
//// .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0)))
//// .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
//// spintakeStart();
//// })
//// .forward(30, SampleMecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
//// SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
//// .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
//// spintakeEnd();
//// })
//// .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0)))
//// .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
//// shoot();
//// })
//// .waitSeconds(1)
//// .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
//// reset();
//// })
//// .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0)))
//// .build();
//
// waitForStart();
// if (opModeIsActive() && !isStopRequested()) {
//// while (opModeIsActive()) {
// drive.followTrajectorySequence(threeBall);
// PoseStorage.currentPose = drive.getPoseEstimate();
//// }
// }
// }
//}

120
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriverControlsDoubleV1.java
View File

@@ -1,120 +0,0 @@
package org.firstinspires.ftc.teamcode;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.arcrobotics.ftclib.gamepad.GamepadEx;
import com.arcrobotics.ftclib.gamepad.GamepadKeys;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence;
import java.lang.reflect.Method;
@TeleOp
@Config
public class DriverControlsDoubleV1 extends LinearOpMode {
@Override
public void runOpMode() {
Hware robot = new Hware();
robot.initialize(hardwareMap);
GamepadEx driverControl = new GamepadEx(gamepad1);
GamepadEx armControl = new GamepadEx(gamepad2);
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
boolean xPrev = false;
boolean bPrev = false;
final boolean[] shooting = { false };
Pose2d startPose = drive.getPoseEstimate();
TrajectorySequence shootSeq = drive.trajectorySequenceBuilder(startPose)
.addTemporalMarker(0.00, () -> {
robot.intake.set(0);
robot.activeTransfer.setPosition(0.7);
robot.topFly.set(1);
robot.bottomFly.set(1);
})
.waitSeconds(0.50)
.addTemporalMarker(0.00, () -> {
robot.activeTransfer.setPosition(1);
robot.intake.set(1);
})
.waitSeconds(0.70)
.addTemporalMarker(0.00, () -> {
stopAll(robot);
shooting[0] = false;
})
.build();
waitForStart();
while (opModeIsActive()) {
drive.update();
if (!shooting[0]) {
if (driverControl.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER) > 0.5) {
robot.intake.set(1);
robot.activeTransfer.setPosition(0.7);
} else if (driverControl.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER) > 0.5) {
robot.intake.set(-1);
} else {
robot.intake.set(0);
}
}
boolean xNow = armControl.getButton(GamepadKeys.Button.X);
if (xNow && !xPrev && !shooting[0]) {
shooting[0] = true;
drive.followTrajectorySequenceAsync(shootSeq);
}
xPrev = xNow;
boolean bNow = armControl.getButton(GamepadKeys.Button.B);
if (bNow && !bPrev && shooting[0]) {
cancelTrajectorySequenceCompat(drive);
stopAll(robot);
shooting[0] = false;
}
bPrev = bNow;
telemetry.addData("Shooting", shooting[0]);
telemetry.addData("RR Busy", drive.isBusy());
telemetry.update();
}
}
private static void stopAll(Hware robot) {
robot.intake.set(0);
robot.topFly.set(0);
robot.bottomFly.set(0);
robot.activeTransfer.setPosition(0.7);
}
private static void cancelTrajectorySequenceCompat(Object drive) {
if (invokeIfExists(drive, "cancelTrajectorySequence")) return;
if (invokeIfExists(drive, "cancelFollowing")) return;
if (invokeIfExists(drive, "breakFollowing")) return;
try {
Method m = drive.getClass().getMethod("setDrivePower", Pose2d.class);
m.invoke(drive, new Pose2d(0, 0, 0));
} catch (Exception ignored) {}
}
private static boolean invokeIfExists(Object obj, String methodName) {
try {
Method m = obj.getClass().getMethod(methodName);
m.invoke(obj);
return true;
} catch (Exception e) {
return false;
}
}
}

48
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/FlyOp.java Normal file
View File

@@ -0,0 +1,48 @@
package org.firstinspires.ftc.teamcode;
import com.acmerobotics.dashboard.config.Config;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.firstinspires.ftc.teamcode.OUTDATED.Hware;
@Config
@TeleOp(name="FlyOpTuner", group="Test")
public class FlyOp extends LinearOpMode {
private Hware robot;
// Dashboard-editable
public static double TARGET = 1500.0; // ticks/sec (flip sign if needed)
public static double launchPos = 1.0; // servo position (0..1 usually)
@Override
public void runOpMode() throws InterruptedException {
robot = new Hware();
robot.initialize(hardwareMap);
waitForStart();
while (opModeIsActive() && !isStopRequested()) {
robot.bottomFly.setVelocity(TARGET);
robot.topFly.setVelocity(TARGET);
robot.launchHood.setPosition(launchPos);
if (gamepad1.dpadUpWasPressed()) {
robot.intake.set(.8);
}
if (gamepad1.dpadDownWasPressed()) {
robot.intake.set(0);
}
telemetry.addData("Target", (int)Math.round(TARGET));
telemetry.addData("Top vel", (int)Math.round(robot.topFly.getVelocity()));
telemetry.addData("Bottom vel", (int)Math.round(robot.bottomFly.getVelocity()));
telemetry.addData("Hood pos", launchPos);
telemetry.update();
}
}
}

84
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/FlywheelTuner.java Normal file
View File

@@ -0,0 +1,84 @@
package org.firstinspires.ftc.teamcode;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.PIDFCoefficients;
import org.firstinspires.ftc.teamcode.OUTDATED.Hware;
@TeleOp(name="FlywheelTuner", group="Tuning")
public class FlywheelTuner extends LinearOpMode {
@Override
public void runOpMode() throws InterruptedException {
Hware robot = new Hware();
robot.initialize(hardwareMap);
// ticks/sec targets (NOT rpm)
double highVelocity = 1500;
double lowVelocity = -900;
double curTargetVelocity = highVelocity;
double F = 0.0;
double P = 0.0;
double[] stepSizes = {10.0, 1.0, 0.1, 0.01, 0.001, 0.0001};
int stepIndex = 1;
telemetry.addLine("Y = toggle target, B = step size, DPAD U/D = P, DPAD L/R = F");
telemetry.addLine("Press START when ready.");
telemetry.update();
waitForStart();
while (opModeIsActive()) {
// ---- controls ----
if (gamepad1.yWasPressed()) {
curTargetVelocity = (curTargetVelocity == highVelocity) ? lowVelocity : highVelocity;
}
if (gamepad1.bWasPressed()) {
stepIndex = (stepIndex + 1) % stepSizes.length;
}
if (gamepad1.dpadLeftWasPressed()) F -= stepSizes[stepIndex];
if (gamepad1.dpadRightWasPressed()) F += stepSizes[stepIndex];
if (gamepad1.dpadUpWasPressed()) P += stepSizes[stepIndex];
if (gamepad1.dpadDownWasPressed()) P -= stepSizes[stepIndex];
// keep P/F non-negative (usually)
if (P < 0) P = 0;
if (F < 0) F = 0;
// ---- apply PIDF LIVE ----
PIDFCoefficients pidf = new PIDFCoefficients(P, 0, 0, F);
robot.bottomFly.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, pidf);
robot.topFly.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, pidf);
// ---- command velocity LIVE ----
robot.topFly.setVelocity(curTargetVelocity);
robot.bottomFly.setVelocity(curTargetVelocity);
// ---- read velocity ----
double curTopVelocity = robot.topFly.getVelocity();
double curBottomVelocity = robot.bottomFly.getVelocity();
// ---- telemetry ----
telemetry.addData("Target (ticks/s)", "%.1f", curTargetVelocity);
telemetry.addData("Top (ticks/s)", "%.1f", curTopVelocity);
telemetry.addData("Bottom (ticks/s)", "%.1f", curBottomVelocity);
telemetry.addData("Top error", "%.1f", (curTargetVelocity - curTopVelocity));
telemetry.addData("Bottom error", "%.1f", (curTargetVelocity - curBottomVelocity));
telemetry.addLine("-----------");
telemetry.addData("P (DPAD U/D)", "%.6f", P);
telemetry.addData("F (DPAD L/R)", "%.6f", F);
telemetry.addData("Step (B)", "%.6f", stepSizes[stepIndex]);
telemetry.update();
// small delay so button presses feel clean + telemetry readable
}
}
}

62
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/HwareV2.java Normal file
View File

@@ -0,0 +1,62 @@
package org.firstinspires.ftc.teamcode;
import com.arcrobotics.ftclib.hardware.SimpleServo;
import com.arcrobotics.ftclib.hardware.motors.Motor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.VoltageSensor;
public class HwareV2 {
/* ===================== HARDWARE ===================== */
public Motor frontRight, frontLeft, backRight, backLeft;
public Motor intake;
public Motor bottomFly, topFly; // declare flywheels here
public DcMotorEx turret;
public SimpleServo activeTransfer, rightPTO, leftPTO, launchHood;
private VoltageSensor battery;
private HardwareMap hardwareMap;
/* ===================== INITIALIZE ===================== */
public void initialize(HardwareMap hwMap) {
hardwareMap = hwMap;
/* -------- Drivetrain -------- */
frontRight = new Motor(hardwareMap, "frontRight", Motor.GoBILDA.RPM_435);
frontLeft = new Motor(hardwareMap, "frontLeft", Motor.GoBILDA.RPM_435);
backRight = new Motor(hardwareMap, "backRight", Motor.GoBILDA.RPM_435);
backLeft = new Motor(hardwareMap, "backLeft", Motor.GoBILDA.RPM_435);
frontRight.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE);
frontLeft.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE);
backRight.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE);
backLeft.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE);
/* -------- Intake -------- */
intake = new Motor(hardwareMap, "intake", Motor.GoBILDA.RPM_1620);
intake.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE);
/* -------- Flywheels -------- */
bottomFly = new Motor(hardwareMap, "bottomFly", Motor.GoBILDA.BARE);
topFly = new Motor(hardwareMap, "topFly", Motor.GoBILDA.BARE);
/* -------- Turret -------- */
turret = hardwareMap.get(DcMotorEx.class, "turret");
/* -------- Servos -------- */
activeTransfer = new SimpleServo(hardwareMap, "activeTransfer", 0, 100);
leftPTO = new SimpleServo(hardwareMap, "leftPTO", 0, 90);
rightPTO = new SimpleServo(hardwareMap, "rightPTO", 0, 90);
launchHood = new SimpleServo(hardwareMap, "launchHood", 0, 180);
/* -------- Battery -------- */
battery = hardwareMap.voltageSensor.iterator().next();
}
/* ===================== BATTERY ===================== */
public double getBatteryVoltage() {
return battery.getVoltage();
}
}

6
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AlignmentTest.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OUTDATED/AlignmentTest.java
View File

@@ -1,19 +1,15 @@
package org.firstinspires.ftc.teamcode;
package org.firstinspires.ftc.teamcode.OUTDATED;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.control.PIDFController;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.geometry.Vector2d;
import com.arcrobotics.ftclib.drivebase.MecanumDrive;
import com.arcrobotics.ftclib.gamepad.GamepadEx;
import com.arcrobotics.ftclib.gamepad.GamepadKeys;
import com.arcrobotics.ftclib.hardware.motors.Motor;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.Gamepad;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.teamcode.drive.DriveConstants;
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
import org.firstinspires.ftc.teamcode.drive.advanced.PoseStorage;

6
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoLock.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OUTDATED/AutoLock.java
View File

@@ -1,6 +1,4 @@
package org.firstinspires.ftc.teamcode;
import android.util.Size;
package org.firstinspires.ftc.teamcode.OUTDATED;
import com.acmerobotics.dashboard.config.Config;
import com.arcrobotics.ftclib.drivebase.MecanumDrive;
@@ -12,10 +10,8 @@ import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.util.ElapsedTime;
import org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection;
import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.firstinspires.ftc.robotcore.external.navigation.Position;
import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;

11
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OUTDATED/AutonV1.java Normal file
View File

@@ -0,0 +1,11 @@
package org.firstinspires.ftc.teamcode.OUTDATED;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
public class AutonV1 extends LinearOpMode {
@Override
public void runOpMode() throws InterruptedException {
}
}

21
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Hware.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OUTDATED/Hware.java
View File

@@ -1,20 +1,20 @@
package org.firstinspires.ftc.teamcode;
package org.firstinspires.ftc.teamcode.OUTDATED;
import com.arcrobotics.ftclib.hardware.ServoEx;
import com.arcrobotics.ftclib.hardware.SimpleServo;
import com.arcrobotics.ftclib.hardware.motors.Motor;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.PIDFCoefficients;
import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
public class Hware {
HardwareMap hardwareMap;
public Motor frontRight, frontLeft, backRight, backLeft, intake, bottomFly, topFly;
public DcMotor turret;
public Motor frontRight, frontLeft, backRight, backLeft, intake;
public DcMotorEx turret, bottomFly, topFly;
public SimpleServo activeTransfer, rightPTO, leftPTO, launchHood;
WebcamName webcam;
public void initialize(HardwareMap hwMap) {
@@ -32,8 +32,11 @@ public class Hware {
intake = new Motor(hardwareMap, "intake", Motor.GoBILDA.RPM_1620);
// Launch
bottomFly = new Motor(hardwareMap, "bottomFly", Motor.GoBILDA.BARE);
topFly = new Motor(hardwareMap, "topFly", Motor.GoBILDA.BARE);
bottomFly = hardwareMap.get(DcMotorEx.class, "bottomFly");
topFly = hardwareMap.get(DcMotorEx.class, "topFly");
bottomFly.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
topFly.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
// Turret
turret = hardwareMap.get(DcMotorEx.class, "turret");
@@ -55,6 +58,10 @@ public class Hware {
backRight.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE);
intake.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE);
bottomFly.setInverted(true);
PIDFCoefficients pidf = new PIDFCoefficients(0.45, 0, 0, 16.3);
bottomFly.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, pidf);
topFly.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, pidf);
bottomFly.setDirection(DcMotorSimple.Direction.REVERSE);
}
}

319
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OUTDATED/OdometryAutoLock.java Normal file
View File

@@ -0,0 +1,319 @@
package org.firstinspires.ftc.teamcode.OUTDATED;//package org.firstinspires.ftc.teamcode.OUTDATED;
//
//import com.acmerobotics.dashboard.config.Config;
//import com.acmerobotics.roadrunner.control.PIDFController;
//import com.acmerobotics.roadrunner.geometry.Pose2d;
//import com.acmerobotics.roadrunner.geometry.Vector2d;
//import com.arcrobotics.ftclib.gamepad.GamepadEx;
//import com.arcrobotics.ftclib.gamepad.GamepadKeys;
//import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
//import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
//import com.qualcomm.robotcore.hardware.DcMotor;
//
//import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
//import org.firstinspires.ftc.teamcode.drive.SampleMecanumDriveRIGGED;
//import org.firstinspires.ftc.teamcode.drive.advanced.PoseStorage;
//import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence;
//import org.firstinspires.ftc.teamcode.util.PersonalPID;
//
//import java.lang.reflect.Method;
//
//
//@TeleOp (name = "OdoAutoLock")
//@Config
//public class OdometryAutoLock extends LinearOpMode {
//
// // Declare a PIDF Controller to regulate heading
// // Use the same gains as SampleMecanumDrive's heading controller
// private PIDFController headingController = new PIDFController(SampleMecanumDrive.HEADING_PID);
// public static double MAX_LOCK_ANGLE = Math.toRadians(90); // ±90°
//
// // Declare a target vector you'd like your bot to align with
// // Can be any x/y coordinate of your choosing
// private Vector2d targetPosition = new Vector2d(0, 45);
// public static double multiplier = 15;
//
// public static double penguin = 0;
// private PersonalPID controller;
//
// public static double p = 0.01, i = 0, d = 0.0001, f = 0.001;
//
// public static int target = 0;
//
// public static double TICKS_PER_DEGREE = (384.5 * 9.2) / 360;
// public static int TURRET_MIN_TICKS = (int) (TICKS_PER_DEGREE * (-90));
// public static int TURRET_MAX_TICKS = (int) (TICKS_PER_DEGREE * (90));
//
// public static double goalX = 0; //OR 10
// public static double goalY = 40;// OR 0
// public static double goalH = - Math.PI / 2; // OR 3.14159265358979
//
//
// @Override
// public void runOpMode() throws InterruptedException {
//
// /** SETUP **/
//
// // Hardware Map Setup
// Hware robot = new Hware();
// robot.initialize(hardwareMap);
// controller = new PersonalPID(p, i, d, f);
//
// // Gamepad Setup
// GamepadEx driverControl = new GamepadEx(gamepad1);
// GamepadEx armControl = new GamepadEx(gamepad2);
//
// // DriveBase Setup
// SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
// SampleMecanumDriveRIGGED driveRIGGED = new SampleMecanumDriveRIGGED(hardwareMap);
//
// double SPEEDCONTROL = 1;
// Boolean auto = false;
// // Retrieve pose
// drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
// drive.setPoseEstimate(PoseStorage.currentPose);
//
//// double cx = PoseStorage.currentPose.getX();
//// double cy = PoseStorage.currentPose.getY();
////
//// double ch = PoseStorage.currentPose.getHeading();
////
//// double x = cx - 60; //tune this 60
////
//// double y = cy - 60; //TUUUNE
////
//// double h = ch - Math.toRadians(45);
////
//// Pose2d newPose = new Pose2d(x,y,h);
//
//
//
// drive.getLocalizer().setPoseEstimate(PoseStorage.currentPose);
// headingController.setInputBounds(-Math.PI, Math.PI);
//
// boolean xPrev = false;
// boolean bPrev = false;
//
// final boolean[] shooting = { false };
//
// Pose2d startPose = drive.getPoseEstimate();
//
// TrajectorySequence shootSeq = driveRIGGED.trajectorySequenceBuilder(startPose)
// .forward(.1)
// .addDisplacementMarker(()-> {
// robot.topFly.set(1);
// robot.bottomFly.set(1);
// robot.activeTransfer.setPosition(1);
// robot.intake.set(1);
// })
// .build();
//
// TrajectorySequence runUp = driveRIGGED.trajectorySequenceBuilder(startPose)
// .forward(.1)
// .addDisplacementMarker(()-> {
// robot.topFly.set(1);
// robot.bottomFly.set(1);
// })
// .forward(.1)
// .waitSeconds(1.5)
// .forward(.1)
// .addDisplacementMarker(()-> {
// gamepad1.rumbleBlips(2);
// gamepad2.rumbleBlips(2);
// })
// .build();
// robot.activeTransfer.setPosition(0.8);
//
//
//
//
// double offset = 0.0;
//
// waitForStart();
//
// robot.turret.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
//
// while (opModeIsActive()) {
//
// double distance = Math.hypot(drive.getPoseEstimate().getX() - goalX, drive.getPoseEstimate().getY() - goalY);
// double height = 70;
// double launchAngle = Math.toDegrees(Math.atan2(height, distance));
// if(launchAngle > 80){
// launchAngle = 80;
// } else if (launchAngle < 40){
// launchAngle = 40;
// }
// double offsetLaunch = (launchAngle / 40);
// if(offsetLaunch > 0.5){
// offsetLaunch = 0.5;
// }
// double hoodPosition = 0.85 - offsetLaunch;
// robot.launchHood.setPosition(hoodPosition);
// /** DRIVER CONTROLS **/
//
// if (driverControl.getButton(GamepadKeys.Button.RIGHT_BUMPER) || driverControl.getButton(GamepadKeys.Button.LEFT_BUMPER)) { SPEEDCONTROL = .5; } else { SPEEDCONTROL = 1; }
//
// // Read pose
// Pose2d poseEstimate = drive.getLocalizer().getPoseEstimate();
//
// // Declare a drive direction
// // Pose representing desired x, y, and angular velocity
// Pose2d driveDirection = new Pose2d();
//
// // Standard teleop control
// // Convert gamepad input into desired pose velocity
// driveDirection = new Pose2d(
// -gamepad1.left_stick_y * SPEEDCONTROL,
// -gamepad1.left_stick_x * SPEEDCONTROL,
// -gamepad1.right_stick_x * SPEEDCONTROL
// );
//
//
// drive.setWeightedDrivePower(driveDirection);
//
// double headingPose = poseEstimate.getHeading();
// //9.26
//
// double totalHeading = ((Math.toRadians((robot.turret.getCurrentPosition() / TICKS_PER_DEGREE))) + headingPose) % (2 * Math.PI);
////
// double theta = Math.atan2(targetPosition.getY() - drive.getPoseEstimate().getY(), targetPosition.getX() - drive.getPoseEstimate().getX()) - drive.getPoseEstimate().getHeading();
//// theta = Math.atan2(Math.sin(theta), Math.cos(theta));
//
// boolean withinAngle = true;
// //Math.abs(theta) < MAX_LOCK_ANGLE;
//
//// if((realTheta > (2 * Math.PI - 0.5)) || (realTheta < (-2 * Math.PI + 0.5))){
//// robot.turret.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
//// }
//
//
//// controller.setPIDF(p, i, d, f);
//// int armPos = robot.turret.getCurrentPosition();
//// double pid = controller.calculate(armPos, target);
//
// double deltaX = drive.getPoseEstimate().getX() - goalX;
// double deltaY = drive.getPoseEstimate().getY() - goalY;
// double deltaH = drive.getPoseEstimate().getHeading() - goalH;
//
// double desiredTurretAngleDeg = Math.toDegrees(
// Math.atan2(deltaY, deltaX)
// ); double robotHeadingDeg = Math.toDegrees(deltaH); double turretAngleDeg = desiredTurretAngleDeg - robotHeadingDeg;
// // turretAngleDeg = turretAngleDeg; //May or may not be needed for u guys...depends on if CCW or CW is positive deg, etc.
//// Normalize to [-180, 180]
// while (turretAngleDeg > 180) turretAngleDeg -= 360;
// while (turretAngleDeg < -180) turretAngleDeg += 360;
// double targetTurretPos = 0 + (turretAngleDeg * TICKS_PER_DEGREE); //Default is where when the robot is head on with the goal and the turret is facing...llike the zero position ig //180Range is the number of ticks or smth that constitutes 180 degrees on the turret)..tune those vars
//// Clamp to physical limits
// targetTurretPos = Math.max(TURRET_MIN_TICKS, Math.min(targetTurretPos, TURRET_MAX_TICKS));
//
// double desiredTarget = targetTurretPos;
//
// offset += multiplier* (gamepad2.left_trigger - gamepad2.right_trigger);
//
// desiredTarget+=offset;
//
// // Clamp to turret mechanical limits
// desiredTarget = Math.max(
// TURRET_MIN_TICKS,
// Math.min(TURRET_MAX_TICKS, desiredTarget)
// );
//
// robot.turret.setTargetPosition((int) desiredTarget);
// robot.turret.setMode(DcMotor.RunMode.RUN_TO_POSITION);
// robot.turret.setPower(1);
//
//
//
//// target = desiredTarget;
//// controller.setPIDF(p, i, d, f);
//// pid = controller.calculate(armPos, target);
// // robot.turret.setPower(pid);
//
// telemetry.addData("target", desiredTarget);
// telemetry.addData("target", target);
//// telemetry.addData("pid", pid);
//
// // auton updates
// // AUTO END -> take x and y and subtract (subtract x and y)
// // heading add 45
//
// driveRIGGED.update();
//
// if (driverControl.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER) > 0.5) {
// auto = false;
// robot.intake.set(1);
// robot.activeTransfer.setPosition(0.8);
// } else if (driverControl.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER) > 0.5) {
// auto = false;
// robot.intake.set(-1);
// } else {
// if (!auto) {
// robot.intake.set(0);
// }
// }
//
// if(gamepad2.dpad_down) {
// robot.topFly.set(0);
// robot.bottomFly.set(0);
// }
//
//
// if (gamepad2.dpad_up) {
// if(!auto) {
// auto = true;
// driveRIGGED.followTrajectorySequenceAsync(shootSeq);
// }
// }
//
// if (gamepad2.dpad_right) {
// driveRIGGED.followTrajectorySequenceAsync(runUp);
// }
//
// telemetry.addData("Shooting", shooting[0]);
// telemetry.addData("RR Busy", drive.isBusy());
//
// // Update the localizer
// drive.getLocalizer().update();
// // Print pose to telemetry
// telemetry.addData("totalHeading", totalHeading);
// telemetry.addData("theta", theta);
// telemetry.addData("x", poseEstimate.getX());
// telemetry.addData("y", poseEstimate.getY());
// telemetry.addData("heading", poseEstimate.getHeading());
// telemetry.addData("auton", auto);
// telemetry.update();
//
// }
//
//
//
// while (!isStopRequested() && opModeIsActive());
// }
//
// private static void stopAll(Hware robot) {
// robot.intake.set(0);
// robot.topFly.set(0);
// robot.bottomFly.set(0);
// robot.activeTransfer.setPosition(0.85);
// }
//
// private static void cancelTrajectorySequenceCompat(Object drive) {
// if (invokeIfExists(drive, "cancelTrajectorySequence")) return;
// if (invokeIfExists(drive, "cancelFollowing")) return;
// if (invokeIfExists(drive, "breakFollowing")) return;
// try {
// Method m = drive.getClass().getMethod("setDrivePower", Pose2d.class);
// m.invoke(drive, new Pose2d(0, 0, 0));
// } catch (Exception ignored) {}
// }
//
// private static boolean invokeIfExists(Object obj, String methodName) {
// try {
// Method m = obj.getClass().getMethod(methodName);
// m.invoke(obj);
// return true;
// } catch (Exception e) {
// return false;
// }
// }
//}

19
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ServoTesting.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OUTDATED/ServoTesting.java
View File

@@ -1,25 +1,10 @@
package org.firstinspires.ftc.teamcode;
package org.firstinspires.ftc.teamcode.OUTDATED;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.control.PIDFController;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.geometry.Vector2d;
import com.arcrobotics.ftclib.drivebase.MecanumDrive;
import com.arcrobotics.ftclib.gamepad.GamepadEx;
import com.arcrobotics.ftclib.gamepad.GamepadKeys;
import com.arcrobotics.ftclib.hardware.motors.Motor;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.Gamepad;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.teamcode.drive.DriveConstants;
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
import org.firstinspires.ftc.teamcode.drive.advanced.PoseStorage;
import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence;
import java.lang.reflect.Method;
@TeleOp (name = "Servo")
@@ -50,7 +35,7 @@ public class ServoTesting extends LinearOpMode {
if (armControl.getButton(GamepadKeys.Button.DPAD_UP)) {
robot.activeTransfer.setPosition(1);
} else if (armControl.getButton(GamepadKeys.Button.DPAD_DOWN)) {
robot.activeTransfer.setPosition(0.85);
robot.activeTransfer.setPosition(0.8);
}
}

330
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OUTDATED/Testing.java Normal file
View File

@@ -0,0 +1,330 @@
package org.firstinspires.ftc.teamcode.OUTDATED;//package org.firstinspires.ftc.teamcode.OUTDATED;
//
//import com.acmerobotics.dashboard.config.Config;
//import com.acmerobotics.roadrunner.control.PIDFController;
//import com.acmerobotics.roadrunner.geometry.Pose2d;
//import com.acmerobotics.roadrunner.geometry.Vector2d;
//import com.arcrobotics.ftclib.gamepad.GamepadEx;
//import com.arcrobotics.ftclib.gamepad.GamepadKeys;
//import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
//import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
//import com.qualcomm.robotcore.hardware.DcMotor;
//
//import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
//import org.firstinspires.ftc.teamcode.drive.SampleMecanumDriveRIGGED;
//import org.firstinspires.ftc.teamcode.drive.advanced.PoseStorage;
//import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence;
//import org.firstinspires.ftc.teamcode.util.PersonalPID;
//
//import java.lang.reflect.Method;
//
//
//@TeleOp (name = "TeleOp")
//@Config
//public class Testing extends LinearOpMode {
//
// // Declare a PIDF Controller to regulate heading
// // Use the same gains as SampleMecanumDrive's heading controller
// private PIDFController headingController = new PIDFController(SampleMecanumDrive.HEADING_PID);
// public static double MAX_LOCK_ANGLE = Math.toRadians(90); // ±90°
//
// // Declare a target vector you'd like your bot to align with
// // Can be any x/y coordinate of your choosing
// private Vector2d targetPosition = new Vector2d(70, 45);
// public static double multiplier = 15;
//
// public static double penguin = 0;
// private PersonalPID controller;
//
// public static double p = 0.01, i = 0, d = 0.0001, f = 0.001;
//
// public static int target = 0;
//
// public static double TICKS_PER_DEGREE = 9.738888;
// public static int TURRET_MIN_TICKS = (int) (TICKS_PER_DEGREE * (-90));
// public static int TURRET_MAX_TICKS = (int) (TICKS_PER_DEGREE * (90));
//
// public static double goalX = 64;
// public static double goalY = 75;
// public static double goalH = - Math.PI / 2; // OR 3.14159265358979
//
// public static double launchMultiplier = 5;
//
//
//
//
// @Override
// public void runOpMode() throws InterruptedException {
//
// /** SETUP **/
//
// // Hardware Map Setup
// Hware robot = new Hware();
// robot.initialize(hardwareMap);
// controller = new PersonalPID(p, i, d, f);
//
// // Gamepad Setup
// GamepadEx driverControl = new GamepadEx(gamepad1);
// GamepadEx armControl = new GamepadEx(gamepad2);
//
// // DriveBase Setup
// SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
// SampleMecanumDriveRIGGED driveRIGGED = new SampleMecanumDriveRIGGED(hardwareMap);
//
// double SPEEDCONTROL = 1;
// double gamepadLaunch = 0;
//
// Boolean auto = false;
// // Retrieve pose
// drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
// drive.getLocalizer().setPoseEstimate(PoseStorage.currentPose);
// drive.setPoseEstimate(PoseStorage.currentPose);
// headingController.setInputBounds(-Math.PI, Math.PI);
//
// boolean xPrev = false;
// boolean bPrev = false;
//
// final boolean[] shooting = { false };
//
// Pose2d startPose = drive.getPoseEstimate();
//
//
// TrajectorySequence shootSeq = driveRIGGED.trajectorySequenceBuilder(startPose)
// .forward(.1)
// .addDisplacementMarker(()-> {
// robot.topFly.set(1);
// robot.bottomFly.set(1);
// robot.activeTransfer.setPosition(1);
// robot.intake.set(0.9);
// })
// .waitSeconds(.5)
// .build();
//
// TrajectorySequence runUp = driveRIGGED.trajectorySequenceBuilder(startPose)
// .forward(.1)
// .addDisplacementMarker(()-> {
// robot.topFly.set(0.8);
// robot.bottomFly.set(0.8);
// })
// .forward(.1)
// .waitSeconds(1.5)
// .forward(.1)
// .addDisplacementMarker(()-> {
// gamepad1.rumbleBlips(2);
// gamepad2.rumbleBlips(2);
// })
// .build();
// robot.activeTransfer.setPosition(0.78);
//
//
//
//
// double offset = 0.0;
//
// waitForStart();
//
// robot.turret.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
//
// while (opModeIsActive()) {
//
// double distance = Math.hypot(drive.getPoseEstimate().getX() - targetPosition.getX(), drive.getPoseEstimate().getY() - targetPosition.getY());
// double height = 70;
// double launchAngle = Math.toDegrees(Math.atan2(distance, height));
// telemetry.addData("angle", launchAngle);
// if(launchAngle > 70){
// launchAngle = 70;
// } else if (launchAngle < 15){
// launchAngle = 15;
// }
// double offsetLaunch = (launchAngle / (55 * launchMultiplier));
// if(offsetLaunch > 0.5){
// offsetLaunch = 0.5;
// }
// double hoodPosition = 0.35 + offsetLaunch;
// if(gamepad2.right_bumper){
// gamepadLaunch += 0.02;
// } else if(gamepad2.left_bumper){
// gamepadLaunch -= 0.02;
// }
// hoodPosition += gamepadLaunch;
// telemetry.addData("hood", hoodPosition);
// robot.launchHood.setPosition(hoodPosition);
// /** DRIVER CONTROLS **/
//
// if (driverControl.getButton(GamepadKeys.Button.RIGHT_BUMPER) || driverControl.getButton(GamepadKeys.Button.LEFT_BUMPER)) { SPEEDCONTROL = .5; } else { SPEEDCONTROL = 1; }
//
// // Read pose
// Pose2d poseEstimate = drive.getLocalizer().getPoseEstimate();
//
// // Declare a drive direction
// // Pose representing desired x, y, and angular velocity
// Pose2d driveDirection = new Pose2d();
//
// // Standard teleop control
// // Convert gamepad input into desired pose velocity
// driveDirection = new Pose2d(
// -gamepad1.left_stick_y * SPEEDCONTROL,
// -gamepad1.left_stick_x * SPEEDCONTROL,
// -gamepad1.right_stick_x * SPEEDCONTROL
// );
//
//
// drive.setWeightedDrivePower(driveDirection);
//
// double headingPose = poseEstimate.getHeading();
// //9.26
//
// double totalHeading = ((Math.toRadians((robot.turret.getCurrentPosition() / TICKS_PER_DEGREE))) + headingPose) % (2 * Math.PI);
////
// double theta = Math.atan2(targetPosition.getY() - drive.getPoseEstimate().getY(), targetPosition.getX() - drive.getPoseEstimate().getX()) - drive.getPoseEstimate().getHeading();
//// theta = Math.atan2(Math.sin(theta), Math.cos(theta));
//
// boolean withinAngle = true;
// //Math.abs(theta) < MAX_LOCK_ANGLE;
//
//// if((realTheta > (2 * Math.PI - 0.5)) || (realTheta < (-2 * Math.PI + 0.5))){
//// robot.turret.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
//// }
//
//
// controller.setPIDF(p, i, d, f);
// int armPos = robot.turret.getCurrentPosition();
// double pid = controller.calculate(armPos, target);
//
// double deltaX = drive.getPoseEstimate().getX() - goalX;
// double deltaY = drive.getPoseEstimate().getY() - goalY;
// double deltaH = drive.getPoseEstimate().getHeading() - goalH;
//
// double desiredTurretAngleDeg = Math.toDegrees(
// Math.atan2(deltaY, deltaX)
// ); double robotHeadingDeg = Math.toDegrees(deltaH); double turretAngleDeg = desiredTurretAngleDeg - robotHeadingDeg;
// // turretAngleDeg = turretAngleDeg; //May or may not be needed for u guys...depends on if CCW or CW is positive deg, etc.
//// Normalize to [-180, 180]
// while (turretAngleDeg > 180) turretAngleDeg -= 360;
// while (turretAngleDeg < -180) turretAngleDeg += 360;
// double targetTurretPos = 0 + (turretAngleDeg * TICKS_PER_DEGREE); //Default is where when the robot is head on with the goal and the turret is facing...llike the zero position ig //180Range is the number of ticks or smth that constitutes 180 degrees on the turret)..tune those vars
//// Clamp to physical limits
// targetTurretPos = Math.max(TURRET_MIN_TICKS, Math.min(targetTurretPos, TURRET_MAX_TICKS));
//
//
// double desiredTarget = targetTurretPos;
//
// offset += multiplier * (gamepad2.left_trigger - gamepad2.right_trigger);
//
// desiredTarget+=offset;
//
// // Clamp to turret mechanical limits
// desiredTarget = Math.max(
// TURRET_MIN_TICKS,
// Math.min(TURRET_MAX_TICKS, desiredTarget)
// );
//
// desiredTarget = TURRET_MIN_TICKS;
// //forBlueSIde MAX TICKS
//
// offset += multiplier* (gamepad2.left_trigger - gamepad2.right_trigger);
//
// desiredTarget+=offset;
//
// desiredTarget = Math.max(
// TURRET_MIN_TICKS,
// Math.min(TURRET_MAX_TICKS, desiredTarget)
// );
//
//
// robot.turret.setTargetPosition((int) desiredTarget);
// robot.turret.setMode(DcMotor.RunMode.RUN_TO_POSITION);
// robot.turret.setPower(1);
//
//
//
//// target = desiredTarget;
//// controller.setPIDF(p, i, d, f);
//// pid = controller.calculate(armPos, target);
// // robot.turret.setPower(pid);
//
//// telemetry.addData("target", desiredTarget);
//// telemetry.addData("target", target);
//// telemetry.addData("pid", pid);
//// telemetry.update();
//
// driveRIGGED.update();
//
// if (driverControl.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER) > 0.5) {
// auto = false;
// robot.intake.set(0.9);
// } else if (driverControl.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER) > 0.5) {
// auto = false;
// robot.intake.set(-0.9);
// } else {
// if (!auto) {
// robot.intake.set(0);
// }
// }
//
// if(gamepad2.dpad_down) {
// robot.topFly.set(0);
// robot.bottomFly.set(0);
// robot.activeTransfer.setPosition(0.78);
// }
//
//
// if (gamepad2.dpad_up) {
// if(!auto) {
// auto = true;
// driveRIGGED.followTrajectorySequenceAsync(shootSeq);
// }
// }
//
// if (gamepad2.dpad_right) {
// driveRIGGED.followTrajectorySequenceAsync(runUp);
// }
//
//// telemetry.addData("Shooting", shooting[0]);
//// telemetry.addData("RR Busy", drive.isBusy());
//
// // Update the localizer
// drive.getLocalizer().update();
// // Print pose to telemetry
//// telemetry.addData("totalHeading", totalHeading);
//// telemetry.addData("theta", theta);
// telemetry.addData("x", poseEstimate.getX());
// telemetry.addData("y", poseEstimate.getY());
// telemetry.addData("heading", poseEstimate.getHeading());
// telemetry.addData("auton", auto);
// telemetry.update();
//
// }
//
//
//
// while (!isStopRequested() && opModeIsActive());
// }
//
// private static void stopAll(Hware robot) {
// robot.intake.set(0);
// robot.topFly.set(0);
// robot.bottomFly.set(0);
// robot.activeTransfer.setPosition(0.78);
// }
//
// private static void cancelTrajectorySequenceCompat(Object drive) {
// if (invokeIfExists(drive, "cancelTrajectorySequence")) return;
// if (invokeIfExists(drive, "cancelFollowing")) return;
// if (invokeIfExists(drive, "breakFollowing")) return;
// try {
// Method m = drive.getClass().getMethod("setDrivePower", Pose2d.class);
// m.invoke(drive, new Pose2d(0, 0, 0));
// } catch (Exception ignored) {}
// }
//
// private static boolean invokeIfExists(Object obj, String methodName) {
// try {
// Method m = obj.getClass().getMethod(methodName);
// m.invoke(obj);
// return true;
// } catch (Exception e) {
// return false;
// }
// }
//}

330
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OUTDATED/Testingv2.java Normal file
View File

@@ -0,0 +1,330 @@
package org.firstinspires.ftc.teamcode.OUTDATED;//package org.firstinspires.ftc.teamcode.OUTDATED;
//
//import com.acmerobotics.dashboard.config.Config;
//import com.acmerobotics.roadrunner.control.PIDFController;
//import com.acmerobotics.roadrunner.geometry.Pose2d;
//import com.acmerobotics.roadrunner.geometry.Vector2d;
//import com.arcrobotics.ftclib.gamepad.GamepadEx;
//import com.arcrobotics.ftclib.gamepad.GamepadKeys;
//import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
//import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
//import com.qualcomm.robotcore.hardware.DcMotor;
//
//import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
//import org.firstinspires.ftc.teamcode.drive.SampleMecanumDriveRIGGED;
//import org.firstinspires.ftc.teamcode.drive.advanced.PoseStorage;
//import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence;
//import org.firstinspires.ftc.teamcode.util.PersonalPID;
//
//import java.lang.reflect.Method;
//
//
//@TeleOp (name = "TeleOp")
//@Config
//public class Testingv2 extends LinearOpMode {
//
// // Declare a PIDF Controller to regulate heading
// // Use the same gains as SampleMecanumDrive's heading controller
// private PIDFController headingController = new PIDFController(SampleMecanumDrive.HEADING_PID);
// public static double MAX_LOCK_ANGLE = Math.toRadians(90); // ±90°
//
// // Declare a target vector you'd like your bot to align with
// // Can be any x/y coordinate of your choosing
// private Vector2d targetPosition = new Vector2d(70, 45);
// public static double multiplier = 15;
//
// public static double penguin = 0;
// private PersonalPID controller;
//
// public static double p = 0.01, i = 0, d = 0.0001, f = 0.001;
//
// public static int target = 0;
//
// public static double TICKS_PER_DEGREE = 9.738888;
// public static int TURRET_MIN_TICKS = (int) (TICKS_PER_DEGREE * (-90));
// public static int TURRET_MAX_TICKS = (int) (TICKS_PER_DEGREE * (90));
//
// public static double goalX = 64;
// public static double goalY = 75;
// public static double goalH = - Math.PI / 2; // OR 3.14159265358979
//
// public static double launchMultiplier = 5;
//
//
//
//
// @Override
// public void runOpMode() throws InterruptedException {
//
// /** SETUP **/
//
// // Hardware Map Setup
// Hware robot = new Hware();
// robot.initialize(hardwareMap);
// controller = new PersonalPID(p, i, d, f);
//
// // Gamepad Setup
// GamepadEx driverControl = new GamepadEx(gamepad1);
// GamepadEx armControl = new GamepadEx(gamepad2);
//
// // DriveBase Setup
// SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
// SampleMecanumDriveRIGGED driveRIGGED = new SampleMecanumDriveRIGGED(hardwareMap);
//
// double SPEEDCONTROL = 1;
// double gamepadLaunch = 0;
//
// Boolean auto = false;
// // Retrieve pose
// drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
// drive.getLocalizer().setPoseEstimate(PoseStorage.currentPose);
// drive.setPoseEstimate(PoseStorage.currentPose);
// headingController.setInputBounds(-Math.PI, Math.PI);
//
// boolean xPrev = false;
// boolean bPrev = false;
//
// final boolean[] shooting = { false };
//
// Pose2d startPose = drive.getPoseEstimate();
//
//
// TrajectorySequence shootSeq = driveRIGGED.trajectorySequenceBuilder(startPose)
// .forward(.1)
// .addDisplacementMarker(()-> {
// robot.topFly.set(1);
// robot.bottomFly.set(1);
// robot.activeTransfer.setPosition(1);
// robot.intake.set(0.9);
// })
// .waitSeconds(.5)
// .build();
//
// TrajectorySequence runUp = driveRIGGED.trajectorySequenceBuilder(startPose)
// .forward(.1)
// .addDisplacementMarker(()-> {
// robot.topFly.set(0.8);
// robot.bottomFly.set(0.8);
// })
// .forward(.1)
// .waitSeconds(1.5)
// .forward(.1)
// .addDisplacementMarker(()-> {
// gamepad1.rumbleBlips(2);
// gamepad2.rumbleBlips(2);
// })
// .build();
// robot.activeTransfer.setPosition(0.78);
//
//
//
//
// double offset = 0.0;
//
// waitForStart();
//
// robot.turret.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
//
// while (opModeIsActive()) {
//
// double distance = Math.hypot(drive.getPoseEstimate().getX() - targetPosition.getX(), drive.getPoseEstimate().getY() - targetPosition.getY());
// double height = 70;
// double launchAngle = Math.toDegrees(Math.atan2(distance, height));
// telemetry.addData("angle", launchAngle);
// if(launchAngle > 70){
// launchAngle = 70;
// } else if (launchAngle < 15){
// launchAngle = 15;
// }
// double offsetLaunch = (launchAngle / (55 * launchMultiplier));
// if(offsetLaunch > 0.5){
// offsetLaunch = 0.5;
// }
// double hoodPosition = 0.35 + offsetLaunch;
// if(gamepad2.right_bumper){
// gamepadLaunch += 0.02;
// } else if(gamepad2.left_bumper){
// gamepadLaunch -= 0.02;
// }
// hoodPosition += gamepadLaunch;
// telemetry.addData("hood", hoodPosition);
// robot.launchHood.setPosition(hoodPosition);
// /** DRIVER CONTROLS **/
//
// if (driverControl.getButton(GamepadKeys.Button.RIGHT_BUMPER) || driverControl.getButton(GamepadKeys.Button.LEFT_BUMPER)) { SPEEDCONTROL = .5; } else { SPEEDCONTROL = 1; }
//
// // Read pose
// Pose2d poseEstimate = drive.getLocalizer().getPoseEstimate();
//
// // Declare a drive direction
// // Pose representing desired x, y, and angular velocity
// Pose2d driveDirection = new Pose2d();
//
// // Standard teleop control
// // Convert gamepad input into desired pose velocity
// driveDirection = new Pose2d(
// -gamepad1.left_stick_y * SPEEDCONTROL,
// -gamepad1.left_stick_x * SPEEDCONTROL,
// -gamepad1.right_stick_x * SPEEDCONTROL
// );
//
//
// drive.setWeightedDrivePower(driveDirection);
//
// double headingPose = poseEstimate.getHeading();
// //9.26
//
// double totalHeading = ((Math.toRadians((robot.turret.getCurrentPosition() / TICKS_PER_DEGREE))) + headingPose) % (2 * Math.PI);
////
// double theta = Math.atan2(targetPosition.getY() - drive.getPoseEstimate().getY(), targetPosition.getX() - drive.getPoseEstimate().getX()) - drive.getPoseEstimate().getHeading();
//// theta = Math.atan2(Math.sin(theta), Math.cos(theta));
//
// boolean withinAngle = true;
// //Math.abs(theta) < MAX_LOCK_ANGLE;
//
//// if((realTheta > (2 * Math.PI - 0.5)) || (realTheta < (-2 * Math.PI + 0.5))){
//// robot.turret.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
//// }
//
//
// controller.setPIDF(p, i, d, f);
// int armPos = robot.turret.getCurrentPosition();
// double pid = controller.calculate(armPos, target);
//
// double deltaX = drive.getPoseEstimate().getX() - goalX;
// double deltaY = drive.getPoseEstimate().getY() - goalY;
// double deltaH = drive.getPoseEstimate().getHeading() - goalH;
//
// double desiredTurretAngleDeg = Math.toDegrees(
// Math.atan2(deltaY, deltaX)
// ); double robotHeadingDeg = Math.toDegrees(deltaH); double turretAngleDeg = desiredTurretAngleDeg - robotHeadingDeg;
// // turretAngleDeg = turretAngleDeg; //May or may not be needed for u guys...depends on if CCW or CW is positive deg, etc.
//// Normalize to [-180, 180]
// while (turretAngleDeg > 180) turretAngleDeg -= 360;
// while (turretAngleDeg < -180) turretAngleDeg += 360;
// double targetTurretPos = 0 + (turretAngleDeg * TICKS_PER_DEGREE); //Default is where when the robot is head on with the goal and the turret is facing...llike the zero position ig //180Range is the number of ticks or smth that constitutes 180 degrees on the turret)..tune those vars
//// Clamp to physical limits
// targetTurretPos = Math.max(TURRET_MIN_TICKS, Math.min(targetTurretPos, TURRET_MAX_TICKS));
//
//
// double desiredTarget = targetTurretPos;
//
// offset += multiplier * (gamepad2.left_trigger - gamepad2.right_trigger);
//
// desiredTarget+=offset;
//
// // Clamp to turret mechanical limits
// desiredTarget = Math.max(
// TURRET_MIN_TICKS,
// Math.min(TURRET_MAX_TICKS, desiredTarget)
// );
//
// desiredTarget = TURRET_MIN_TICKS;
// //forBlueSIde MAX TICKS
//
// offset += multiplier* (gamepad2.left_trigger - gamepad2.right_trigger);
//
// desiredTarget+=offset;
//
// desiredTarget = Math.max(
// TURRET_MIN_TICKS,
// Math.min(TURRET_MAX_TICKS, desiredTarget)
// );
//
//
// robot.turret.setTargetPosition((int) desiredTarget);
// robot.turret.setMode(DcMotor.RunMode.RUN_TO_POSITION);
// robot.turret.setPower(1);
//
//
//
//// target = desiredTarget;
//// controller.setPIDF(p, i, d, f);
//// pid = controller.calculate(armPos, target);
// // robot.turret.setPower(pid);
//
//// telemetry.addData("target", desiredTarget);
//// telemetry.addData("target", target);
//// telemetry.addData("pid", pid);
//// telemetry.update();
//
// driveRIGGED.update();
//
// if (driverControl.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER) > 0.5) {
// auto = false;
// robot.intake.set(0.9);
// } else if (driverControl.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER) > 0.5) {
// auto = false;
// robot.intake.set(-0.9);
// } else {
// if (!auto) {
// robot.intake.set(0);
// }
// }
//
// if(gamepad2.dpad_down) {
// robot.topFly.set(0);
// robot.bottomFly.set(0);
// robot.activeTransfer.setPosition(0.78);
// }
//
//
// if (gamepad2.dpad_up) {
// if(!auto) {
// auto = true;
// driveRIGGED.followTrajectorySequenceAsync(shootSeq);
// }
// }
//
// if (gamepad2.dpad_right) {
// driveRIGGED.followTrajectorySequenceAsync(runUp);
// }
//
//// telemetry.addData("Shooting", shooting[0]);
//// telemetry.addData("RR Busy", drive.isBusy());
//
// // Update the localizer
// drive.getLocalizer().update();
// // Print pose to telemetry
//// telemetry.addData("totalHeading", totalHeading);
//// telemetry.addData("theta", theta);
// telemetry.addData("x", poseEstimate.getX());
// telemetry.addData("y", poseEstimate.getY());
// telemetry.addData("heading", poseEstimate.getHeading());
// telemetry.addData("auton", auto);
// telemetry.update();
//
// }
//
//
//
// while (!isStopRequested() && opModeIsActive());
// }
//
// private static void stopAll(Hware robot) {
// robot.intake.set(0);
// robot.topFly.set(0);
// robot.bottomFly.set(0);
// robot.activeTransfer.setPosition(0.78);
// }
//
// private static void cancelTrajectorySequenceCompat(Object drive) {
// if (invokeIfExists(drive, "cancelTrajectorySequence")) return;
// if (invokeIfExists(drive, "cancelFollowing")) return;
// if (invokeIfExists(drive, "breakFollowing")) return;
// try {
// Method m = drive.getClass().getMethod("setDrivePower", Pose2d.class);
// m.invoke(drive, new Pose2d(0, 0, 0));
// } catch (Exception ignored) {}
// }
//
// private static boolean invokeIfExists(Object obj, String methodName) {
// try {
// Method m = obj.getClass().getMethod(methodName);
// m.invoke(obj);
// return true;
// } catch (Exception e) {
// return false;
// }
// }
//}

326
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OUTDATED/odoTest.java Normal file
View File

@@ -0,0 +1,326 @@
package org.firstinspires.ftc.teamcode.OUTDATED;//package org.firstinspires.ftc.teamcode.OUTDATED;
//
//import com.acmerobotics.dashboard.config.Config;
//import com.acmerobotics.roadrunner.control.PIDFController;
//import com.acmerobotics.roadrunner.geometry.Pose2d;
//import com.acmerobotics.roadrunner.geometry.Vector2d;
//import com.arcrobotics.ftclib.gamepad.GamepadEx;
//import com.arcrobotics.ftclib.gamepad.GamepadKeys;
//import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
//import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
//import com.qualcomm.robotcore.hardware.DcMotor;
//
//import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
//import org.firstinspires.ftc.teamcode.drive.SampleMecanumDriveRIGGED;
//import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence;
//import org.firstinspires.ftc.teamcode.util.PersonalPID;
//
//import java.lang.reflect.Method;
//
//
//@TeleOp (name = "odoTest")
//@Config
//public class odoTest extends LinearOpMode {
//
// // Declare a PIDF Controller to regulate heading
// // Use the same gains as SampleMecanumDrive's heading controller
// private PIDFController headingController = new PIDFController(SampleMecanumDrive.HEADING_PID);
// public static double MAX_LOCK_ANGLE = Math.toRadians(90); // ±90°
//
// // Declare a target vector you'd like your bot to align with
// // Can be any x/y coordinate of your choosing
// private Vector2d targetPosition = new Vector2d(45, 70);
// public static double multiplier = 15;
//
// public static double penguin = 0;
// private PersonalPID controller;
//
// public static double p = 0.01, i = 0, d = 0.0001, f = 0.001;
//
// public static int target = 0;
//
// public static double TICKS_PER_DEGREE = 9.738888;
// public static int TURRET_MIN_TICKS = (int) (TICKS_PER_DEGREE * (-90));
// public static int TURRET_MAX_TICKS = (int) (TICKS_PER_DEGREE * (90));
//
// public static double goalX = 64;
// public static double goalY = 75;
// public static double goalH = ((2 * Math.PI) / 3); // OR 3.14159265358979
//
// public static double launchMultiplier = 5;
//
//
//
//
// @Override
// public void runOpMode() throws InterruptedException {
//
// /** SETUP **/
//
// // Hardware Map Setup
// Hware robot = new Hware();
// robot.initialize(hardwareMap);
// controller = new PersonalPID(p, i, d, f);
//
// // Gamepad Setup
// GamepadEx driverControl = new GamepadEx(gamepad1);
// GamepadEx armControl = new GamepadEx(gamepad2);
//
// // DriveBase Setup
// SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
// SampleMecanumDriveRIGGED driveRIGGED = new SampleMecanumDriveRIGGED(hardwareMap);
//
// double SPEEDCONTROL = 1;
// double gamepadLaunch = 0;
//
// Boolean auto = false;
// // Retrieve pose
// drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
// drive.getLocalizer().setPoseEstimate(new Pose2d(0,0,0));
// drive.setPoseEstimate(new Pose2d(0,0,0));
// headingController.setInputBounds(-Math.PI, Math.PI);
//
// boolean xPrev = false;
// boolean bPrev = false;
//
// final boolean[] shooting = { false };
//
// Pose2d startPose = drive.getPoseEstimate();
//
//
// TrajectorySequence shootSeq = driveRIGGED.trajectorySequenceBuilder(startPose)
// .forward(.1)
// .addDisplacementMarker(()-> {
// robot.topFly.set(1);
// robot.bottomFly.set(1);
// robot.activeTransfer.setPosition(1);
// robot.intake.set(0.9);
// })
// .waitSeconds(.5)
// .build();
//
// TrajectorySequence runUp = driveRIGGED.trajectorySequenceBuilder(startPose)
// .forward(.1)
// .addDisplacementMarker(()-> {
// robot.topFly.set(0.8);
// robot.bottomFly.set(0.8);
// })
// .forward(.1)
// .waitSeconds(1.5)
// .forward(.1)
// .addDisplacementMarker(()-> {
// gamepad1.rumbleBlips(2);
// gamepad2.rumbleBlips(2);
// })
// .build();
// robot.activeTransfer.setPosition(0.78);
//
//
//
//
// double offset = 0.0;
//
// waitForStart();
//
// robot.turret.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
//
// while (opModeIsActive()) {
//
// double distance = Math.hypot(drive.getPoseEstimate().getX() - targetPosition.getX(), drive.getPoseEstimate().getY() - targetPosition.getY());
// double height = 70;
// double launchAngle = Math.toDegrees(Math.atan2(distance, height));
// telemetry.addData("angle", launchAngle);
// if(launchAngle > 70){
// launchAngle = 70;
// } else if (launchAngle < 15){
// launchAngle = 15;
// }
// double offsetLaunch = (launchAngle / (55 * launchMultiplier));
// if(offsetLaunch > 0.5){
// offsetLaunch = 0.5;
// }
// double hoodPosition = 0.35 + offsetLaunch;
// if(gamepad2.right_bumper){
// gamepadLaunch += 0.02;
// } else if(gamepad2.left_bumper){
// gamepadLaunch -= 0.02;
// }
// hoodPosition += gamepadLaunch;
// telemetry.addData("hood", hoodPosition);
// robot.launchHood.setPosition(hoodPosition);
// /** DRIVER CONTROLS **/
//
// if (driverControl.getButton(GamepadKeys.Button.RIGHT_BUMPER) || driverControl.getButton(GamepadKeys.Button.LEFT_BUMPER)) { SPEEDCONTROL = .5; } else { SPEEDCONTROL = 1; }
//
// // Read pose
// Pose2d poseEstimate = drive.getLocalizer().getPoseEstimate();
//
// // Declare a drive direction
// // Pose representing desired x, y, and angular velocity
// Pose2d driveDirection = new Pose2d();
//
// // Standard teleop control
// // Convert gamepad input into desired pose velocity
// driveDirection = new Pose2d(
// -gamepad1.left_stick_y * SPEEDCONTROL,
// -gamepad1.left_stick_x * SPEEDCONTROL,
// -gamepad1.right_stick_x * SPEEDCONTROL
// );
//
//
// drive.setWeightedDrivePower(driveDirection);
//
// double headingPose = poseEstimate.getHeading();
// //9.26
//
// double totalHeading = ((Math.toRadians((robot.turret.getCurrentPosition() / TICKS_PER_DEGREE))) + headingPose) % (2 * Math.PI);
////
// double theta = Math.atan2(targetPosition.getY() - drive.getPoseEstimate().getY(), targetPosition.getX() - drive.getPoseEstimate().getX()) - drive.getPoseEstimate().getHeading();
//// theta = Math.atan2(Math.sin(theta), Math.cos(theta));
//
// boolean withinAngle = true;
// //Math.abs(theta) < MAX_LOCK_ANGLE;
//
//// if((realTheta > (2 * Math.PI - 0.5)) || (realTheta < (-2 * Math.PI + 0.5))){
//// robot.turret.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
//// }
//
//
// controller.setPIDF(p, i, d, f);
// int armPos = robot.turret.getCurrentPosition();
// double pid = controller.calculate(armPos, target);
//
// double deltaX = drive.getPoseEstimate().getX() - targetPosition.getX();
// double deltaY = drive.getPoseEstimate().getY() - targetPosition.getY();
// double deltaH = drive.getPoseEstimate().getHeading();// - goalH;
//
// double desiredTurretAngleDeg = Math.toDegrees(
// Math.atan2(deltaY, deltaX)
// );
// double robotHeadingDeg = Math.toDegrees(deltaH);
// double turretAngleDeg = desiredTurretAngleDeg - robotHeadingDeg;
// // turretAngleDeg = turretAngleDeg; //May or may not be needed for u guys...depends on if CCW or CW is positive deg, etc.
//// Normalize to [-180, 180]
// while (turretAngleDeg > 180) turretAngleDeg -= 360;
// while (turretAngleDeg < -180) turretAngleDeg += 360;
// double targetTurretPos = 0 + (turretAngleDeg * TICKS_PER_DEGREE); //Default is where when the robot is head on with the goal and the turret is facing...llike the zero position ig //180Range is the number of ticks or smth that constitutes 180 degrees on the turret)..tune those vars
//// Clamp to physical limits
// targetTurretPos = Math.max(TURRET_MIN_TICKS, Math.min(targetTurretPos, TURRET_MAX_TICKS));
//
//
// double desiredTarget = targetTurretPos;
//
// offset += multiplier * (gamepad2.left_trigger - gamepad2.right_trigger);
//
// desiredTarget+=offset;
//
// // desiredTarget = TURRET_MIN_TICKS;
// //forBlueSIde MAX TICKS
//
// offset += multiplier* (gamepad2.left_trigger - gamepad2.right_trigger);
//
// desiredTarget+=offset;
//
// // Clamp to turret mechanical limits
// desiredTarget = Math.max(
// TURRET_MIN_TICKS,
// Math.min(TURRET_MAX_TICKS, desiredTarget)
// );
//
//
// robot.turret.setTargetPosition((int) desiredTarget);
// robot.turret.setMode(DcMotor.RunMode.RUN_TO_POSITION);
// robot.turret.setPower(1);
//
//
//
//// target = desiredTarget;
//// controller.setPIDF(p, i, d, f);
//// pid = controller.calculate(armPos, target);
// // robot.turret.setPower(pid);
//
//// telemetry.addData("target", desiredTarget);
//// telemetry.addData("target", target);
//// telemetry.addData("pid", pid);
//// telemetry.update();
//
// driveRIGGED.update();
//
// if (driverControl.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER) > 0.5) {
// auto = false;
// robot.intake.set(0.9);
// } else if (driverControl.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER) > 0.5) {
// auto = false;
// robot.intake.set(-0.9);
// } else {
// if (!auto) {
// robot.intake.set(0);
// }
// }
//
// if(gamepad2.dpad_down) {
// robot.topFly.set(0);
// robot.bottomFly.set(0);
// robot.activeTransfer.setPosition(0.78);
// }
//
//
// if (gamepad2.dpad_up) {
// if(!auto) {
// auto = true;
// driveRIGGED.followTrajectorySequenceAsync(shootSeq);
// }
// }
//
// if (gamepad2.dpad_right) {
// driveRIGGED.followTrajectorySequenceAsync(runUp);
// }
//
//// telemetry.addData("Shooting", shooting[0]);
//// telemetry.addData("RR Busy", drive.isBusy());
//
// // Update the localizer
// drive.getLocalizer().update();
// // Print pose to telemetry
//// telemetry.addData("totalHeading", totalHeading);
//// telemetry.addData("theta", theta);
// telemetry.addData("x", poseEstimate.getX());
// telemetry.addData("y", poseEstimate.getY());
// telemetry.addData("heading", poseEstimate.getHeading());
// telemetry.addData("auton", auto);
// telemetry.update();
//
// }
//
//
//
// while (!isStopRequested() && opModeIsActive());
// }
//
// private static void stopAll(Hware robot) {
// robot.intake.set(0);
// robot.topFly.set(0);
// robot.bottomFly.set(0);
// robot.activeTransfer.setPosition(0.78);
// }
//
// private static void cancelTrajectorySequenceCompat(Object drive) {
// if (invokeIfExists(drive, "cancelTrajectorySequence")) return;
// if (invokeIfExists(drive, "cancelFollowing")) return;
// if (invokeIfExists(drive, "breakFollowing")) return;
// try {
// Method m = drive.getClass().getMethod("setDrivePower", Pose2d.class);
// m.invoke(drive, new Pose2d(0, 0, 0));
// } catch (Exception ignored) {}
// }
//
// private static boolean invokeIfExists(Object obj, String methodName) {
// try {
// Method m = obj.getClass().getMethod(methodName);
// m.invoke(obj);
// return true;
// } catch (Exception e) {
// return false;
// }
// }
//}

0
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/readme.md → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OUTDATED/readme.md
View File

262
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdometryAutoLock.java
View File

@@ -1,262 +0,0 @@
package org.firstinspires.ftc.teamcode;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.control.PIDFController;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.geometry.Vector2d;
import com.arcrobotics.ftclib.gamepad.GamepadEx;
import com.arcrobotics.ftclib.gamepad.GamepadKeys;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDriveRIGGED;
import org.firstinspires.ftc.teamcode.drive.advanced.PoseStorage;
import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence;
import org.firstinspires.ftc.teamcode.util.PersonalPID;
import java.lang.reflect.Method;
import java.util.concurrent.atomic.AtomicBoolean;
@TeleOp (name = "OdoAutoLock")
@Config
public class OdometryAutoLock extends LinearOpMode {
// Declare a PIDF Controller to regulate heading
// Use the same gains as SampleMecanumDrive's heading controller
private PIDFController headingController = new PIDFController(SampleMecanumDrive.HEADING_PID);
public static double MAX_LOCK_ANGLE = Math.toRadians(90); // ±90°
// Declare a target vector you'd like your bot to align with
// Can be any x/y coordinate of your choosing
private Vector2d targetPosition = new Vector2d(0, 45);
private PersonalPID controller;
public static double p = 0.025, i = 0, d = 0.0001, f = 0.001;
public static int target = 0;
public static double TICKS_PER_DEGREE = (384.5 * 9.26) / 360;
public static int TURRET_MIN_TICKS = (int) (TICKS_PER_DEGREE * (-90));
public static int TURRET_MAX_TICKS = (int) (TICKS_PER_DEGREE * (90));
@Override
public void runOpMode() throws InterruptedException {
/** SETUP **/
// Hardware Map Setup
Hware robot = new Hware();
robot.initialize(hardwareMap);
controller = new PersonalPID(p, i, d, f);
// Gamepad Setup
GamepadEx driverControl = new GamepadEx(gamepad1);
GamepadEx armControl = new GamepadEx(gamepad2);
// DriveBase Setup
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
SampleMecanumDriveRIGGED driveRIGGED = new SampleMecanumDriveRIGGED(hardwareMap);
double SPEEDCONTROL = 1;
Boolean auto = false;
// Retrieve pose
drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
drive.getLocalizer().setPoseEstimate(PoseStorage.currentPose);
headingController.setInputBounds(-Math.PI, Math.PI);
boolean xPrev = false;
boolean bPrev = false;
final boolean[] shooting = { false };
Pose2d startPose = drive.getPoseEstimate();
TrajectorySequence shootSeq = driveRIGGED.trajectorySequenceBuilder(startPose)
.forward(.1)
.addDisplacementMarker(()-> {
robot.activeTransfer.setPosition(1);
robot.intake.set(1);
})
.build();
TrajectorySequence runUp = driveRIGGED.trajectorySequenceBuilder(startPose)
.forward(.1)
.addDisplacementMarker(()-> {
robot.topFly.set(1);
robot.bottomFly.set(1);
})
.forward(.1)
.waitSeconds(1.5)
.forward(.1)
.addDisplacementMarker(()-> {
gamepad1.rumbleBlips(2);
gamepad2.rumbleBlips(2);
})
.build();
waitForStart();
robot.turret.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
while (opModeIsActive()) {
double distance = Math.hypot(drive.getPoseEstimate().getX() - targetPosition.getX(), drive.getPoseEstimate().getY() - targetPosition.getY());
double height = 62;
double launchAngle = Math.atan2(height, distance);
/** DRIVER CONTROLS **/
if (driverControl.getButton(GamepadKeys.Button.RIGHT_BUMPER) || driverControl.getButton(GamepadKeys.Button.LEFT_BUMPER)) { SPEEDCONTROL = .5; } else { SPEEDCONTROL = 1; }
// Read pose
Pose2d poseEstimate = drive.getLocalizer().getPoseEstimate();
// Declare a drive direction
// Pose representing desired x, y, and angular velocity
Pose2d driveDirection = new Pose2d();
// Standard teleop control
// Convert gamepad input into desired pose velocity
driveDirection = new Pose2d(
-gamepad1.left_stick_y * SPEEDCONTROL,
-gamepad1.left_stick_x * SPEEDCONTROL,
-gamepad1.right_stick_x * SPEEDCONTROL
);
drive.setWeightedDrivePower(driveDirection);
double headingPose = poseEstimate.getHeading();
//9.26
double totalHeading = ((Math.toRadians((robot.turret.getCurrentPosition() / TICKS_PER_DEGREE))) + headingPose) % (2 * Math.PI);
//
double theta = Math.atan2(targetPosition.getY() - drive.getPoseEstimate().getY(), targetPosition.getX() - drive.getPoseEstimate().getX()) - drive.getPoseEstimate().getHeading();
// theta = Math.atan2(Math.sin(theta), Math.cos(theta));
boolean withinAngle =
Math.abs(theta) < MAX_LOCK_ANGLE;
// if((realTheta > (2 * Math.PI - 0.5)) || (realTheta < (-2 * Math.PI + 0.5))){
// robot.turret.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
// }
controller.setPIDF(p, i, d, f);
int armPos = robot.turret.getCurrentPosition();
double pid = controller.calculate(armPos, target);
if (withinAngle) {
int desiredTarget = (int)(TICKS_PER_DEGREE * Math.toDegrees(theta));
// Clamp to turret mechanical limits
desiredTarget = Math.max(
TURRET_MIN_TICKS,
Math.min(TURRET_MAX_TICKS, desiredTarget)
);
target = desiredTarget;
controller.setPIDF(p, i, d, f);
pid = controller.calculate(armPos, target);
robot.turret.setPower(pid);
} else {
// Outside lock range → stop or hold
robot.turret.setPower(0);
}
telemetry.addData("armPos", armPos);
telemetry.addData("target", target);
telemetry.addData("pid", pid);
telemetry.update();
driveRIGGED.update();
if (driverControl.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER) > 0.5) {
auto = false;
robot.intake.set(1);
robot.activeTransfer.setPosition(0.85);
} else if (driverControl.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER) > 0.5) {
auto = false;
robot.intake.set(-1);
} else {
if (!auto) {
robot.intake.set(0);
}
}
if(gamepad2.dpad_down) {
robot.topFly.set(0);
robot.bottomFly.set(0);
}
if (gamepad2.dpad_up) {
if(!auto) {
auto = true;
driveRIGGED.followTrajectorySequenceAsync(shootSeq);
}
}
if (gamepad2.dpad_right) {
driveRIGGED.followTrajectorySequenceAsync(runUp);
}
telemetry.addData("Shooting", shooting[0]);
telemetry.addData("RR Busy", drive.isBusy());
// Update the localizer
drive.getLocalizer().update();
// Print pose to telemetry
telemetry.addData("totalHeading", totalHeading);
telemetry.addData("theta", theta);
telemetry.addData("x", poseEstimate.getX());
telemetry.addData("y", poseEstimate.getY());
telemetry.addData("heading", poseEstimate.getHeading());
telemetry.addData("auton", auto);
telemetry.update();
}
while (!isStopRequested() && opModeIsActive());
}
private static void stopAll(Hware robot) {
robot.intake.set(0);
robot.topFly.set(0);
robot.bottomFly.set(0);
robot.activeTransfer.setPosition(0.85);
}
private static void cancelTrajectorySequenceCompat(Object drive) {
if (invokeIfExists(drive, "cancelTrajectorySequence")) return;
if (invokeIfExists(drive, "cancelFollowing")) return;
if (invokeIfExists(drive, "breakFollowing")) return;
try {
Method m = drive.getClass().getMethod("setDrivePower", Pose2d.class);
m.invoke(drive, new Pose2d(0, 0, 0));
} catch (Exception ignored) {}
}
private static boolean invokeIfExists(Object obj, String methodName) {
try {
Method m = obj.getClass().getMethod(methodName);
m.invoke(obj);
return true;
} catch (Exception e) {
return false;
}
}
}

143
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpTurretLock.java Normal file
View File

@@ -0,0 +1,143 @@
package org.firstinspires.ftc.teamcode;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
import org.firstinspires.ftc.teamcode.OUTDATED.Hware;
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
import org.firstinspires.ftc.vision.VisionPortal;
import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;
import java.util.List;
@TeleOp(name = "TeleOp TurretLock (Odom + AprilTag)", group = "TeleOp")
@Config
public class TeleOpTurretLock extends LinearOpMode {
/* ------------ CONFIG (Dashboard editable) ------------ */
// Put the APRIL TAG / BACKDROP location in YOUR RoadRunner coordinate system.
// You said your start is (0,0). So measure these from that same origin.
public static double GOAL_X = 64.0;
public static double GOAL_Y = 75.0;
// Set to a specific tag ID if you want (ex: 5). Use -1 to lock to any visible tag.
public static int TARGET_TAG_ID = -1;
// Manual trim speed: degrees added per loop from triggers
public static double MANUAL_TRIM_DEG_PER_LOOP = 0.8;
// Basic drive scaling
public static double DRIVE_SPEED = 1.0;
public static double SLOW_SPEED = 0.45;
// If your webcam name in RC config is different, change this
public static String WEBCAM_NAME = "Webcam 1";
/* ----------------------------------------------------- */
private VisionPortal visionPortal;
private AprilTagProcessor aprilTag;
@Override
public void runOpMode() {
// Hardware
Hware robot = new Hware();
robot.initialize(hardwareMap);
// Drive
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
// Start pose (you said 0,0)
drive.setPoseEstimate(new Pose2d(0, 0, 0));
// AprilTag
aprilTag = AprilTagProcessor.easyCreateWithDefaults();
visionPortal = new VisionPortal.Builder()
.setCamera(hardwareMap.get(WebcamName.class, WEBCAM_NAME))
.addProcessor(aprilTag)
.build();
// TurretLock subsystem
TurretLock turretLock = new TurretLock(robot.turret);
turretLock.goalX = GOAL_X;
turretLock.goalY = GOAL_Y;
turretLock.targetTagId = TARGET_TAG_ID;
// Optional: if you want these controlled from dashboard too, you can,
// but you already have them inside TurretLock.
// turretLock.ticksPerDegree = ...
// turretLock.minTicks = ...
// turretLock.maxTicks = ...
boolean autoLockEnabled = true;
boolean xPrev = false;
waitForStart();
while (opModeIsActive()) {
/* ---------------- DRIVE ---------------- */
double speed = DRIVE_SPEED;
if (gamepad1.left_bumper || gamepad1.right_bumper) speed = SLOW_SPEED;
Pose2d drivePower = new Pose2d(
-gamepad1.left_stick_y * speed,
-gamepad1.left_stick_x * speed,
-gamepad1.right_stick_x * speed
);
drive.setWeightedDrivePower(drivePower);
drive.update();
Pose2d pose = drive.getPoseEstimate();
/* ---------------- TOGGLE AUTOLOCK ---------------- */
boolean xNow = gamepad2.x;
if (xNow && !xPrev) autoLockEnabled = !autoLockEnabled;
xPrev = xNow;
/* ---------------- UPDATE TARGETS (Dashboard) ---------------- */
turretLock.goalX = GOAL_X;
turretLock.goalY = GOAL_Y;
turretLock.targetTagId = TARGET_TAG_ID;
/* ---------------- MANUAL TRIM ---------------- */
// Left trigger = +trim, Right trigger = -trim (same style as your old code)
double trimDeltaDeg = MANUAL_TRIM_DEG_PER_LOOP * (gamepad2.left_trigger - gamepad2.right_trigger);
if (Math.abs(trimDeltaDeg) > 1e-6) turretLock.addManualTrimDeg(trimDeltaDeg);
if (gamepad2.y) turretLock.resetManualTrim(); // reset driver trim
if (gamepad2.b) turretLock.resetVisionTrim(); // reset vision trim
/* ---------------- AUTOLOCK UPDATE ---------------- */
List<AprilTagDetection> detections = aprilTag.getDetections();
if (autoLockEnabled) {
turretLock.update(pose, detections);
} else {
// If autolock is off, just stop turret motor where it is
// (or you can put your own manual turret code here)
robot.turret.setPower(0);
}
/* ---------------- TELEMETRY ---------------- */
telemetry.addData("AutoLock", autoLockEnabled);
telemetry.addData("Pose", "x=%.1f y=%.1f h=%.1fdeg",
pose.getX(), pose.getY(), Math.toDegrees(pose.getHeading()));
telemetry.addData("Goal", "x=%.1f y=%.1f", GOAL_X, GOAL_Y);
telemetry.addData("TargetTag", TARGET_TAG_ID);
telemetry.addData("TagCount", (detections == null) ? 0 : detections.size());
telemetry.addData("TurretTicks", robot.turret.getCurrentPosition());
telemetry.addLine("Controls: GP2 X=toggle, triggers=trim, Y=reset trim, B=reset vision");
telemetry.update();
}
if (visionPortal != null) visionPortal.close();
}
}

60
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Test.java Normal file
View File

@@ -0,0 +1,60 @@
package org.firstinspires.ftc.teamcode;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import org.firstinspires.ftc.teamcode.OUTDATED.Hware;
@TeleOp(name = "ENCODER TEST - Flywheel", group = "Test")
public class Test extends LinearOpMode {
@Override
public void runOpMode() {
Hware robot = new Hware();
robot.initialize(hardwareMap);
DcMotorEx top = (DcMotorEx) robot.topFly;
DcMotorEx bottom = (DcMotorEx) robot.bottomFly;
// Reset encoders
top.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
bottom.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
top.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
bottom.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
telemetry.addLine("A = Spin Motors");
telemetry.addLine("Watch Position + Velocity");
telemetry.update();
waitForStart();
while (opModeIsActive()) {
if (gamepad1.a) {
top.setPower(0.7);
bottom.setPower(0.7);
} else {
top.setPower(0);
bottom.setPower(0);
}
telemetry.addLine("=== TOP MOTOR ===");
telemetry.addData("Position", top.getCurrentPosition());
telemetry.addData("Velocity", top.getVelocity());
telemetry.addLine("");
telemetry.addLine("=== BOTTOM MOTOR ===");
telemetry.addData("Position", bottom.getCurrentPosition());
telemetry.addData("Velocity", bottom.getVelocity());
telemetry.update();
sleep(50);
}
}
}

186
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TurretLock.java Normal file
View File

@@ -0,0 +1,186 @@
package org.firstinspires.ftc.teamcode;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.util.ElapsedTime;
import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
import java.util.List;
public class TurretLock {
/* ------------------- TUNABLES ------------------- */
// Field target (in RoadRunner field units; usually inches)
// Set this to the AprilTag/backdrop location you want to aim at.
public double goalX = 64.0;
public double goalY = 75.0;
// Turret conversion
// If you already trust your TICKS_PER_DEGREE, keep it.
public double ticksPerDegree = 9.738888;
// Mechanical limits (ticks). Keep your existing numbers.
public int minTicks = (int) (ticksPerDegree * (-90));
public int maxTicks = (int) (ticksPerDegree * ( 90));
// Which AprilTag to lock to (set -1 to accept any visible tag)
public int targetTagId = -1;
// Vision correction: degrees of turret change per degree of tag "bearing"/yaw error.
// Start small (0.6–1.2) and tune.
public double visionDegPerDegError = 0.9;
// Deadband so it doesn’t twitch
public double visionDeadbandDeg = 0.6;
// How hard we trust vision vs odom.
// 0 = ignore vision, 1 = full correction.
public double visionBlend = 1.0;
// Smooth the vision correction so it doesn’t jump
// 0.0 = no smoothing, 0.8 = heavy smoothing
public double visionLowPass = 0.65;
// If we haven't seen a tag in this many seconds, slowly decay vision trim back to 0
public double trimDecayPerSec = 12.0; // deg/sec back toward 0
// Optional: flip direction if your turret sign is backwards
public boolean invertTurret = false;
/* ------------------- STATE ------------------- */
private final DcMotorEx turret;
private double manualTrimDeg = 0.0; // driver trim (triggers)
private double visionTrimDeg = 0.0; // learned trim from camera
private double filteredVisionTrimDeg = 0.0;
private final ElapsedTime tagTimer = new ElapsedTime();
public TurretLock(DcMotorEx turretMotor) {
this.turret = turretMotor;
tagTimer.reset();
}
/** Call this if you want driver trim like your triggers (degrees). */
public void addManualTrimDeg(double deltaDeg) {
manualTrimDeg += deltaDeg;
}
public void resetManualTrim() {
manualTrimDeg = 0.0;
}
public void resetVisionTrim() {
visionTrimDeg = 0.0;
filteredVisionTrimDeg = 0.0;
}
/** Main update: call every loop. */
public void update(Pose2d robotPose, List<AprilTagDetection> detections) {
// 1) ODOM: compute turret angle needed to face goal (in degrees)
double dx = goalX - robotPose.getX();
double dy = goalY - robotPose.getY();
// angle to goal in field frame
double fieldToGoalDeg = Math.toDegrees(Math.atan2(dy, dx));
// robot heading in field frame
double robotHeadingDeg = Math.toDegrees(robotPose.getHeading());
// turret angle relative to robot (so turret points at goal)
double turretCmdDeg = fieldToGoalDeg - robotHeadingDeg;
// normalize to [-180, 180]
turretCmdDeg = normalizeDeg(turretCmdDeg);
if (invertTurret) turretCmdDeg = -turretCmdDeg;
// 2) VISION: find tag error and update vision trim
Double tagErrorDeg = getTagErrorDeg(detections);
if (tagErrorDeg != null) {
// deadband
if (Math.abs(tagErrorDeg) < visionDeadbandDeg) tagErrorDeg = 0.0;
// Convert tag error into turret trim change.
// Sign: if tag is to the right (+error), turret should move right or left?
// If it goes the wrong way, flip the sign here (multiply by -1).
double deltaTrim = (tagErrorDeg * visionDegPerDegError) * visionBlend;
// We want trim to counteract the observed error, so subtract.
visionTrimDeg -= deltaTrim;
tagTimer.reset();
} else {
// no tag: decay vision trim toward 0 over time
double dt = 0.02; // assume ~50Hz; fine for a simple decay
double decay = trimDecayPerSec * dt;
if (visionTrimDeg > 0) visionTrimDeg = Math.max(0, visionTrimDeg - decay);
else if (visionTrimDeg < 0) visionTrimDeg = Math.min(0, visionTrimDeg + decay);
}
// Low-pass filter the trim so it doesn’t jerk
filteredVisionTrimDeg = filteredVisionTrimDeg * visionLowPass + visionTrimDeg * (1.0 - visionLowPass);
// 3) Combine
double finalDeg = turretCmdDeg + manualTrimDeg + filteredVisionTrimDeg;
// Optional: keep within your allowed lock range (like ±90)
finalDeg = clamp(finalDeg, -90, 90);
// 4) Convert degrees -> ticks and command motor
int targetTicks = (int) Math.round(finalDeg * ticksPerDegree);
targetTicks = clamp(targetTicks, minTicks, maxTicks);
turret.setTargetPosition(targetTicks);
turret.setMode(DcMotorEx.RunMode.RUN_TO_POSITION);
turret.setPower(1.0);
}
/** Returns tag horizontal error in degrees (positive means tag is to the right), or null if no suitable tag. */
private Double getTagErrorDeg(List<AprilTagDetection> detections) {
if (detections == null || detections.isEmpty()) return null;
AprilTagDetection best = null;
for (AprilTagDetection d : detections) {
if (d == null || d.metadata == null) continue; // metadata can be null on some setups
if (targetTagId != -1 && d.id != targetTagId) continue;
// pick the first good one (or you can pick closest by range if you want)
best = d;
break;
}
if (best == null) return null;
// FTC AprilTagDetection typically gives:
// best.ftcPose.yaw (degrees) // yaw ~ left-right rotation of tag relative to camera
// best.ftcPose.bearing (degrees) on some versions
//
// We'll prefer "bearing" if present; otherwise use yaw.
try {
// If your SDK has bearing:
// return best.ftcPose.bearing;
// Otherwise:
return best.ftcPose.yaw;
} catch (Exception e) {
return null;
}
}
private static double normalizeDeg(double deg) {
while (deg > 180) deg -= 360;
while (deg < -180) deg += 360;
return deg;
}
private static double clamp(double v, double lo, double hi) {
return Math.max(lo, Math.min(hi, v));
}
private static int clamp(int v, int lo, int hi) {
return Math.max(lo, Math.min(hi, v));
}
}

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

@@ -44,7 +44,7 @@ public class DriveConstants {
* angular distances although most angular parameters are wrapped in Math.toRadians() for
* convenience. Make sure to exclude any gear ratio included in MOTOR_CONFIG from GEAR_RATIO.
*/
public static double WHEEL_RADIUS = 1.8898; // in
public static double WHEEL_RADIUS = 2.047; // in
public static double GEAR_RATIO = (24 / 22); // output (wheel) speed / input (motor) speed
public static double TRACK_WIDTH = 12.3; // in

34
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleMecanumDrive.java
View File

@@ -1,5 +1,17 @@
package org.firstinspires.ftc.teamcode.drive;
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ACCEL;
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ANG_ACCEL;
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ANG_VEL;
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_VEL;
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MOTOR_VELO_PID;
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.RUN_USING_ENCODER;
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.TRACK_WIDTH;
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.encoderTicksToInches;
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kA;
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kStatic;
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kV;
import androidx.annotation.NonNull;
import com.acmerobotics.dashboard.config.Config;
@@ -18,7 +30,6 @@ import com.acmerobotics.roadrunner.trajectory.constraints.ProfileAccelerationCon
import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryAccelerationConstraint;
import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryVelocityConstraint;
import com.qualcomm.hardware.lynx.LynxModule;
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
@@ -28,7 +39,6 @@ import com.qualcomm.robotcore.hardware.PIDFCoefficients;
import com.qualcomm.robotcore.hardware.VoltageSensor;
import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence;
import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequenceBuilder;
import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequenceRunner;
@@ -38,25 +48,13 @@ import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ACCEL;
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ANG_ACCEL;
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ANG_VEL;
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_VEL;
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MOTOR_VELO_PID;
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.RUN_USING_ENCODER;
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.TRACK_WIDTH;
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.encoderTicksToInches;
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kA;
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kStatic;
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kV;
/*
* Simple mecanum drive hardware implementation for REV hardware.
*/
@Config
public class SampleMecanumDrive extends MecanumDrive {
public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(0, 0, 0);
public static PIDCoefficients HEADING_PID = new PIDCoefficients(0, 0, 0);
public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(8, 0, 1);
public static PIDCoefficients HEADING_PID = new PIDCoefficients(8, 0, 0.5);
public static double LATERAL_MULTIPLIER = 1;
@@ -293,12 +291,12 @@ public class SampleMecanumDrive extends MecanumDrive {
@Override
public double getRawExternalHeading() {
return imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
return 0;
}
@Override
public Double getExternalHeadingVelocity() {
return (double) imu.getRobotAngularVelocity(AngleUnit.RADIANS).zRotationRate;
return 0.0;
}
public static TrajectoryVelocityConstraint getVelocityConstraint(double maxVel, double maxAngularVel, double trackWidth) {

24
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleTankDrive.java
View File

@@ -1,5 +1,17 @@
package org.firstinspires.ftc.teamcode.drive;
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ACCEL;
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ANG_ACCEL;
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ANG_VEL;
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_VEL;
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MOTOR_VELO_PID;
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.RUN_USING_ENCODER;
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.TRACK_WIDTH;
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.encoderTicksToInches;
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kA;
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kStatic;
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kV;
import androidx.annotation.NonNull;
import com.acmerobotics.dashboard.config.Config;
@@ -37,18 +49,6 @@ import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ACCEL;
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ANG_ACCEL;
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ANG_VEL;
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_VEL;
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MOTOR_VELO_PID;
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.RUN_USING_ENCODER;
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.TRACK_WIDTH;
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.encoderTicksToInches;
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kA;
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kStatic;
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kV;
/*
* Simple tank drive hardware implementation for REV hardware.
*/

13
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/StandardTrackingWheelLocalizer.java
View File

@@ -7,6 +7,7 @@ import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.localization.ThreeTrackingWheelLocalizer;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.HardwareMap;
import org.firstinspires.ftc.teamcode.util.Encoder;
import java.util.Arrays;
@@ -28,11 +29,11 @@ import java.util.List;
@Config
public class StandardTrackingWheelLocalizer extends ThreeTrackingWheelLocalizer {
public static double TICKS_PER_REV = 2000;
public static double WHEEL_RADIUS = 0.944882; // in
public static double WHEEL_RADIUS = 0.6299; // in
public static double GEAR_RATIO = 1; // output (wheel) speed / input (encoder) speed
public static double LATERAL_DISTANCE = 6.2; // in; distance between the left and right wheels
public static double FORWARD_OFFSET = -6; // in; offset of the lateral wheel
public static double LATERAL_DISTANCE = 6; // in; distance between the left and right wheels
public static double FORWARD_OFFSET = -6.45; // in; offset of the lateral wheel
private Encoder leftEncoder, rightEncoder, frontEncoder;
@@ -52,9 +53,9 @@ public class StandardTrackingWheelLocalizer extends ThreeTrackingWheelLocalizer
lastEncPositions = lastTrackingEncPositions;
lastEncVels = lastTrackingEncVels;
leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "intake"));
rightEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "topFly"));
frontEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "bottomFly"));
leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "frontLeft"));
rightEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "intake"));
frontEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "frontRight"));
// TODO: reverse any encoders using Encoder.setDirection(Encoder.Direction.REVERSE)
// leftEncoder.setDirection(Encoder.Direction.REVERSE);

42
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/LocalizationTest.java
View File

@@ -7,15 +7,28 @@ import com.qualcomm.robotcore.hardware.DcMotor;
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
/**
* This is a simple teleop routine for testing localization. Drive the robot around like a normal
* teleop routine and make sure the robot's estimated pose matches the robot's actual pose (slight
* errors are not out of the ordinary, especially with sudden drive motions). The goal of this
* exercise is to ascertain whether the localizer has been configured properly (note: the pure
* encoder localizer heading may be significantly off if the track width has not been tuned).
*/
@TeleOp(group = "drive")
public class LocalizationTest extends LinearOpMode {
private static final double TILE = 24.0; // inches
private static final double HALF = 72.0; // 144/2
private static int clamp(int v, int lo, int hi) {
return Math.max(lo, Math.min(hi, v));
}
// Col 1..6 left->right, nearest tile
private static int colFromX(double x) {
int col = (int) Math.round((x + HALF) / TILE) + 1;
return clamp(col, 1, 6);
}
// Row 1..6 top->bottom, nearest tile
private static int rowFromY(double y) {
int row = (int) Math.round((HALF - y) / TILE) + 1;
return clamp(row, 1, 6);
}
@Override
public void runOpMode() throws InterruptedException {
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
@@ -36,9 +49,18 @@ public class LocalizationTest extends LinearOpMode {
drive.update();
Pose2d poseEstimate = drive.getPoseEstimate();
telemetry.addData("x", poseEstimate.getX());
telemetry.addData("y", poseEstimate.getY());
telemetry.addData("heading", poseEstimate.getHeading());
int xInt = (int) Math.round(poseEstimate.getX());
int yInt = (int) Math.round(poseEstimate.getY());
int hDeg = (int) Math.round(Math.toDegrees(poseEstimate.getHeading()));
int row = rowFromY(poseEstimate.getY());
int col = colFromX(poseEstimate.getX());
telemetry.addData("x", xInt);
telemetry.addData("y", yInt);
telemetry.addData("heading (deg)", hDeg);
telemetry.addData("tile", "R" + row + " C" + col);
telemetry.update();
}
}

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/ManualFeedforwardTuner.java
View File

@@ -10,7 +10,6 @@ import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kV;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.drive.DriveSignal;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.kinematics.Kinematics;
import com.acmerobotics.roadrunner.profile.MotionProfile;
@@ -23,7 +22,6 @@ import com.qualcomm.robotcore.hardware.VoltageSensor;
import com.qualcomm.robotcore.util.RobotLog;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.teamcode.drive.DriveConstants;
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
import java.util.Objects;

88
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/flywheelSystem.java Normal file
View File

@@ -0,0 +1,88 @@
package org.firstinspires.ftc.teamcode;
import com.arcrobotics.ftclib.hardware.motors.Motor;
/**
* Flywheel subsystem for FTC using FTCLib Motor
* - Velocity control for consistent RPM
* - RPM is estimated from encoder ticks over time
*/
public class flywheelSystem {
private Motor bottomFly, topFly;
private static final double TICKS_PER_REV = 28; // goBILDA bare motor
// Variables for RPM calculation
private int lastBottomTicks = 0;
private int lastTopTicks = 0;
private long lastTime = 0; // in ms
/**
* Constructor
* @param bottom bottom flywheel motor from Hware
* @param top top flywheel motor from Hware
*/
public flywheelSystem(Motor bottom, Motor top) {
bottomFly = bottom;
topFly = top;
// Set velocity control mode
bottomFly.setRunMode(Motor.RunMode.VelocityControl);
topFly.setRunMode(Motor.RunMode.VelocityControl);
// Float to stabilize flywheel
bottomFly.setZeroPowerBehavior(Motor.ZeroPowerBehavior.FLOAT);
topFly.setZeroPowerBehavior(Motor.ZeroPowerBehavior.FLOAT);
bottomFly.setInverted(true); // flip if needed
// Initialize encoder tracking
lastBottomTicks = bottomFly.getCurrentPosition();
lastTopTicks = topFly.getCurrentPosition();
lastTime = System.currentTimeMillis();
}
/** Set target RPM for both flywheels */
public void setRPM(double rpm) {
double ticksPerSecond = rpm * TICKS_PER_REV / 60.0;
bottomFly.set(ticksPerSecond);
topFly.set(ticksPerSecond);
}
/** Stop both flywheels */
public void stop() {
bottomFly.stopMotor();
topFly.stopMotor();
}
/**
* Get current average RPM
* Calculates RPM from encoder ticks since last call
*/
public double getRPM() {
long currentTime = System.currentTimeMillis();
int currentBottomTicks = bottomFly.getCurrentPosition();
int currentTopTicks = topFly.getCurrentPosition();
double deltaTime = (currentTime - lastTime) / 1000.0; // seconds
if (deltaTime <= 0) deltaTime = 0.001; // prevent divide by zero
int deltaBottom = currentBottomTicks - lastBottomTicks;
int deltaTop = currentTopTicks - lastTopTicks;
lastBottomTicks = currentBottomTicks;
lastTopTicks = currentTopTicks;
lastTime = currentTime;
double avgTPS = ((deltaBottom + deltaTop) / 2.0) / deltaTime; // ticks/sec
return avgTPS * 60.0 / TICKS_PER_REV; // convert to RPM
}
/**
* Check if flywheels are within tolerance of target RPM
*/
public boolean atTargetRPM(double targetRPM, double tolerance) {
double currentRPM = getRPM();
return Math.abs(currentRPM - targetRPM) <= tolerance;
}
}