2/11 changes
This commit is contained in:
Plano East Robotics
@@ -3,6 +3,7 @@ package org.firstinspires.ftc.teamcode;
|
||||
import com.acmerobotics.roadrunner.geometry.Pose2d;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||
import com.qualcomm.robotcore.util.Range;
|
||||
|
||||
import org.firstinspires.ftc.teamcode.OUTDATED.Hware;
|
||||
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
|
||||
@@ -15,6 +16,7 @@ public class AutoShotTeleOp extends LinearOpMode {
|
||||
|
||||
@Override
|
||||
public void runOpMode() {
|
||||
// Initialization
|
||||
drive = new SampleMecanumDrive(hardwareMap);
|
||||
robot = new Hware();
|
||||
robot.initialize(hardwareMap);
|
||||
@@ -27,25 +29,48 @@ public class AutoShotTeleOp extends LinearOpMode {
|
||||
drive.update();
|
||||
Pose2d myPose = drive.getPoseEstimate();
|
||||
|
||||
// 2. Get Targets based on current X, Y
|
||||
// 2. Get Base Targets from Interpolation Table
|
||||
double[] targets = shooterMap.getInterpolatedShot(myPose.getX(), myPose.getY());
|
||||
|
||||
double targetVelocity = targets[0];
|
||||
double targetHood = targets[1];
|
||||
double baseHoodPos = targets[1];
|
||||
|
||||
robot.bottomFly.setVelocity(targetVelocity);
|
||||
robot.topFly.setVelocity(targetVelocity);
|
||||
robot.launchHood.setPosition(targetHood);
|
||||
// 3. Calculate Velocity Error and Hood Correction
|
||||
// Get current speed (averaging both flywheels for accuracy)
|
||||
double currentVel = (robot.topFly.getVelocity() + robot.bottomFly.getVelocity()) / 2.0;
|
||||
double velocityError = targetVelocity - currentVel;
|
||||
|
||||
/* Logic: Decrease hood by 0.05 for every 100 ticks/sec the motor is off.
|
||||
If velocityError is 200 (motor is too slow), adjustment is (200/100) * 0.05 = 0.10.
|
||||
We subtract this from the base hood position.
|
||||
*/
|
||||
double hoodAdjustment = (velocityError / 100.0) * 0.03;
|
||||
double finalHoodPos = baseHoodPos - hoodAdjustment;
|
||||
|
||||
// 4. Ensure the hood stays within physical servo limits [0.0 to 1.0]
|
||||
finalHoodPos = Range.clip(finalHoodPos, 0.0, 1.0);
|
||||
|
||||
// 5. Apply Power and Positions
|
||||
robot.bottomFly.setVelocity(targetVelocity);
|
||||
robot.topFly.setVelocity(targetVelocity);
|
||||
robot.launchHood.setPosition(finalHoodPos);
|
||||
|
||||
robot.intake.set(1);
|
||||
|
||||
if (gamepad1.dpadUpWasPressed()) {
|
||||
robot.activeTransfer.setPosition(.95);
|
||||
robot.intake.set(.65);
|
||||
}
|
||||
|
||||
if (gamepad1.dpadDownWasPressed()) {
|
||||
robot.activeTransfer.setPosition(.7);
|
||||
}
|
||||
|
||||
// Optional: Turret tracking (Angle to Goal)
|
||||
// double angleToGoal = Math.atan2(GOAL_Y - myPose.getY(), GOAL_X - myPose.getX());
|
||||
// robot.turret.setRotation(angleToGoal - myPose.getHeading());
|
||||
|
||||
// Telemetry for real-time tuning
|
||||
telemetry.addData("Robot X", myPose.getX());
|
||||
telemetry.addData("Robot Y", myPose.getY());
|
||||
telemetry.addData("Target Velocity", targetVelocity);
|
||||
telemetry.addData("Target Hood", targetHood);
|
||||
telemetry.addData("Actual Velocity", currentVel);
|
||||
telemetry.addData("Base Hood", baseHoodPos);
|
||||
telemetry.addData("Corrected Hood", finalHoodPos);
|
||||
telemetry.update();
|
||||
}
|
||||
}
|
||||
|
||||
@@ -41,18 +41,18 @@ public class blueDepot extends LinearOpMode {
|
||||
}
|
||||
|
||||
public void shoot(){
|
||||
robot.topFly.setVelocity(1750);
|
||||
robot.bottomFly.setVelocity(1750);
|
||||
robot.topFly.setVelocity(1610);
|
||||
robot.bottomFly.setVelocity(1610);
|
||||
robot.launchHood.setPosition(0.6);
|
||||
}
|
||||
|
||||
public void reset(){
|
||||
robot.intake.set(0);
|
||||
robot.topFly.setVelocity(1750);
|
||||
robot.bottomFly.setVelocity(1750);
|
||||
// robot.topFly.setVelocity(1500);
|
||||
// robot.bottomFly.setVelocity(1500);
|
||||
// robot.topFly.set(0);
|
||||
// robot.bottomFly.set(0);
|
||||
robot.launchHood.setPosition(0.65);
|
||||
// robot.launchHood.setPosition(0.6);
|
||||
robot.activeTransfer.setPosition(0.8);
|
||||
}
|
||||
|
||||
@@ -68,14 +68,14 @@ public class blueDepot extends LinearOpMode {
|
||||
drive.setPoseEstimate(startPose);
|
||||
|
||||
TrajectorySequence threeBall = drive.trajectorySequenceBuilder(startPose)
|
||||
.UNSTABLE_addTemporalMarkerOffset(.3, () -> {
|
||||
.UNSTABLE_addTemporalMarkerOffset(.1, () -> {
|
||||
reset();
|
||||
turretPos(5);
|
||||
})
|
||||
.strafeLeft(35)
|
||||
.UNSTABLE_addTemporalMarkerOffset(.1, () -> {
|
||||
reset();
|
||||
.UNSTABLE_addTemporalMarkerOffset(.3, () -> {
|
||||
shoot();
|
||||
})
|
||||
.strafeLeft(35)
|
||||
.UNSTABLE_addTemporalMarkerOffset(.5, () -> {
|
||||
robot.intake.set(0.6);
|
||||
robot.activeTransfer.setPosition(1);
|
||||
@@ -90,18 +90,19 @@ public class blueDepot extends LinearOpMode {
|
||||
})
|
||||
|
||||
//6ball
|
||||
.lineToLinearHeading(new Pose2d(-13,27, Math.toRadians(90)))
|
||||
.lineToLinearHeading(new Pose2d(11,28, Math.toRadians(90)))
|
||||
.UNSTABLE_addTemporalMarkerOffset(.1, () -> {
|
||||
spintakeStart();
|
||||
})
|
||||
.forward(35)
|
||||
.forward(35, SampleMecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
|
||||
SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
|
||||
.UNSTABLE_addTemporalMarkerOffset(.1, () -> {
|
||||
spintakeEnd();
|
||||
})
|
||||
.lineToLinearHeading(new Pose2d(24,23, Math.toRadians(135)))
|
||||
.UNSTABLE_addTemporalMarkerOffset(.1, () -> {
|
||||
reset();
|
||||
shoot();
|
||||
})
|
||||
.lineToLinearHeading(new Pose2d(24,23, Math.toRadians(135)))
|
||||
.UNSTABLE_addTemporalMarkerOffset(.5, () -> {
|
||||
robot.intake.set(0.6);
|
||||
robot.activeTransfer.setPosition(1);
|
||||
@@ -116,19 +117,15 @@ public class blueDepot extends LinearOpMode {
|
||||
.UNSTABLE_addTemporalMarkerOffset(.1, () -> {
|
||||
reset();
|
||||
})
|
||||
.lineToLinearHeading(new Pose2d(-16,27, Math.toRadians(90)))
|
||||
.lineToLinearHeading(new Pose2d(-16,65, Math.toRadians(55)))
|
||||
.lineToLinearHeading(new Pose2d(-14,27, Math.toRadians(90)))
|
||||
.UNSTABLE_addTemporalMarkerOffset(.1, () -> {
|
||||
spintakeStart();
|
||||
})
|
||||
.back(5, SampleMecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
|
||||
SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
|
||||
.forward(7, SampleMecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
|
||||
.forward(35, SampleMecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
|
||||
SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
|
||||
.UNSTABLE_addTemporalMarkerOffset(.1, () -> {
|
||||
spintakeEnd();
|
||||
})
|
||||
.lineToLinearHeading(new Pose2d(-13,27, Math.toRadians(90)))
|
||||
.lineToLinearHeading(new Pose2d(24,23, Math.toRadians(135)))
|
||||
.UNSTABLE_addTemporalMarkerOffset(.1, () -> {
|
||||
reset();
|
||||
@@ -143,22 +140,18 @@ public class blueDepot extends LinearOpMode {
|
||||
.UNSTABLE_addTemporalMarkerOffset(.1, () -> {
|
||||
reset();
|
||||
})
|
||||
.lineToLinearHeading(new Pose2d(-16,27, Math.toRadians(90)))
|
||||
.lineToLinearHeading(new Pose2d(-18 ,65, Math.toRadians(55)))
|
||||
.lineToLinearHeading(new Pose2d(-33,27, Math.toRadians(90)))
|
||||
.UNSTABLE_addTemporalMarkerOffset(.1, () -> {
|
||||
spintakeStart();
|
||||
})
|
||||
.back(5, SampleMecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
|
||||
SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
|
||||
.forward(7, SampleMecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
|
||||
.forward(35, SampleMecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
|
||||
SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
|
||||
.UNSTABLE_addTemporalMarkerOffset(.1, () -> {
|
||||
spintakeEnd();
|
||||
})
|
||||
.lineToLinearHeading(new Pose2d(-16,27, Math.toRadians(90)))
|
||||
.lineToLinearHeading(new Pose2d(24,23, Math.toRadians(135)))
|
||||
.UNSTABLE_addTemporalMarkerOffset(.1, () -> {
|
||||
reset();
|
||||
shoot();
|
||||
})
|
||||
.UNSTABLE_addTemporalMarkerOffset(.5, () -> {
|
||||
robot.intake.set(0.6);
|
||||
|
||||
@@ -1,80 +1,106 @@
|
||||
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();
|
||||
//// }
|
||||
// }
|
||||
// }
|
||||
//}
|
||||
|
||||
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;
|
||||
import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequenceBuilder;
|
||||
|
||||
@Config
|
||||
@Autonomous(name = "Blue / Red Far")
|
||||
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.setVelocity(2200);
|
||||
robot.bottomFly.setVelocity(2200);
|
||||
robot.launchHood.setPosition(0.66);
|
||||
}
|
||||
|
||||
public void reset(){
|
||||
robot.intake.set(0);
|
||||
robot.topFly.setVelocity(0);
|
||||
robot.bottomFly.setVelocity(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(-64, -20, Math.toRadians(25));
|
||||
|
||||
drive.setPoseEstimate(startPose);
|
||||
|
||||
TrajectorySequence threeBall = drive.trajectorySequenceBuilder(startPose)
|
||||
.forward(3)
|
||||
.addDisplacementMarker(()-> {
|
||||
shoot();
|
||||
})
|
||||
.build();
|
||||
|
||||
TrajectorySequence letGo = drive.trajectorySequenceBuilder(threeBall.end())
|
||||
.forward(.01)
|
||||
.addDisplacementMarker(()-> {
|
||||
robot.activeTransfer.setPosition(1);
|
||||
robot.intake.set(.5);
|
||||
})
|
||||
.build();
|
||||
|
||||
TrajectorySequence waitTime = drive.trajectorySequenceBuilder(startPose)
|
||||
.waitSeconds(4)
|
||||
.build();
|
||||
|
||||
TrajectorySequence goOut = drive.trajectorySequenceBuilder(startPose)
|
||||
.forward(4)
|
||||
.build();
|
||||
|
||||
waitForStart();
|
||||
if (opModeIsActive() && !isStopRequested()) {
|
||||
while (opModeIsActive()) {
|
||||
robot.leftPTO.setPosition(0.2);
|
||||
robot.rightPTO.setPosition(0.2);
|
||||
drive.followTrajectorySequence(threeBall);
|
||||
drive.followTrajectorySequence(waitTime);
|
||||
drive.followTrajectorySequence(letGo);
|
||||
drive.followTrajectorySequence(waitTime);
|
||||
drive.followTrajectorySequence(goOut);
|
||||
|
||||
PoseStorage.currentPose = drive.getPoseEstimate();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,215 +1,110 @@
|
||||
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(){
|
||||
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.trajectorysequence.TrajectorySequence;
|
||||
|
||||
@Config
|
||||
@Autonomous(name = "Clean Red Depot")
|
||||
public class redDepot extends LinearOpMode {
|
||||
|
||||
Hware robot = new Hware();
|
||||
public static double TICKS_PER_DEGREE = 9.738888 * 0.29969;
|
||||
|
||||
// --- Action Helpers ---
|
||||
public void setTurret(int degrees) {
|
||||
robot.turret.setTargetPosition((int) (degrees * TICKS_PER_DEGREE));
|
||||
robot.turret.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||
robot.turret.setPower(1.0);
|
||||
}
|
||||
|
||||
public void startShooter(double velocity) {
|
||||
robot.topFly.setVelocity(velocity);
|
||||
robot.bottomFly.setVelocity(velocity);
|
||||
robot.launchHood.setPosition(0.54);
|
||||
}
|
||||
|
||||
public void stopEverything() {
|
||||
// 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.topFly.setVelocity(0);
|
||||
// robot.bottomFly.setVelocity(0);
|
||||
// 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();
|
||||
//// }
|
||||
// }
|
||||
// }
|
||||
//}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void runOpMode() throws InterruptedException {
|
||||
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
|
||||
robot.initialize(hardwareMap);
|
||||
robot.turret.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||
|
||||
Pose2d startPose = new Pose2d(54, -52, Math.toRadians(205));
|
||||
drive.setPoseEstimate(startPose);
|
||||
|
||||
// --- TRAJECTORY 1: Initial Score ---
|
||||
TrajectorySequence traj1 = drive.trajectorySequenceBuilder(startPose)
|
||||
.addDisplacementMarker(() -> {
|
||||
setTurret(-5);
|
||||
startShooter(1660);
|
||||
robot.intake.set(.8);
|
||||
})
|
||||
.strafeRight(35)
|
||||
.addDisplacementMarker(() -> robot.activeTransfer.setPosition(1.0))
|
||||
.waitSeconds(1.5)
|
||||
.addDisplacementMarker(this::stopEverything)
|
||||
.build();
|
||||
|
||||
// --- TRAJECTORY 2: Intake 6-Ball ---
|
||||
TrajectorySequence traj2 = drive.trajectorySequenceBuilder(traj1.end())
|
||||
.lineToLinearHeading(new Pose2d(11, -28, Math.toRadians(265)))
|
||||
.addDisplacementMarker(() -> robot.intake.set(1.0))
|
||||
.forward(35)
|
||||
.addDisplacementMarker(() -> robot.intake.set(0))
|
||||
.build();
|
||||
|
||||
// --- TRAJECTORY 3: Score 6-Ball ---
|
||||
TrajectorySequence traj3 = drive.trajectorySequenceBuilder(traj2.end())
|
||||
.lineToLinearHeading(new Pose2d(22, -20, Math.toRadians(205)))
|
||||
.addDisplacementMarker(0.5, () -> startShooter(1660)) // Trigger half-way through
|
||||
.addDisplacementMarker(() -> robot.activeTransfer.setPosition(1.0))
|
||||
.waitSeconds(1.5)
|
||||
.addDisplacementMarker(this::stopEverything)
|
||||
.build();
|
||||
|
||||
// --- TRAJECTORY 4: Intake 9-Ball ---
|
||||
TrajectorySequence traj4 = drive.trajectorySequenceBuilder(traj3.end())
|
||||
.lineToLinearHeading(new Pose2d(-14, -27, Math.toRadians(265)))
|
||||
.addDisplacementMarker(() -> robot.intake.set(1.0))
|
||||
.forward(35)
|
||||
.addDisplacementMarker(() -> robot.intake.set(0))
|
||||
.build();
|
||||
|
||||
// --- TRAJECTORY 5: Score 9-Ball & Park ---
|
||||
TrajectorySequence traj5 = drive.trajectorySequenceBuilder(traj4.end())
|
||||
.lineToLinearHeading(new Pose2d(22, -20, Math.toRadians(205)))
|
||||
.addDisplacementMarker(() -> {
|
||||
startShooter(1660);
|
||||
robot.activeTransfer.setPosition(1.0);
|
||||
})
|
||||
.waitSeconds(1.5)
|
||||
.addDisplacementMarker(this::stopEverything)
|
||||
.lineToLinearHeading(new Pose2d(11, -28, Math.toRadians(-90)))
|
||||
.build();
|
||||
|
||||
waitForStart();
|
||||
|
||||
if (isStopRequested()) return;
|
||||
|
||||
// Execute sequentially
|
||||
drive.followTrajectorySequence(traj1);
|
||||
drive.followTrajectorySequence(traj2);
|
||||
drive.followTrajectorySequence(traj3);
|
||||
drive.followTrajectorySequence(traj4);
|
||||
drive.followTrajectorySequence(traj5);
|
||||
}
|
||||
}
|
||||
@@ -1,10 +1,7 @@
|
||||
package org.firstinspires.ftc.teamcode;
|
||||
|
||||
import static org.firstinspires.ftc.teamcode.TeleOpTurretLock.MANUAL_TRIM_DEG_PER_LOOP;
|
||||
|
||||
import com.acmerobotics.dashboard.config.Config;
|
||||
import com.acmerobotics.roadrunner.geometry.Pose2d;
|
||||
import com.fasterxml.jackson.databind.node.POJONode;
|
||||
import com.qualcomm.hardware.limelightvision.LLResult;
|
||||
import com.qualcomm.hardware.limelightvision.Limelight3A;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||
@@ -12,19 +9,45 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||
import com.qualcomm.robotcore.hardware.PIDFCoefficients;
|
||||
import com.qualcomm.robotcore.util.ElapsedTime;
|
||||
import com.qualcomm.robotcore.util.Range;
|
||||
|
||||
import org.firstinspires.ftc.robotcore.external.Telemetry;
|
||||
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.util.PersonalPID;
|
||||
|
||||
@Config
|
||||
@TeleOp (name = "TeleOp BLUE")
|
||||
@TeleOp(name = "TeleOp BLUE - Final Optimized")
|
||||
public class DriverControlsV1 extends LinearOpMode {
|
||||
enum ShootState { IDLE, START_SHOOT, LOWER_HOOD, RESET }
|
||||
|
||||
// --- DASHBOARD TUNING ---
|
||||
public static double flywheelTuner = 1.0;
|
||||
private final double P_BASE = 1.09;
|
||||
private final double F_BASE = 14.12;
|
||||
|
||||
public static double HOOD_MIN = 0.3;
|
||||
public static double HOOD_MAX = 1.0;
|
||||
public static double HOOD_CORRECTION_SENSITIVITY = 0.04;
|
||||
// -------------------------
|
||||
|
||||
// State tracking to prevent redundant hardware writes (The Fix)
|
||||
private double lastTuner = -1.0;
|
||||
private double lastPTOPosition = -1.0;
|
||||
|
||||
enum ShootState { IDLE, START_SHOOT, RESET }
|
||||
|
||||
//turret
|
||||
public static double p = 0.0000003, i = 0, d = 0.00001, f = 0.007;
|
||||
public static double target = 0;
|
||||
|
||||
public static double ticksperdegree = 9.738888 * 0.29969;
|
||||
|
||||
public int minTicks = (int) (ticksperdegree * (-90));
|
||||
public int maxTicks = (int) (ticksperdegree * (75));
|
||||
PersonalPID pid;
|
||||
Limelight3A limelight;
|
||||
LLResult result = null;
|
||||
public static double visionMult = 1.5;
|
||||
LLResult result;
|
||||
|
||||
|
||||
@Override
|
||||
public void runOpMode() throws InterruptedException {
|
||||
@@ -32,123 +55,124 @@ public class DriverControlsV1 extends LinearOpMode {
|
||||
robot.initialize(hardwareMap);
|
||||
limelight = hardwareMap.get(Limelight3A.class, "limelight");
|
||||
|
||||
|
||||
double SPEEDCONTROL = 1;
|
||||
|
||||
Boolean adjust = false;
|
||||
Boolean shooting = false;
|
||||
|
||||
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
|
||||
drive.setPoseEstimate(PoseStorage.currentPose);
|
||||
drive.getLocalizer().setPoseEstimate(PoseStorage.currentPose);
|
||||
|
||||
Pose2d driveDirection;
|
||||
if (PoseStorage.currentPose != null) drive.setPoseEstimate(PoseStorage.currentPose);
|
||||
|
||||
ShooterSubsystem shooterMap = new ShooterSubsystem();
|
||||
|
||||
ShootState shootStatus = ShootState.IDLE;
|
||||
ElapsedTime shootTimer = new ElapsedTime();
|
||||
|
||||
PIDFCoefficients pidf = new PIDFCoefficients(3, 0, 0, 14);
|
||||
|
||||
drive.setPoseEstimate(new Pose2d(0, 0, 0));
|
||||
TurretLock turretLock = new TurretLock(robot.turret, limelight, TurretLock.SIDE.BLUE);
|
||||
|
||||
waitForStart();
|
||||
boolean adjust = false;
|
||||
boolean shooting = false;
|
||||
|
||||
// Initial Hardware Setup
|
||||
// robot.turret.setTargetPosition(8);
|
||||
// robot.turret.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||
// robot.turret.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
// robot.turret.setPower(0.2);
|
||||
pid = new PersonalPID(p, i, d, f);
|
||||
limelight.pipelineSwitch(1);
|
||||
limelight.start();
|
||||
robot.turret.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||
robot.bottomFly.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, pidf);
|
||||
robot.topFly.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, pidf);
|
||||
robot.turret.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
||||
robot.turret.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
|
||||
pid = new PersonalPID(p, i, d, f);
|
||||
|
||||
waitForStart();
|
||||
|
||||
while (opModeIsActive()) {
|
||||
robot.leftPTO.setPosition(0.2);
|
||||
robot.rightPTO.setPosition(0.2);
|
||||
result = limelight.getLatestResult();
|
||||
double vision = result.getTx();
|
||||
|
||||
double currentVoltage = robot.batteryVoltageSensor.getVoltage();
|
||||
if (currentVoltage > 13.5) {
|
||||
pidf = new PIDFCoefficients(3, 0, 0, 13.8
|
||||
// 1. INPUTS & TURRET
|
||||
// if (gamepad2.yWasPressed()) {
|
||||
// robot.turret.setTargetPosition(8);
|
||||
// } else if (gamepad2.xWasPressed()) {
|
||||
// robot.turret.setTargetPosition(88);
|
||||
// } else if (gamepad2.bWasPressed()) {
|
||||
// robot.turret.setTargetPosition(-88);
|
||||
// }
|
||||
double target = ticksperdegree * vision;
|
||||
double targetTicks = pid.calculate(robot.turret.getCurrentPosition(), target);
|
||||
pid.setPIDF(p, i, d, f);
|
||||
|
||||
//double targetTicks = turretLock.update(pose, result, vision, robot.turret.getCurrentPosition());
|
||||
// robot.turret.setTargetPosition((int) (robot.turret.getCurrentPosition() - targetTicks));
|
||||
// robot.turret.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||
|
||||
telemetry.addData("PID power", - targetTicks);
|
||||
telemetry.addData("Angle", result.getTx());
|
||||
|
||||
|
||||
// 2. PTO OPTIMIZATION (Only write if value is new)
|
||||
double targetPTO = 0.2;
|
||||
if (targetPTO != lastPTOPosition) {
|
||||
robot.leftPTO.setPosition(targetPTO);
|
||||
robot.rightPTO.setPosition(targetPTO);
|
||||
lastPTOPosition = targetPTO;
|
||||
}
|
||||
|
||||
// 3. FLYWHEEL PIDF OPTIMIZATION (Crucial for preventing lag/DC)
|
||||
if (flywheelTuner != lastTuner) {
|
||||
PIDFCoefficients dynamicPIDF = new PIDFCoefficients(
|
||||
P_BASE * flywheelTuner, 0, 0, F_BASE * flywheelTuner
|
||||
);
|
||||
} else if (currentVoltage > 12.7) {
|
||||
pidf = new PIDFCoefficients(3, 0, 0, 14);
|
||||
} else if (currentVoltage > 11.2) {
|
||||
pidf = new PIDFCoefficients(3, 0, 0, 14.5 );
|
||||
} else {
|
||||
pidf = new PIDFCoefficients(3, 0, 0, 14 );
|
||||
|
||||
robot.bottomFly.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, dynamicPIDF);
|
||||
robot.topFly.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, dynamicPIDF);
|
||||
lastTuner = flywheelTuner;
|
||||
}
|
||||
|
||||
// 4. DRIVETRAIN
|
||||
drive.update();
|
||||
Pose2d myPose = drive.getPoseEstimate();
|
||||
double turrPos = robot.turret.getCurrentPosition();
|
||||
double speedControl = gamepad1.right_bumper ? 0.5 : 1.0;
|
||||
|
||||
result = limelight.getLatestResult();
|
||||
double vision = result.getTx() * visionMult;
|
||||
if((result.getTx() < 2 || result.getTy() > -2) && result.isValid()){
|
||||
// drive.setPoseEstimate(new Pose2d(drive.getPoseEstimate().getX(), drive.getPoseEstimate().getY(), turretLock.updateHeading(myPose, result, turrPos)));
|
||||
}
|
||||
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
|
||||
// telemetry.update();\
|
||||
//
|
||||
double targetTicks = turretLock.update(myPose, result, vision, turrPos);
|
||||
robot.turret.setTargetPosition((int) targetTicks);
|
||||
robot.turret.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||
robot.turret.setPower(1);
|
||||
drive.setWeightedDrivePower(new Pose2d(
|
||||
-gamepad1.left_stick_y * speedControl,
|
||||
-gamepad1.left_stick_x * speedControl,
|
||||
-gamepad1.right_stick_x * speedControl
|
||||
));
|
||||
|
||||
if (gamepad1.right_bumper) {
|
||||
SPEEDCONTROL= 0.5;
|
||||
}
|
||||
// 5. INTAKE / TRANSFER
|
||||
if (!shooting) {
|
||||
if (gamepad1.right_trigger > 0.3) {
|
||||
robot.intake.set(0.8);
|
||||
robot.activeTransfer.setPosition(0.7);
|
||||
robot.intake.set(1.0);
|
||||
robot.activeTransfer.setPosition(1.0);
|
||||
} else if (gamepad1.left_trigger > 0.5) {
|
||||
robot.intake.set(-0.8);
|
||||
robot.activeTransfer.setPosition(0.7);
|
||||
robot.intake.set(-1.0);
|
||||
robot.activeTransfer.setPosition(1.0);
|
||||
} else {
|
||||
robot.intake.set(0);
|
||||
robot.activeTransfer.setPosition(0.7);
|
||||
robot.activeTransfer.setPosition(1.0);
|
||||
}
|
||||
}
|
||||
|
||||
driveDirection = new Pose2d(
|
||||
-gamepad1.left_stick_y * SPEEDCONTROL,
|
||||
-gamepad1.left_stick_x * SPEEDCONTROL,
|
||||
-gamepad1.right_stick_x * SPEEDCONTROL
|
||||
);
|
||||
|
||||
// Arm
|
||||
|
||||
|
||||
// 2. Get Targets based on current X, Y
|
||||
// 6. SHOOTER & HOOD LOGIC
|
||||
double[] targets = shooterMap.getInterpolatedShot(myPose.getX(), myPose.getY());
|
||||
double targetVelocity = targets[0];
|
||||
double targetHood = targets[1];
|
||||
double baseHood = targets[1];
|
||||
|
||||
// targetVelocity = targetVelocity + 40 ;
|
||||
|
||||
if (shooting) {
|
||||
robot.launchHood.setPosition(targetHood);
|
||||
}
|
||||
double currentVel = (robot.topFly.getVelocity() + robot.bottomFly.getVelocity()) / 2.0;
|
||||
double velocityError = targetVelocity - currentVel;
|
||||
|
||||
if(gamepad2.dpadUpWasPressed()) {
|
||||
adjust = true;
|
||||
} else if (gamepad2.dpadDownWasPressed()){
|
||||
adjust = false;
|
||||
}
|
||||
double hoodAdjustment = (velocityError / 80.0) * HOOD_CORRECTION_SENSITIVITY;
|
||||
double correctedHood = Range.clip(baseHood - hoodAdjustment, HOOD_MIN, HOOD_MAX);
|
||||
|
||||
if (adjust) {
|
||||
if (gamepad2.dpad_up) adjust = true;
|
||||
else if (gamepad2.dpad_down) adjust = false;
|
||||
|
||||
if (adjust || shooting) {
|
||||
robot.bottomFly.setVelocity(targetVelocity);
|
||||
robot.topFly.setVelocity(targetVelocity);
|
||||
robot.launchHood.setPosition(correctedHood);
|
||||
} else {
|
||||
robot.bottomFly.setVelocity(0);
|
||||
robot.topFly.setVelocity(0);
|
||||
}
|
||||
|
||||
|
||||
// 1. TRIGGER: Start the sequence
|
||||
if (gamepad2.xWasPressed()) {
|
||||
// 7. SHOOTING STATE MACHINE
|
||||
if (gamepad2.dpadRightWasPressed()) {
|
||||
shooting = true;
|
||||
shootStatus = ShootState.START_SHOOT;
|
||||
shootTimer.reset();
|
||||
@@ -156,46 +180,46 @@ public class DriverControlsV1 extends LinearOpMode {
|
||||
|
||||
switch (shootStatus) {
|
||||
case START_SHOOT:
|
||||
// Initial actions
|
||||
robot.activeTransfer.setPosition(0.95);
|
||||
robot.intake.set(0.55);
|
||||
robot.launchHood.setPosition(targetHood);
|
||||
|
||||
// Wait 250ms for the shot to physically fire before moving the hood
|
||||
if (shootTimer.milliseconds() > 100) {
|
||||
shootStatus = ShootState.LOWER_HOOD;
|
||||
robot.activeTransfer.setPosition(0.81);
|
||||
robot.intake.set(1.0);
|
||||
targetTicks = 0;
|
||||
if (shootTimer.milliseconds() > 2000) {
|
||||
shootStatus = ShootState.RESET;
|
||||
}
|
||||
break;
|
||||
|
||||
case LOWER_HOOD:
|
||||
targetHood = Math.max(0.34, targetHood - 0.2);
|
||||
targetVelocity = targetVelocity + 100;
|
||||
robot.launchHood.setPosition(targetHood);
|
||||
robot.bottomFly.setVelocity(targetVelocity);
|
||||
robot.topFly.setVelocity(targetVelocity);
|
||||
shootStatus = ShootState.RESET;
|
||||
break;
|
||||
|
||||
case RESET:
|
||||
if (shootTimer.milliseconds() > 3000) {
|
||||
if (shootTimer.milliseconds() > 2500) {
|
||||
shootStatus = ShootState.IDLE;
|
||||
shooting = false;
|
||||
}
|
||||
break;
|
||||
case IDLE:
|
||||
// Do nothing
|
||||
break;
|
||||
}
|
||||
|
||||
if (gamepad2.yWasPressed()) {
|
||||
if((robot.turret.getCurrentPosition() > maxTicks) && (targetTicks > 0)){
|
||||
targetTicks = targetTicks;
|
||||
} else if((robot.turret.getCurrentPosition() < maxTicks) && (targetTicks < 0)){
|
||||
targetTicks = targetTicks;
|
||||
|
||||
} else if((robot.turret.getCurrentPosition() > maxTicks) || (robot.turret.getCurrentPosition() < minTicks) ){
|
||||
targetTicks = 0;
|
||||
}
|
||||
robot.turret.setPower(- targetTicks);
|
||||
|
||||
|
||||
if (gamepad2.dpadLeftWasPressed()) {
|
||||
shooting = false;
|
||||
robot.activeTransfer.setPosition(.7);
|
||||
shootStatus = ShootState.IDLE;
|
||||
robot.activeTransfer.setPosition(1.0);
|
||||
}
|
||||
|
||||
drive.setWeightedDrivePower(driveDirection);
|
||||
drive.update();
|
||||
|
||||
telemetry.addData("Vision", vision);
|
||||
telemetry.addData("Voltage Sensor", currentVoltage);
|
||||
// 8. TELEMETRY (Keep it clean to save bandwidth)
|
||||
telemetry.addData("Status", "Running");
|
||||
telemetry.addData("Vel Error", (int)velocityError);
|
||||
telemetry.addData("Hood", "%.2f", correctedHood);
|
||||
telemetry.update();
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
@@ -1,7 +1,5 @@
|
||||
package org.firstinspires.ftc.teamcode;
|
||||
|
||||
import static org.firstinspires.ftc.teamcode.TeleOpTurretLock.MANUAL_TRIM_DEG_PER_LOOP;
|
||||
|
||||
import com.acmerobotics.dashboard.config.Config;
|
||||
import com.acmerobotics.roadrunner.geometry.Pose2d;
|
||||
import com.qualcomm.hardware.limelightvision.LLResult;
|
||||
@@ -11,17 +9,45 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||
import com.qualcomm.robotcore.hardware.PIDFCoefficients;
|
||||
import com.qualcomm.robotcore.util.ElapsedTime;
|
||||
import com.qualcomm.robotcore.util.Range;
|
||||
|
||||
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.util.PersonalPID;
|
||||
|
||||
@Config
|
||||
@TeleOp (name = "TeleOp RED")
|
||||
@TeleOp(name = "TeleOp RED - Final Optimized")
|
||||
public class DriverControlsV1Red extends LinearOpMode {
|
||||
enum ShootState { IDLE, START_SHOOT, LOWER_HOOD, RESET }
|
||||
|
||||
// --- DASHBOARD TUNING ---
|
||||
public static double flywheelTuner = 1.0;
|
||||
private final double P_BASE = 1.09;
|
||||
private final double F_BASE = 14.12;
|
||||
|
||||
public static double HOOD_MIN = 0.3;
|
||||
public static double HOOD_MAX = 1.0;
|
||||
public static double HOOD_CORRECTION_SENSITIVITY = 0.04;
|
||||
// -------------------------
|
||||
|
||||
// State tracking to prevent redundant hardware writes (The Fix)
|
||||
private double lastTuner = -1.0;
|
||||
private double lastPTOPosition = -1.0;
|
||||
|
||||
enum ShootState { IDLE, START_SHOOT, RESET }
|
||||
|
||||
//turret
|
||||
public static double p = 0.0000003, i = 0, d = 0.00001, f = 0.007;
|
||||
public static double target = 0;
|
||||
|
||||
public static double ticksperdegree = 9.738888 * 0.29969;
|
||||
|
||||
public int minTicks = (int) (ticksperdegree * (-90));
|
||||
public int maxTicks = (int) (ticksperdegree * (75));
|
||||
PersonalPID pid;
|
||||
Limelight3A limelight;
|
||||
LLResult result = null;
|
||||
public static double visionMult = 1.5;
|
||||
LLResult result;
|
||||
|
||||
|
||||
@Override
|
||||
public void runOpMode() throws InterruptedException {
|
||||
@@ -29,123 +55,124 @@ public class DriverControlsV1Red extends LinearOpMode {
|
||||
robot.initialize(hardwareMap);
|
||||
limelight = hardwareMap.get(Limelight3A.class, "limelight");
|
||||
|
||||
|
||||
double SPEEDCONTROL = 1;
|
||||
|
||||
Boolean adjust = false;
|
||||
Boolean shooting = false;
|
||||
|
||||
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
|
||||
drive.setPoseEstimate(new Pose2d(0,0,0));
|
||||
|
||||
Pose2d driveDirection;
|
||||
|
||||
ShooterSubsystem shooterMap = new ShooterSubsystem();
|
||||
if (PoseStorage.currentPose != null) drive.setPoseEstimate(PoseStorage.currentPose);
|
||||
|
||||
ShooterSubsystemRed shooterMap = new ShooterSubsystemRed();
|
||||
ShootState shootStatus = ShootState.IDLE;
|
||||
ElapsedTime shootTimer = new ElapsedTime();
|
||||
|
||||
PIDFCoefficients pidf = new PIDFCoefficients(3, 0, 0, 14);
|
||||
boolean adjust = false;
|
||||
boolean shooting = false;
|
||||
|
||||
drive.setPoseEstimate(new Pose2d(0, 0, 0));
|
||||
TurretLock turretLock = new TurretLock(robot.turret, limelight, TurretLock.SIDE.RED);
|
||||
// Initial Hardware Setup
|
||||
// robot.turret.setTargetPosition(8);
|
||||
// robot.turret.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||
// robot.turret.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
// robot.turret.setPower(0.2);
|
||||
pid = new PersonalPID(p, i, d, f);
|
||||
limelight.pipelineSwitch(2);
|
||||
limelight.start();
|
||||
robot.turret.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||
robot.turret.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
|
||||
pid = new PersonalPID(p, i, d, f);
|
||||
|
||||
waitForStart();
|
||||
|
||||
limelight.pipelineSwitch(1);
|
||||
limelight.start();
|
||||
|
||||
robot.turret.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||
robot.bottomFly.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, pidf);
|
||||
robot.topFly.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, pidf);
|
||||
robot.turret.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
||||
|
||||
while (opModeIsActive()) {
|
||||
robot.leftPTO.setPosition(0.2);
|
||||
robot.rightPTO.setPosition(0.2);
|
||||
|
||||
double currentVoltage = robot.batteryVoltageSensor.getVoltage();
|
||||
if (currentVoltage > 13.5) {
|
||||
pidf = new PIDFCoefficients(3, 0, 0, 13.8
|
||||
);
|
||||
} else if (currentVoltage > 12.7) {
|
||||
pidf = new PIDFCoefficients(3, 0, 0, 14);
|
||||
} else if (currentVoltage > 11.2) {
|
||||
pidf = new PIDFCoefficients(3, 0, 0, 14.5 );
|
||||
} else {
|
||||
pidf = new PIDFCoefficients(3, 0, 0, 14 );
|
||||
|
||||
}
|
||||
Pose2d myPose = drive.getPoseEstimate();
|
||||
double turrPos = robot.turret.getCurrentPosition();
|
||||
|
||||
result = limelight.getLatestResult();
|
||||
double vision = result.getTx() * visionMult;
|
||||
// if((result.getTx() < 2 || result.getTy() > -2) && result.isValid()){
|
||||
// drive.setPoseEstimate(new Pose2d(drive.getPoseEstimate().getX(), drive.getPoseEstimate().getY(), turretLock.updateHeading(myPose, result, turrPos)));
|
||||
// }
|
||||
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
|
||||
// telemetry.update();\
|
||||
//
|
||||
double targetTicks = turretLock.update(myPose, result, vision, turrPos);
|
||||
robot.turret.setTargetPosition((int) targetTicks);
|
||||
robot.turret.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||
robot.turret.setPower(1);
|
||||
double vision = result.getTx();
|
||||
|
||||
if (gamepad1.right_bumper) {
|
||||
SPEEDCONTROL= 0.5;
|
||||
// 1. INPUTS & TURRET
|
||||
// if (gamepad2.yWasPressed()) {
|
||||
// robot.turret.setTargetPosition(8);
|
||||
// } else if (gamepad2.xWasPressed()) {
|
||||
// robot.turret.setTargetPosition(88);
|
||||
// } else if (gamepad2.bWasPressed()) {
|
||||
// robot.turret.setTargetPosition(-88);
|
||||
// }
|
||||
double target = ticksperdegree * vision;
|
||||
double targetTicks = pid.calculate(robot.turret.getCurrentPosition(), target);
|
||||
pid.setPIDF(p, i, d, f);
|
||||
|
||||
//double targetTicks = turretLock.update(pose, result, vision, robot.turret.getCurrentPosition());
|
||||
// robot.turret.setTargetPosition((int) (robot.turret.getCurrentPosition() - targetTicks));
|
||||
// robot.turret.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||
|
||||
telemetry.addData("PID power", - targetTicks);
|
||||
telemetry.addData("Angle", result.getTx());
|
||||
|
||||
|
||||
// 2. PTO OPTIMIZATION (Only write if value is new)
|
||||
double targetPTO = 0.2;
|
||||
if (targetPTO != lastPTOPosition) {
|
||||
robot.leftPTO.setPosition(targetPTO);
|
||||
robot.rightPTO.setPosition(targetPTO);
|
||||
lastPTOPosition = targetPTO;
|
||||
}
|
||||
|
||||
// 3. FLYWHEEL PIDF OPTIMIZATION (Crucial for preventing lag/DC)
|
||||
if (flywheelTuner != lastTuner) {
|
||||
PIDFCoefficients dynamicPIDF = new PIDFCoefficients(
|
||||
P_BASE * flywheelTuner, 0, 0, F_BASE * flywheelTuner
|
||||
);
|
||||
robot.bottomFly.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, dynamicPIDF);
|
||||
robot.topFly.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, dynamicPIDF);
|
||||
lastTuner = flywheelTuner;
|
||||
}
|
||||
|
||||
// 4. DRIVETRAIN
|
||||
drive.update();
|
||||
Pose2d myPose = drive.getPoseEstimate();
|
||||
double speedControl = gamepad1.right_bumper ? 0.5 : 1.0;
|
||||
|
||||
drive.setWeightedDrivePower(new Pose2d(
|
||||
-gamepad1.left_stick_y * speedControl,
|
||||
-gamepad1.left_stick_x * speedControl,
|
||||
-gamepad1.right_stick_x * speedControl
|
||||
));
|
||||
|
||||
// 5. INTAKE / TRANSFER
|
||||
if (!shooting) {
|
||||
if (gamepad1.right_trigger > 0.3) {
|
||||
robot.intake.set(0.8);
|
||||
robot.activeTransfer.setPosition(0.7);
|
||||
robot.intake.set(1.0);
|
||||
robot.activeTransfer.setPosition(1.0);
|
||||
} else if (gamepad1.left_trigger > 0.5) {
|
||||
robot.intake.set(-0.8);
|
||||
robot.activeTransfer.setPosition(0.7);
|
||||
robot.intake.set(-1.0);
|
||||
robot.activeTransfer.setPosition(1.0);
|
||||
} else {
|
||||
robot.intake.set(0);
|
||||
robot.activeTransfer.setPosition(0.7);
|
||||
robot.activeTransfer.setPosition(1.0);
|
||||
}
|
||||
}
|
||||
|
||||
driveDirection = new Pose2d(
|
||||
-gamepad1.left_stick_y * SPEEDCONTROL,
|
||||
-gamepad1.left_stick_x * SPEEDCONTROL,
|
||||
-gamepad1.right_stick_x * SPEEDCONTROL
|
||||
);
|
||||
|
||||
// Arm
|
||||
|
||||
|
||||
// 2. Get Targets based on current X, Y
|
||||
// 6. SHOOTER & HOOD LOGIC
|
||||
double[] targets = shooterMap.getInterpolatedShot(myPose.getX(), myPose.getY());
|
||||
double targetVelocity = targets[0];
|
||||
double targetHood = targets[1];
|
||||
double baseHood = targets[1];
|
||||
|
||||
// targetVelocity = targetVelocity + 40 ;
|
||||
|
||||
if (shooting) {
|
||||
robot.launchHood.setPosition(targetHood);
|
||||
}
|
||||
double currentVel = (robot.topFly.getVelocity() + robot.bottomFly.getVelocity()) / 2.0;
|
||||
double velocityError = targetVelocity - currentVel;
|
||||
|
||||
if(gamepad2.dpadUpWasPressed()) {
|
||||
adjust = true;
|
||||
} else if (gamepad2.dpadDownWasPressed()){
|
||||
adjust = false;
|
||||
}
|
||||
double hoodAdjustment = (velocityError / 80.0) * HOOD_CORRECTION_SENSITIVITY;
|
||||
double correctedHood = Range.clip(baseHood - hoodAdjustment, HOOD_MIN, HOOD_MAX);
|
||||
|
||||
if (adjust) {
|
||||
if (gamepad2.dpad_up) adjust = true;
|
||||
else if (gamepad2.dpad_down) adjust = false;
|
||||
|
||||
if (adjust || shooting) {
|
||||
robot.bottomFly.setVelocity(targetVelocity);
|
||||
robot.topFly.setVelocity(targetVelocity);
|
||||
robot.launchHood.setPosition(correctedHood);
|
||||
} else {
|
||||
robot.bottomFly.setVelocity(0);
|
||||
robot.topFly.setVelocity(0);
|
||||
}
|
||||
|
||||
|
||||
// 1. TRIGGER: Start the sequence
|
||||
if (gamepad2.xWasPressed()) {
|
||||
// 7. SHOOTING STATE MACHINE
|
||||
if (gamepad2.dpadRightWasPressed()) {
|
||||
shooting = true;
|
||||
shootStatus = ShootState.START_SHOOT;
|
||||
shootTimer.reset();
|
||||
@@ -153,46 +180,46 @@ public class DriverControlsV1Red extends LinearOpMode {
|
||||
|
||||
switch (shootStatus) {
|
||||
case START_SHOOT:
|
||||
// Initial actions
|
||||
robot.activeTransfer.setPosition(0.95);
|
||||
robot.intake.set(0.55);
|
||||
robot.launchHood.setPosition(targetHood);
|
||||
|
||||
// Wait 250ms for the shot to physically fire before moving the hood
|
||||
if (shootTimer.milliseconds() > 100) {
|
||||
shootStatus = ShootState.LOWER_HOOD;
|
||||
robot.activeTransfer.setPosition(0.81);
|
||||
robot.intake.set(1.0);
|
||||
targetTicks = 0;
|
||||
if (shootTimer.milliseconds() > 2000) {
|
||||
shootStatus = ShootState.RESET;
|
||||
}
|
||||
break;
|
||||
|
||||
case LOWER_HOOD:
|
||||
targetHood = Math.max(0.34, targetHood - 0.2);
|
||||
targetVelocity = targetVelocity + 100;
|
||||
robot.launchHood.setPosition(targetHood);
|
||||
robot.bottomFly.setVelocity(targetVelocity);
|
||||
robot.topFly.setVelocity(targetVelocity);
|
||||
shootStatus = ShootState.RESET;
|
||||
break;
|
||||
|
||||
case RESET:
|
||||
if (shootTimer.milliseconds() > 3000) {
|
||||
if (shootTimer.milliseconds() > 2500) {
|
||||
shootStatus = ShootState.IDLE;
|
||||
shooting = false;
|
||||
}
|
||||
break;
|
||||
case IDLE:
|
||||
// Do nothing
|
||||
break;
|
||||
}
|
||||
|
||||
if (gamepad2.yWasPressed()) {
|
||||
if((robot.turret.getCurrentPosition() > maxTicks) && (targetTicks > 0)){
|
||||
targetTicks = targetTicks;
|
||||
} else if((robot.turret.getCurrentPosition() < maxTicks) && (targetTicks < 0)){
|
||||
targetTicks = targetTicks;
|
||||
|
||||
} else if((robot.turret.getCurrentPosition() > maxTicks) || (robot.turret.getCurrentPosition() < minTicks) ){
|
||||
targetTicks = 0;
|
||||
}
|
||||
robot.turret.setPower(- targetTicks);
|
||||
|
||||
|
||||
if (gamepad2.dpadLeftWasPressed()) {
|
||||
shooting = false;
|
||||
robot.activeTransfer.setPosition(.7);
|
||||
shootStatus = ShootState.IDLE;
|
||||
robot.activeTransfer.setPosition(1.0);
|
||||
}
|
||||
|
||||
drive.setWeightedDrivePower(driveDirection);
|
||||
drive.update();
|
||||
|
||||
telemetry.addData("Vision", vision);
|
||||
telemetry.addData("Voltage Sensor", currentVoltage);
|
||||
// 8. TELEMETRY (Keep it clean to save bandwidth)
|
||||
telemetry.addData("Status", "Running");
|
||||
telemetry.addData("Vel Error", (int)velocityError);
|
||||
telemetry.addData("Hood", "%.2f", correctedHood);
|
||||
telemetry.update();
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,166 @@
|
||||
package org.firstinspires.ftc.teamcode;
|
||||
|
||||
import static org.firstinspires.ftc.teamcode.TeleOpTurretLock.MANUAL_TRIM_DEG_PER_LOOP;
|
||||
|
||||
import com.acmerobotics.dashboard.config.Config;
|
||||
import com.acmerobotics.roadrunner.geometry.Pose2d;
|
||||
import com.qualcomm.hardware.limelightvision.LLResult;
|
||||
import com.qualcomm.hardware.limelightvision.Limelight3A;
|
||||
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 com.qualcomm.robotcore.util.ElapsedTime;
|
||||
|
||||
import org.firstinspires.ftc.teamcode.OUTDATED.Hware;
|
||||
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
|
||||
import org.firstinspires.ftc.teamcode.drive.advanced.PoseStorage;
|
||||
|
||||
@Config
|
||||
@TeleOp (name = "TeleOp V2")
|
||||
public class DriverControlsV2 extends LinearOpMode {
|
||||
enum ShootState { IDLE, START_SHOOT, LOWER_HOOD, RESET }
|
||||
Limelight3A limelight;
|
||||
LLResult result = null;
|
||||
|
||||
@Override
|
||||
public void runOpMode() throws InterruptedException {
|
||||
Hware robot = new Hware();
|
||||
robot.initialize(hardwareMap);
|
||||
|
||||
|
||||
double SPEEDCONTROL = 1;
|
||||
|
||||
Boolean adjust = false;
|
||||
Boolean shooting = false;
|
||||
|
||||
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
|
||||
drive.setPoseEstimate(PoseStorage.currentPose);
|
||||
drive.getLocalizer().setPoseEstimate(PoseStorage.currentPose);
|
||||
|
||||
Pose2d driveDirection;
|
||||
|
||||
ShooterSubsystem shooterMap = new ShooterSubsystem();
|
||||
|
||||
ShootState shootStatus = ShootState.IDLE;
|
||||
ElapsedTime shootTimer = new ElapsedTime();
|
||||
|
||||
PIDFCoefficients pidf = new PIDFCoefficients(3, 0, 0, 14);
|
||||
waitForStart();
|
||||
|
||||
|
||||
|
||||
while (opModeIsActive()) {
|
||||
robot.leftPTO.setPosition(0.2);
|
||||
robot.rightPTO.setPosition(0.2);
|
||||
|
||||
Pose2d myPose = drive.getPoseEstimate();
|
||||
|
||||
if (gamepad1.right_bumper) {
|
||||
SPEEDCONTROL= 0.5;
|
||||
}
|
||||
if (!shooting) {
|
||||
if (gamepad1.right_trigger > 0.3) {
|
||||
robot.intake.set(0.8);
|
||||
robot.activeTransfer.setPosition(0.7);
|
||||
} else if (gamepad1.left_trigger > 0.5) {
|
||||
robot.intake.set(-0.8);
|
||||
robot.activeTransfer.setPosition(0.7);
|
||||
} else {
|
||||
robot.intake.set(0);
|
||||
robot.activeTransfer.setPosition(0.7);
|
||||
}
|
||||
}
|
||||
|
||||
driveDirection = new Pose2d(
|
||||
-gamepad1.left_stick_y * SPEEDCONTROL,
|
||||
-gamepad1.left_stick_x * SPEEDCONTROL,
|
||||
-gamepad1.right_stick_x * SPEEDCONTROL
|
||||
);
|
||||
|
||||
// Arm
|
||||
|
||||
|
||||
// 2. Get Targets based on current X, Y
|
||||
double[] targets = shooterMap.getInterpolatedShot(myPose.getX(), myPose.getY());
|
||||
double targetVelocity = targets[0];
|
||||
double targetHood = targets[1];
|
||||
|
||||
|
||||
if (shooting) {
|
||||
robot.launchHood.setPosition(targetHood);
|
||||
}
|
||||
|
||||
if(gamepad2.dpadUpWasPressed()) {
|
||||
adjust = true;
|
||||
} else if (gamepad2.dpadDownWasPressed()){
|
||||
adjust = false;
|
||||
}
|
||||
|
||||
if (adjust) {
|
||||
robot.bottomFly.setVelocity(targetVelocity);
|
||||
robot.topFly.setVelocity(targetVelocity);
|
||||
} else {
|
||||
robot.bottomFly.setVelocity(0);
|
||||
robot.topFly.setVelocity(0);
|
||||
}
|
||||
|
||||
|
||||
// 1. TRIGGER: Start the sequence
|
||||
if (gamepad2.xWasPressed()) {
|
||||
shooting = true;
|
||||
shootStatus = ShootState.START_SHOOT;
|
||||
shootTimer.reset();
|
||||
}
|
||||
|
||||
switch (shootStatus) {
|
||||
case START_SHOOT:
|
||||
// Initial actions
|
||||
robot.activeTransfer.setPosition(0.95);
|
||||
robot.intake.set(0.55);
|
||||
robot.launchHood.setPosition(targetHood);
|
||||
|
||||
// Wait 250ms for the shot to physically fire before moving the hood
|
||||
if (shootTimer.milliseconds() > 100) {
|
||||
shootStatus = ShootState.LOWER_HOOD;
|
||||
}
|
||||
break;
|
||||
|
||||
case LOWER_HOOD:
|
||||
targetHood = Math.max(0.34, targetHood - 0.2);
|
||||
targetVelocity = targetVelocity + 100;
|
||||
robot.launchHood.setPosition(targetHood);
|
||||
robot.bottomFly.setVelocity(targetVelocity);
|
||||
robot.topFly.setVelocity(targetVelocity);
|
||||
shootStatus = ShootState.RESET;
|
||||
break;
|
||||
|
||||
case RESET:
|
||||
if (shootTimer.milliseconds() > 3000) {
|
||||
shootStatus = ShootState.IDLE;
|
||||
shooting = false;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
if (gamepad2.yWasPressed()) {
|
||||
shooting = false;
|
||||
robot.activeTransfer.setPosition(.7);
|
||||
}
|
||||
|
||||
if (gamepad2.right_trigger > 0.5) {
|
||||
robot.turret.setPower(.05);
|
||||
} else if (gamepad2.left_trigger > 0.5) {
|
||||
robot.turret.setPower(.05);
|
||||
} else {
|
||||
robot.turret.setPower(0);
|
||||
}
|
||||
|
||||
drive.setWeightedDrivePower(driveDirection);
|
||||
drive.update();
|
||||
|
||||
telemetry.update();
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
@@ -16,7 +16,7 @@ public class FlywheelTuner extends LinearOpMode {
|
||||
robot.initialize(hardwareMap);
|
||||
|
||||
// ticks/sec targets (NOT rpm)
|
||||
double highVelocity = 1500;
|
||||
double highVelocity = 1700;
|
||||
double lowVelocity = -900;
|
||||
double curTargetVelocity = highVelocity;
|
||||
|
||||
@@ -52,7 +52,8 @@ public class FlywheelTuner extends LinearOpMode {
|
||||
// keep P/F non-negative (usually)
|
||||
if (P < 0) P = 0;
|
||||
if (F < 0) F = 0;
|
||||
|
||||
// P - 1.09
|
||||
// F - 14.12
|
||||
// ---- apply PIDF LIVE ----
|
||||
PIDFCoefficients pidf = new PIDFCoefficients(P, 0, 0, F);
|
||||
robot.bottomFly.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, pidf);
|
||||
|
||||
@@ -1,337 +1,337 @@
|
||||
package org.firstinspires.ftc.teamcode.OUTDATED;
|
||||
|
||||
import com.acmerobotics.dashboard.config.Config;
|
||||
import com.arcrobotics.ftclib.drivebase.MecanumDrive;
|
||||
import com.arcrobotics.ftclib.gamepad.GamepadEx;
|
||||
import com.arcrobotics.ftclib.gamepad.GamepadKeys;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||
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.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;
|
||||
import org.firstinspires.ftc.vision.VisionPortal;
|
||||
import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
|
||||
import org.firstinspires.ftc.vision.apriltag.AprilTagGameDatabase;
|
||||
import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;
|
||||
|
||||
import java.util.List;
|
||||
@Config
|
||||
@TeleOp(name = "Auto-aiming turret (Webcam AprilTag)", group = "Iterative Opmode")
|
||||
public class AutoLock extends OpMode {
|
||||
|
||||
// Hardware
|
||||
private DcMotorEx turretMotor;
|
||||
|
||||
// Vision
|
||||
private VisionPortal visionPortal;
|
||||
private AprilTagProcessor aprilTag;
|
||||
|
||||
// PID gains
|
||||
public double kP = 0.0012;
|
||||
public static double kI = 0.001;
|
||||
public static double kD = 0.002;
|
||||
|
||||
// Target center (degrees). 0 means center of camera.
|
||||
private double targetX = 0.0;
|
||||
|
||||
private double integral = 0.0;
|
||||
private double lastError = 0.0;
|
||||
|
||||
GamepadEx driverControl = null;
|
||||
GamepadEx armControl = null;
|
||||
|
||||
private final ElapsedTime loopTimer = new ElapsedTime();
|
||||
private final ElapsedTime targetLostTimer = new ElapsedTime();
|
||||
|
||||
// Tuning
|
||||
private static final double POSITION_TOLERANCE_DEG = 1.5;
|
||||
private static final double MIN_POWER = 0.05;
|
||||
private static final double MAX_POWER = 0.4;
|
||||
private static final double TARGET_LOST_TIMEOUT_SEC = 0.5;
|
||||
|
||||
// Flip if turret moves the wrong way
|
||||
private static final boolean INVERT_MOTOR = true;
|
||||
|
||||
// OPTIONAL: only lock onto certain tag IDs. Leave empty to accept any.
|
||||
// Example: new int[]{1,2,3}
|
||||
private static final int[] DESIRED_TAG_IDS = new int[]{23, 24}; // empty = any
|
||||
|
||||
private boolean targetWasVisible = false;
|
||||
private Position cameraPosition = new Position(DistanceUnit.INCH,
|
||||
0, 0, 0, 0);
|
||||
private YawPitchRollAngles cameraOrientation = new YawPitchRollAngles(AngleUnit.DEGREES,
|
||||
0, -90, 0, 0);
|
||||
|
||||
// Drive Variables
|
||||
double x, y, xy;
|
||||
double SPEEDCONTROL;
|
||||
boolean driveMode = true;
|
||||
MecanumDrive drive = null;
|
||||
Hware robot = null;
|
||||
|
||||
@Override
|
||||
public void init() {
|
||||
try {
|
||||
|
||||
// Hardware Map Setup
|
||||
robot = new Hware();
|
||||
robot.initialize(hardwareMap);
|
||||
|
||||
driverControl = new GamepadEx(gamepad1);
|
||||
armControl = new GamepadEx(gamepad2);
|
||||
|
||||
// DriveBase Setup
|
||||
drive = new MecanumDrive(true, robot.frontLeft, robot.frontRight, robot.backLeft, robot.backRight);
|
||||
|
||||
|
||||
turretMotor = hardwareMap.get(DcMotorEx.class, "turret");
|
||||
turretMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
|
||||
// AprilTag processor
|
||||
aprilTag = new AprilTagProcessor.Builder()
|
||||
.setTagFamily(AprilTagProcessor.TagFamily.TAG_36h11)
|
||||
.setTagLibrary(AprilTagGameDatabase.getDecodeTagLibrary())
|
||||
.setOutputUnits(DistanceUnit.INCH, AngleUnit.DEGREES)
|
||||
// If you have a Logitech C920/C270 etc, you can usually leave defaults.
|
||||
// If you have calibrated intrinsics, you can set them here (advanced).
|
||||
.build();
|
||||
|
||||
// Vision portal with webcam
|
||||
visionPortal = new VisionPortal.Builder()
|
||||
.setCamera(hardwareMap.get(WebcamName.class, "Webcam 1"))
|
||||
.addProcessor(aprilTag)
|
||||
.build();
|
||||
|
||||
|
||||
loopTimer.reset();
|
||||
targetLostTimer.reset();
|
||||
|
||||
telemetry.addData("Status", "Initialized (Webcam + AprilTag)");
|
||||
telemetry.addData("Motor Inverted", INVERT_MOTOR);
|
||||
} catch (Exception e) {
|
||||
telemetry.addData("Init Error", e.getMessage());
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void loop() {
|
||||
try {
|
||||
// Get current detections
|
||||
List<AprilTagDetection> detections = aprilTag.getDetections();
|
||||
|
||||
AprilTagDetection best = pickBestDetection(detections);
|
||||
|
||||
telemetry.addData("Detections", detections.size());
|
||||
for (AprilTagDetection d : detections) {
|
||||
telemetry.addData("ID", d.id);
|
||||
telemetry.addData("Metadata null?", d.metadata == null);
|
||||
telemetry.addData("Pose null?", d.ftcPose == null);
|
||||
|
||||
}
|
||||
|
||||
/** DRIVER CONTROLS **/
|
||||
|
||||
if (gamepad2.x) { driveMode = true; } else if (gamepad2.y) { driveMode = false;}
|
||||
|
||||
// Chassis
|
||||
if (!driveMode) { SPEEDCONTROL = 0;} else if (driverControl.getButton(GamepadKeys.Button.RIGHT_BUMPER) || driverControl.getButton(GamepadKeys.Button.LEFT_BUMPER)) { SPEEDCONTROL = .5; } else { SPEEDCONTROL = 1; }
|
||||
x = driverControl.getLeftX();
|
||||
y = driverControl.getLeftY();
|
||||
xy = driverControl.getRightX();
|
||||
|
||||
drive.driveRobotCentric(
|
||||
-x * SPEEDCONTROL,
|
||||
-y * SPEEDCONTROL,
|
||||
-xy * SPEEDCONTROL
|
||||
);
|
||||
|
||||
// Intake
|
||||
if (driverControl.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER) > 0.5) { robot.intake.set(1); } else if (driverControl.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER) > 0.5) { robot.intake.set(-1); } else { robot.intake.set(0); }
|
||||
|
||||
/** Arm Controls **/
|
||||
// if (armControl.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER) > 0.5) { robot.turret.set(.3); } else if (armControl.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER) > 0.5) { robot.turret.set(-0.3); } else { robot.turret.set(0); }
|
||||
// if (armControl.getButton(GamepadKeys.Button.X)) { robot.launchLeft.set(1); robot.launchRight.set(1); } else {robot.launchLeft.set(0); robot.launchRight.set(0);}
|
||||
// if (!driveMode) {robot.rightPTO.setPosition(1); robot.leftPTO.setPosition(1);} else {robot.rightPTO.setPosition(0); robot.rightPTO.setPosition(0);}
|
||||
|
||||
|
||||
if (best != null) {
|
||||
targetWasVisible = true;
|
||||
targetLostTimer.reset();
|
||||
|
||||
// ----- "tx" equivalent -----
|
||||
// AprilTag gives pose relative to camera. We want horizontal angle offset.
|
||||
// Use bearing/yaw style value if available from metadata or pose.
|
||||
//
|
||||
// Most reliable: compute from pose X/Z (left/right vs forward):
|
||||
// angle = atan2(x, z)
|
||||
//
|
||||
// In FTC pose:
|
||||
// - pose.x: left/right (inches or meters depending on config; typically inches)
|
||||
// - pose.z: forward distance
|
||||
//
|
||||
// If your coordinate axes differ, flip sign once using INVERT_MOTOR or sign flip here.
|
||||
double x = best.ftcPose.x; // left(+) / right(-) depending on convention
|
||||
double z = best.ftcPose.z; // forward distance
|
||||
double txDeg = Math.toDegrees(Math.atan2(x, z));
|
||||
|
||||
// Timing
|
||||
double dt = loopTimer.seconds();
|
||||
loopTimer.reset();
|
||||
|
||||
if (dt < 0.001) dt = 0.001;
|
||||
if (dt > 1.0) dt = 1.0;
|
||||
|
||||
// Error (positive tx means tag is to one side)
|
||||
double error = txDeg - targetX;
|
||||
|
||||
// PID
|
||||
integral += error * dt;
|
||||
integral = Math.max(-50, Math.min(50, integral)); // anti-windup
|
||||
|
||||
double derivative = (error - lastError) / dt;
|
||||
|
||||
double pidOutput = (kP * error) + (kI * integral) + (kD * derivative);
|
||||
|
||||
// Deadband
|
||||
if (Math.abs(error) < POSITION_TOLERANCE_DEG) {
|
||||
pidOutput = 0;
|
||||
integral = 0;
|
||||
}
|
||||
|
||||
// Min power to overcome friction
|
||||
if (pidOutput != 0 && Math.abs(pidOutput) < MIN_POWER) {
|
||||
pidOutput = MIN_POWER * Math.signum(pidOutput);
|
||||
}
|
||||
|
||||
// Clamp
|
||||
double motorPower = clamp(pidOutput, -MAX_POWER, MAX_POWER);
|
||||
|
||||
if (INVERT_MOTOR) motorPower = -motorPower;
|
||||
|
||||
if (!Double.isFinite(motorPower)) {
|
||||
motorPower = 0;
|
||||
resetPID();
|
||||
}
|
||||
|
||||
turretMotor.setPower(motorPower);
|
||||
lastError = error;
|
||||
|
||||
telemetry.addData("Status", "LOCKED ON");
|
||||
telemetry.addData("Tag ID", best.id);
|
||||
telemetry.addData("tx (deg)", "%.2f", txDeg);
|
||||
telemetry.addData("Error", "%.2f", error);
|
||||
telemetry.addData("PID Output", "%.3f", pidOutput);
|
||||
telemetry.addData("Motor Power", "%.3f", motorPower);
|
||||
|
||||
// Extra pose telemetry (helpful for debugging)
|
||||
telemetry.addData("Pose x", "%.2f", best.ftcPose.x);
|
||||
telemetry.addData("Pose z", "%.2f", best.ftcPose.z);
|
||||
telemetry.addData("Range: ", best.ftcPose.range);
|
||||
telemetry.addData("Bearing", "%.2f deg", best.ftcPose.bearing);
|
||||
telemetry.addData("Yaw", "%.2f deg", best.ftcPose.yaw);
|
||||
|
||||
if(best.ftcPose.range < 40){
|
||||
kP = 0.008;
|
||||
} else if (best.ftcPose.range < 70) {
|
||||
kP = 0.005;
|
||||
} else if (best.ftcPose.range < 90){
|
||||
kP = 0.004;
|
||||
} else if (best.ftcPose.range < 110) {
|
||||
kP = 0.003;
|
||||
} else if (best.ftcPose.range < 130) {
|
||||
kP = 0.002;
|
||||
} else if (best.ftcPose.range < 150) {
|
||||
kP = 0.001;
|
||||
} else {
|
||||
kP = 0.0005;
|
||||
}
|
||||
telemetry.addData("kP: ", kP);
|
||||
|
||||
} else {
|
||||
handleNoTarget();
|
||||
}
|
||||
|
||||
} catch (Exception e) {
|
||||
telemetry.addData("Error", e.getMessage());
|
||||
telemetry.addData("Error Type", e.getClass().getSimpleName());
|
||||
stopTurret();
|
||||
}
|
||||
|
||||
telemetry.update();
|
||||
}
|
||||
|
||||
/**
|
||||
* Pick "best" detection. Simple approach:
|
||||
* - If DESIRED_TAG_IDS set: only accept those
|
||||
* - Choose closest (smallest range) among candidates
|
||||
*/
|
||||
private AprilTagDetection pickBestDetection(List<AprilTagDetection> detections) {
|
||||
if (detections == null || detections.isEmpty()) return null;
|
||||
|
||||
AprilTagDetection best = null;
|
||||
double bestRange = Double.POSITIVE_INFINITY;
|
||||
|
||||
for (AprilTagDetection d : detections) {
|
||||
if (d == null) continue;
|
||||
|
||||
if (!isDesiredId(d.id)) continue;
|
||||
|
||||
// Use range as "quality" metric
|
||||
double r = d.ftcPose.range;
|
||||
if (r < bestRange) {
|
||||
bestRange = r;
|
||||
best = d;
|
||||
}
|
||||
}
|
||||
return best;
|
||||
}
|
||||
|
||||
private boolean isDesiredId(int id) {
|
||||
if (DESIRED_TAG_IDS == null || DESIRED_TAG_IDS.length == 0) return true;
|
||||
for (int x : DESIRED_TAG_IDS) {
|
||||
if (x == id) return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
private void handleNoTarget() {
|
||||
if (targetWasVisible && targetLostTimer.seconds() < TARGET_LOST_TIMEOUT_SEC) {
|
||||
telemetry.addData("Status", "TRACKING LOST - Coasting");
|
||||
telemetry.addData("Time Lost", "%.2f s", targetLostTimer.seconds());
|
||||
// You could also keep last motor power for a brief time if you store it.
|
||||
} else {
|
||||
stopTurret();
|
||||
telemetry.addData("Status", "SEARCHING");
|
||||
telemetry.addData("Target Visible", "No");
|
||||
}
|
||||
}
|
||||
|
||||
private void stopTurret() {
|
||||
if (turretMotor != null) turretMotor.setPower(0);
|
||||
resetPID();
|
||||
targetWasVisible = false;
|
||||
}
|
||||
|
||||
private void resetPID() {
|
||||
integral = 0;
|
||||
lastError = 0;
|
||||
}
|
||||
|
||||
private static double clamp(double v, double lo, double hi) {
|
||||
return Math.max(lo, Math.min(hi, v));
|
||||
}
|
||||
|
||||
@Override
|
||||
public void stop() {
|
||||
try {
|
||||
if (turretMotor != null) turretMotor.setPower(0);
|
||||
if (visionPortal != null) visionPortal.close();
|
||||
} catch (Exception ignored) {}
|
||||
}
|
||||
}
|
||||
//package org.firstinspires.ftc.teamcode.OUTDATED;
|
||||
//
|
||||
//import com.acmerobotics.dashboard.config.Config;
|
||||
//import com.arcrobotics.ftclib.drivebase.MecanumDrive;
|
||||
//import com.arcrobotics.ftclib.gamepad.GamepadEx;
|
||||
//import com.arcrobotics.ftclib.gamepad.GamepadKeys;
|
||||
//import com.qualcomm.robotcore.eventloop.opmode.OpMode;
|
||||
//import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||
//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.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;
|
||||
//import org.firstinspires.ftc.vision.VisionPortal;
|
||||
//import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
|
||||
//import org.firstinspires.ftc.vision.apriltag.AprilTagGameDatabase;
|
||||
//import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;
|
||||
//
|
||||
//import java.util.List;
|
||||
//@Config
|
||||
//@TeleOp(name = "Auto-aiming turret (Webcam AprilTag)", group = "Iterative Opmode")
|
||||
//public class AutoLock extends OpMode {
|
||||
//
|
||||
// // Hardware
|
||||
// private DcMotorEx turretMotor;
|
||||
//
|
||||
// // Vision
|
||||
// private VisionPortal visionPortal;
|
||||
// private AprilTagProcessor aprilTag;
|
||||
//
|
||||
// // PID gains
|
||||
// public double kP = 0.0012;
|
||||
// public static double kI = 0.001;
|
||||
// public static double kD = 0.002;
|
||||
//
|
||||
// // Target center (degrees). 0 means center of camera.
|
||||
// private double targetX = 0.0;
|
||||
//
|
||||
// private double integral = 0.0;
|
||||
// private double lastError = 0.0;
|
||||
//
|
||||
// GamepadEx driverControl = null;
|
||||
// GamepadEx armControl = null;
|
||||
//
|
||||
// private final ElapsedTime loopTimer = new ElapsedTime();
|
||||
// private final ElapsedTime targetLostTimer = new ElapsedTime();
|
||||
//
|
||||
// // Tuning
|
||||
// private static final double POSITION_TOLERANCE_DEG = 1.5;
|
||||
// private static final double MIN_POWER = 0.05;
|
||||
// private static final double MAX_POWER = 0.4;
|
||||
// private static final double TARGET_LOST_TIMEOUT_SEC = 0.5;
|
||||
//
|
||||
// // Flip if turret moves the wrong way
|
||||
// private static final boolean INVERT_MOTOR = true;
|
||||
//
|
||||
// // OPTIONAL: only lock onto certain tag IDs. Leave empty to accept any.
|
||||
// // Example: new int[]{1,2,3}
|
||||
// private static final int[] DESIRED_TAG_IDS = new int[]{23, 24}; // empty = any
|
||||
//
|
||||
// private boolean targetWasVisible = false;
|
||||
// private Position cameraPosition = new Position(DistanceUnit.INCH,
|
||||
// 0, 0, 0, 0);
|
||||
// private YawPitchRollAngles cameraOrientation = new YawPitchRollAngles(AngleUnit.DEGREES,
|
||||
// 0, -90, 0, 0);
|
||||
//
|
||||
// // Drive Variables
|
||||
// double x, y, xy;
|
||||
// double SPEEDCONTROL;
|
||||
// boolean driveMode = true;
|
||||
// MecanumDrive drive = null;
|
||||
// Hware robot = null;
|
||||
//
|
||||
// @Override
|
||||
// public void init() {
|
||||
// try {
|
||||
//
|
||||
// // Hardware Map Setup
|
||||
// robot = new Hware();
|
||||
// robot.initialize(hardwareMap);
|
||||
//
|
||||
// driverControl = new GamepadEx(gamepad1);
|
||||
// armControl = new GamepadEx(gamepad2);
|
||||
//
|
||||
// // DriveBase Setup
|
||||
//// drive = new MecanumDrive(true, robot.frontLeft, robot.frontRight, robot.backLeft, robot.backRight);
|
||||
//
|
||||
//
|
||||
// turretMotor = hardwareMap.get(DcMotorEx.class, "turret");
|
||||
// turretMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
//
|
||||
// // AprilTag processor
|
||||
// aprilTag = new AprilTagProcessor.Builder()
|
||||
// .setTagFamily(AprilTagProcessor.TagFamily.TAG_36h11)
|
||||
// .setTagLibrary(AprilTagGameDatabase.getDecodeTagLibrary())
|
||||
// .setOutputUnits(DistanceUnit.INCH, AngleUnit.DEGREES)
|
||||
// // If you have a Logitech C920/C270 etc, you can usually leave defaults.
|
||||
// // If you have calibrated intrinsics, you can set them here (advanced).
|
||||
// .build();
|
||||
//
|
||||
// // Vision portal with webcam
|
||||
// visionPortal = new VisionPortal.Builder()
|
||||
// .setCamera(hardwareMap.get(WebcamName.class, "Webcam 1"))
|
||||
// .addProcessor(aprilTag)
|
||||
// .build();
|
||||
//
|
||||
//
|
||||
// loopTimer.reset();
|
||||
// targetLostTimer.reset();
|
||||
//
|
||||
// telemetry.addData("Status", "Initialized (Webcam + AprilTag)");
|
||||
// telemetry.addData("Motor Inverted", INVERT_MOTOR);
|
||||
// } catch (Exception e) {
|
||||
// telemetry.addData("Init Error", e.getMessage());
|
||||
// }
|
||||
// }
|
||||
//
|
||||
// @Override
|
||||
// public void loop() {
|
||||
// try {
|
||||
// // Get current detections
|
||||
// List<AprilTagDetection> detections = aprilTag.getDetections();
|
||||
//
|
||||
// AprilTagDetection best = pickBestDetection(detections);
|
||||
//
|
||||
// telemetry.addData("Detections", detections.size());
|
||||
// for (AprilTagDetection d : detections) {
|
||||
// telemetry.addData("ID", d.id);
|
||||
// telemetry.addData("Metadata null?", d.metadata == null);
|
||||
// telemetry.addData("Pose null?", d.ftcPose == null);
|
||||
//
|
||||
// }
|
||||
//
|
||||
// /** DRIVER CONTROLS **/
|
||||
//
|
||||
// if (gamepad2.x) { driveMode = true; } else if (gamepad2.y) { driveMode = false;}
|
||||
//
|
||||
// // Chassis
|
||||
// if (!driveMode) { SPEEDCONTROL = 0;} else if (driverControl.getButton(GamepadKeys.Button.RIGHT_BUMPER) || driverControl.getButton(GamepadKeys.Button.LEFT_BUMPER)) { SPEEDCONTROL = .5; } else { SPEEDCONTROL = 1; }
|
||||
// x = driverControl.getLeftX();
|
||||
// y = driverControl.getLeftY();
|
||||
// xy = driverControl.getRightX();
|
||||
//
|
||||
// drive.driveRobotCentric(
|
||||
// -x * SPEEDCONTROL,
|
||||
// -y * SPEEDCONTROL,
|
||||
// -xy * SPEEDCONTROL
|
||||
// );
|
||||
//
|
||||
// // Intake
|
||||
// if (driverControl.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER) > 0.5) { robot.intake.set(1); } else if (driverControl.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER) > 0.5) { robot.intake.set(-1); } else { robot.intake.set(0); }
|
||||
//
|
||||
// /** Arm Controls **/
|
||||
//// if (armControl.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER) > 0.5) { robot.turret.set(.3); } else if (armControl.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER) > 0.5) { robot.turret.set(-0.3); } else { robot.turret.set(0); }
|
||||
//// if (armControl.getButton(GamepadKeys.Button.X)) { robot.launchLeft.set(1); robot.launchRight.set(1); } else {robot.launchLeft.set(0); robot.launchRight.set(0);}
|
||||
//// if (!driveMode) {robot.rightPTO.setPosition(1); robot.leftPTO.setPosition(1);} else {robot.rightPTO.setPosition(0); robot.rightPTO.setPosition(0);}
|
||||
//
|
||||
//
|
||||
// if (best != null) {
|
||||
// targetWasVisible = true;
|
||||
// targetLostTimer.reset();
|
||||
//
|
||||
// // ----- "tx" equivalent -----
|
||||
// // AprilTag gives pose relative to camera. We want horizontal angle offset.
|
||||
// // Use bearing/yaw style value if available from metadata or pose.
|
||||
// //
|
||||
// // Most reliable: compute from pose X/Z (left/right vs forward):
|
||||
// // angle = atan2(x, z)
|
||||
// //
|
||||
// // In FTC pose:
|
||||
// // - pose.x: left/right (inches or meters depending on config; typically inches)
|
||||
// // - pose.z: forward distance
|
||||
// //
|
||||
// // If your coordinate axes differ, flip sign once using INVERT_MOTOR or sign flip here.
|
||||
// double x = best.ftcPose.x; // left(+) / right(-) depending on convention
|
||||
// double z = best.ftcPose.z; // forward distance
|
||||
// double txDeg = Math.toDegrees(Math.atan2(x, z));
|
||||
//
|
||||
// // Timing
|
||||
// double dt = loopTimer.seconds();
|
||||
// loopTimer.reset();
|
||||
//
|
||||
// if (dt < 0.001) dt = 0.001;
|
||||
// if (dt > 1.0) dt = 1.0;
|
||||
//
|
||||
// // Error (positive tx means tag is to one side)
|
||||
// double error = txDeg - targetX;
|
||||
//
|
||||
// // PID
|
||||
// integral += error * dt;
|
||||
// integral = Math.max(-50, Math.min(50, integral)); // anti-windup
|
||||
//
|
||||
// double derivative = (error - lastError) / dt;
|
||||
//
|
||||
// double pidOutput = (kP * error) + (kI * integral) + (kD * derivative);
|
||||
//
|
||||
// // Deadband
|
||||
// if (Math.abs(error) < POSITION_TOLERANCE_DEG) {
|
||||
// pidOutput = 0;
|
||||
// integral = 0;
|
||||
// }
|
||||
//
|
||||
// // Min power to overcome friction
|
||||
// if (pidOutput != 0 && Math.abs(pidOutput) < MIN_POWER) {
|
||||
// pidOutput = MIN_POWER * Math.signum(pidOutput);
|
||||
// }
|
||||
//
|
||||
// // Clamp
|
||||
// double motorPower = clamp(pidOutput, -MAX_POWER, MAX_POWER);
|
||||
//
|
||||
// if (INVERT_MOTOR) motorPower = -motorPower;
|
||||
//
|
||||
// if (!Double.isFinite(motorPower)) {
|
||||
// motorPower = 0;
|
||||
// resetPID();
|
||||
// }
|
||||
//
|
||||
// turretMotor.setPower(motorPower);
|
||||
// lastError = error;
|
||||
//
|
||||
// telemetry.addData("Status", "LOCKED ON");
|
||||
// telemetry.addData("Tag ID", best.id);
|
||||
// telemetry.addData("tx (deg)", "%.2f", txDeg);
|
||||
// telemetry.addData("Error", "%.2f", error);
|
||||
// telemetry.addData("PID Output", "%.3f", pidOutput);
|
||||
// telemetry.addData("Motor Power", "%.3f", motorPower);
|
||||
//
|
||||
// // Extra pose telemetry (helpful for debugging)
|
||||
// telemetry.addData("Pose x", "%.2f", best.ftcPose.x);
|
||||
// telemetry.addData("Pose z", "%.2f", best.ftcPose.z);
|
||||
// telemetry.addData("Range: ", best.ftcPose.range);
|
||||
// telemetry.addData("Bearing", "%.2f deg", best.ftcPose.bearing);
|
||||
// telemetry.addData("Yaw", "%.2f deg", best.ftcPose.yaw);
|
||||
//
|
||||
// if(best.ftcPose.range < 40){
|
||||
// kP = 0.008;
|
||||
// } else if (best.ftcPose.range < 70) {
|
||||
// kP = 0.005;
|
||||
// } else if (best.ftcPose.range < 90){
|
||||
// kP = 0.004;
|
||||
// } else if (best.ftcPose.range < 110) {
|
||||
// kP = 0.003;
|
||||
// } else if (best.ftcPose.range < 130) {
|
||||
// kP = 0.002;
|
||||
// } else if (best.ftcPose.range < 150) {
|
||||
// kP = 0.001;
|
||||
// } else {
|
||||
// kP = 0.0005;
|
||||
// }
|
||||
// telemetry.addData("kP: ", kP);
|
||||
//
|
||||
// } else {
|
||||
// handleNoTarget();
|
||||
// }
|
||||
//
|
||||
// } catch (Exception e) {
|
||||
// telemetry.addData("Error", e.getMessage());
|
||||
// telemetry.addData("Error Type", e.getClass().getSimpleName());
|
||||
// stopTurret();
|
||||
// }
|
||||
//
|
||||
// telemetry.update();
|
||||
// }
|
||||
//
|
||||
// /**
|
||||
// * Pick "best" detection. Simple approach:
|
||||
// * - If DESIRED_TAG_IDS set: only accept those
|
||||
// * - Choose closest (smallest range) among candidates
|
||||
// */
|
||||
// private AprilTagDetection pickBestDetection(List<AprilTagDetection> detections) {
|
||||
// if (detections == null || detections.isEmpty()) return null;
|
||||
//
|
||||
// AprilTagDetection best = null;
|
||||
// double bestRange = Double.POSITIVE_INFINITY;
|
||||
//
|
||||
// for (AprilTagDetection d : detections) {
|
||||
// if (d == null) continue;
|
||||
//
|
||||
// if (!isDesiredId(d.id)) continue;
|
||||
//
|
||||
// // Use range as "quality" metric
|
||||
// double r = d.ftcPose.range;
|
||||
// if (r < bestRange) {
|
||||
// bestRange = r;
|
||||
// best = d;
|
||||
// }
|
||||
// }
|
||||
// return best;
|
||||
// }
|
||||
//
|
||||
// private boolean isDesiredId(int id) {
|
||||
// if (DESIRED_TAG_IDS == null || DESIRED_TAG_IDS.length == 0) return true;
|
||||
// for (int x : DESIRED_TAG_IDS) {
|
||||
// if (x == id) return true;
|
||||
// }
|
||||
// return false;
|
||||
// }
|
||||
//
|
||||
// private void handleNoTarget() {
|
||||
// if (targetWasVisible && targetLostTimer.seconds() < TARGET_LOST_TIMEOUT_SEC) {
|
||||
// telemetry.addData("Status", "TRACKING LOST - Coasting");
|
||||
// telemetry.addData("Time Lost", "%.2f s", targetLostTimer.seconds());
|
||||
// // You could also keep last motor power for a brief time if you store it.
|
||||
// } else {
|
||||
// stopTurret();
|
||||
// telemetry.addData("Status", "SEARCHING");
|
||||
// telemetry.addData("Target Visible", "No");
|
||||
// }
|
||||
// }
|
||||
//
|
||||
// private void stopTurret() {
|
||||
// if (turretMotor != null) turretMotor.setPower(0);
|
||||
// resetPID();
|
||||
// targetWasVisible = false;
|
||||
// }
|
||||
//
|
||||
// private void resetPID() {
|
||||
// integral = 0;
|
||||
// lastError = 0;
|
||||
// }
|
||||
//
|
||||
// private static double clamp(double v, double lo, double hi) {
|
||||
// return Math.max(lo, Math.min(hi, v));
|
||||
// }
|
||||
//
|
||||
// @Override
|
||||
// public void stop() {
|
||||
// try {
|
||||
// if (turretMotor != null) turretMotor.setPower(0);
|
||||
// if (visionPortal != null) visionPortal.close();
|
||||
// } catch (Exception ignored) {}
|
||||
// }
|
||||
//}
|
||||
@@ -15,8 +15,8 @@ import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
|
||||
public class Hware {
|
||||
|
||||
HardwareMap hardwareMap;
|
||||
public Motor frontRight, frontLeft, backRight, backLeft, intake;
|
||||
public DcMotorEx turret, bottomFly, topFly;
|
||||
public Motor intake;
|
||||
public DcMotorEx frontRight, frontLeft, backRight, backLeft, turret, bottomFly, topFly;
|
||||
public SimpleServo activeTransfer, rightPTO, leftPTO, launchHood;
|
||||
// public Limelight3A limelight;
|
||||
public VoltageSensor batteryVoltageSensor;
|
||||
@@ -34,10 +34,15 @@ public class Hware {
|
||||
// Chassis Motors
|
||||
|
||||
|
||||
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 = hardwareMap.get(DcMotorEx.class, "frontRight");
|
||||
frontLeft = hardwareMap.get(DcMotorEx.class, "frontLeft");
|
||||
backRight = hardwareMap.get(DcMotorEx.class, "backRight");
|
||||
backLeft = hardwareMap.get(DcMotorEx.class, "backLeft");
|
||||
//
|
||||
// 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);
|
||||
|
||||
// Intake
|
||||
intake = new Motor(hardwareMap, "intake", Motor.GoBILDA.RPM_1620);
|
||||
@@ -63,10 +68,10 @@ public class Hware {
|
||||
// webcam = hardwareMap.get(WebcamName.class, "cam");
|
||||
|
||||
// Zero Power Behaviors
|
||||
frontRight.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE);
|
||||
frontLeft.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE);
|
||||
backLeft.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE);
|
||||
backRight.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE);
|
||||
frontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
frontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
backLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
intake.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE);
|
||||
|
||||
batteryVoltageSensor = hardwareMap.get(VoltageSensor.class, "Control Hub"); // Or "Expansion Hub"
|
||||
@@ -74,7 +79,9 @@ public class Hware {
|
||||
// To get the actual number:
|
||||
|
||||
|
||||
PIDFCoefficients pidf = new PIDFCoefficients(3, 0, 0, 14
|
||||
// P - 1.09
|
||||
// F - 14.12
|
||||
PIDFCoefficients pidf = new PIDFCoefficients(1.09, 0, 0, 14.12
|
||||
);
|
||||
|
||||
bottomFly.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, pidf);
|
||||
@@ -82,6 +89,15 @@ public class Hware {
|
||||
|
||||
bottomFly.setDirection(DcMotorSimple.Direction.REVERSE);
|
||||
|
||||
turret.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
||||
|
||||
// Turret Positions
|
||||
// Center = 8
|
||||
// 45 deg = -88
|
||||
// -45 deg = 88
|
||||
// 90 deg = -240
|
||||
// -90 deg = 240
|
||||
|
||||
//Vision
|
||||
// limelight = hardwareMap.get(Limelight3A.class, "limelight");
|
||||
}
|
||||
|
||||
@@ -11,8 +11,7 @@ import org.firstinspires.ftc.teamcode.OUTDATED.Hware;
|
||||
@Config
|
||||
|
||||
public class PTOTest extends LinearOpMode {
|
||||
public static double leftPTO = 0;
|
||||
public static double rightPTO = 0;
|
||||
public static double activeTransfer = 0;
|
||||
|
||||
@Override
|
||||
public void runOpMode() throws InterruptedException {
|
||||
@@ -21,8 +20,7 @@ public class PTOTest extends LinearOpMode {
|
||||
|
||||
waitForStart();
|
||||
while (opModeIsActive()) {
|
||||
robot.leftPTO.setPosition(leftPTO);
|
||||
robot.rightPTO.setPosition(rightPTO);
|
||||
robot.activeTransfer.setPosition(activeTransfer);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -16,23 +16,24 @@ public class ShooterSubsystem {
|
||||
|
||||
public ShooterSubsystem() {
|
||||
// Top Row (Y = 60)
|
||||
shotData.add(new ShotPoint(60, 36, 1600, 0.35)); // Field (-36, 60)
|
||||
shotData.add(new ShotPoint(60, 12, 1700, 0.63)); // Field (-12, 60)
|
||||
shotData.add(new ShotPoint(60, 36, 1500, 0.32)); // Field (-36, 60)
|
||||
shotData.add(new ShotPoint(60, 12, 1700, 0.57)); // Field (-12, 60)
|
||||
shotData.add(new ShotPoint(60, -12, 1800, 0.63)); // Field (12, 60)
|
||||
shotData.add(new ShotPoint(60, -36, 2000, 0.68)); // Field (36, 60)
|
||||
|
||||
// Field Middle Row (Field Y = 36)
|
||||
shotData.add(new ShotPoint(36, 36, 1600, 0.45)); // Field (-36, 36)
|
||||
shotData.add(new ShotPoint(36, 12, 1700, 0.55)); // Field (-12, 36)
|
||||
shotData.add(new ShotPoint(36, 36, 1570, 0.42)); // Field (-36, 36)
|
||||
shotData.add(new ShotPoint(36, 12, 1680, 0.55)); // Field (-12, 36)
|
||||
shotData.add(new ShotPoint(36, -12, 1800, 0.65)); // Field (12, 36)
|
||||
shotData.add(new ShotPoint(36, -36, 1900, 0.65)); // Field (36, 36)
|
||||
|
||||
// Field Bottom "V" Points (Field Y = 12)
|
||||
shotData.add(new ShotPoint(12, 12, 1750, 0.65)); // Field (-12, 12)
|
||||
shotData.add(new ShotPoint(12, 12, 1750, 0.62)); // Field (-12, 12)
|
||||
shotData.add(new ShotPoint(12, -12, 1900, 0.65)); // Field (12, 12)
|
||||
shotData.add(new ShotPoint(0, 0, 1900, 0.65)); // Field (12, 12)
|
||||
|
||||
shotData.add(new ShotPoint(-60, 12, 2300, 0.7));
|
||||
shotData.add(new ShotPoint(-60, 12, 2300, 0.72));
|
||||
shotData.add(new ShotPoint(-60, -12, 2300, 0.7));
|
||||
|
||||
}
|
||||
|
||||
|
||||
@@ -0,0 +1,71 @@
|
||||
package org.firstinspires.ftc.teamcode;
|
||||
|
||||
import java.util.ArrayList;
|
||||
import java.util.Collections;
|
||||
import java.util.List;
|
||||
|
||||
public class ShooterSubsystemRed {
|
||||
public static class ShotPoint {
|
||||
double x, y, velocity, hood;
|
||||
public ShotPoint(double x, double y, double v, double h) {
|
||||
this.x = x; this.y = y; this.velocity = v; this.hood = h;
|
||||
}
|
||||
}
|
||||
|
||||
private final List<ShotPoint> shotData = new ArrayList<>();
|
||||
|
||||
public ShooterSubsystemRed() {
|
||||
// Top Row (X = 60) - Y Reflected
|
||||
shotData.add(new ShotPoint(60, -36, 1500, 0.32));
|
||||
shotData.add(new ShotPoint(60, -12, 1700, 0.57));
|
||||
shotData.add(new ShotPoint(60, 12, 1800, 0.63));
|
||||
shotData.add(new ShotPoint(60, 36, 2000, 0.68));
|
||||
|
||||
// Field Middle Row (X = 36) - Y Reflected
|
||||
shotData.add(new ShotPoint(36, -36, 1570, 0.42));
|
||||
shotData.add(new ShotPoint(36, -12, 1680, 0.55));
|
||||
shotData.add(new ShotPoint(36, 12, 1800, 0.65));
|
||||
shotData.add(new ShotPoint(36, 36, 1900, 0.65));
|
||||
|
||||
// Field Bottom "V" Points (X = 12) - Y Reflected
|
||||
shotData.add(new ShotPoint(12, -12, 1750, 0.62));
|
||||
shotData.add(new ShotPoint(12, 12, 1900, 0.65));
|
||||
shotData.add(new ShotPoint(0, 0, 1900, 0.65));
|
||||
|
||||
// Far Points - Y Reflected
|
||||
shotData.add(new ShotPoint(-60, -12, 2300, 0.72));
|
||||
shotData.add(new ShotPoint(-60, 12, 2300, 0.7));
|
||||
}
|
||||
|
||||
public double[] getInterpolatedShot(double robotX, double robotY) {
|
||||
class Node {
|
||||
ShotPoint p; double dist;
|
||||
Node(ShotPoint p, double d) { this.p = p; this.dist = d; }
|
||||
}
|
||||
|
||||
List<Node> nodes = new ArrayList<>();
|
||||
for (ShotPoint p : shotData) {
|
||||
nodes.add(new Node(p, Math.hypot(p.x - robotX, p.y - robotY)));
|
||||
}
|
||||
|
||||
Collections.sort(nodes, (a, b) -> Double.compare(a.dist, b.dist));
|
||||
|
||||
if (nodes.get(0).dist < 1.0) {
|
||||
return new double[]{nodes.get(0).p.velocity, nodes.get(0).p.hood};
|
||||
}
|
||||
|
||||
double totalWeight = 0;
|
||||
double weightedVel = 0;
|
||||
double weightedHood = 0;
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
Node n = nodes.get(i);
|
||||
double weight = 1.0 / (n.dist * n.dist);
|
||||
weightedVel += n.p.velocity * weight;
|
||||
weightedHood += n.p.hood * weight;
|
||||
totalWeight += weight;
|
||||
}
|
||||
|
||||
return new double[]{weightedVel / totalWeight, weightedHood / totalWeight};
|
||||
}
|
||||
}
|
||||
@@ -29,16 +29,18 @@ public class TeleOpTurretLock extends LinearOpMode {
|
||||
// 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 = 75;
|
||||
public static double GOAL_Y = 75;
|
||||
public static double GOAL_Y = -75;
|
||||
|
||||
public static double p = 0.00438, i = 0, d = 0.0001, f = 0.0053;
|
||||
public static double p = 0.0000003, i = 0, d = 0.00001, f = 0.007;
|
||||
public static double target = 0;
|
||||
|
||||
PersonalPID pid;
|
||||
|
||||
|
||||
public static double ticksperdegree = 9.738888 * 0.29969;
|
||||
|
||||
public int minTicks = (int) (ticksperdegree * (-90));
|
||||
public int maxTicks = (int) (ticksperdegree * (90));
|
||||
|
||||
PersonalPID pid;
|
||||
|
||||
Limelight3A limelight;
|
||||
|
||||
|
||||
@@ -70,7 +72,7 @@ public class TeleOpTurretLock extends LinearOpMode {
|
||||
|
||||
// Start pose (you said 0,0)
|
||||
drive.setPoseEstimate(new Pose2d(0, 0, 0));
|
||||
TurretLock turretLock = new TurretLock(robot.turret, limelight, TurretLock.SIDE.BLUE);
|
||||
// TurretLock turretLock = new TurretLock(robot.turret, limelight, TurretLock.SIDE.RED);
|
||||
|
||||
|
||||
// limelight
|
||||
@@ -95,6 +97,7 @@ public class TeleOpTurretLock extends LinearOpMode {
|
||||
limelight.start();
|
||||
robot.turret.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||
robot.turret.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
|
||||
pid = new PersonalPID(p, i, d, f);
|
||||
|
||||
while (opModeIsActive()) {
|
||||
result = limelight.getLatestResult();
|
||||
@@ -116,22 +119,36 @@ public class TeleOpTurretLock extends LinearOpMode {
|
||||
|
||||
Pose2d pose = drive.getPoseEstimate();
|
||||
|
||||
/* ---------------- UPDATE TARGETS (Dashboard) ---------------- */
|
||||
turretLock.goalX = GOAL_X;
|
||||
turretLock.goalY = GOAL_Y;
|
||||
// /* ---------------- UPDATE TARGETS (Dashboard) ---------------- */
|
||||
// turretLock.goalX = GOAL_X;
|
||||
// turretLock.goalY = GOAL_Y;
|
||||
|
||||
/* ---------------- 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
|
||||
// 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
|
||||
// telemetry.update();\
|
||||
//
|
||||
double targetTicks = turretLock.update(pose, result, vision, robot.turret.getCurrentPosition());
|
||||
robot.turret.setTargetPosition((int) targetTicks);
|
||||
robot.turret.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||
robot.turret.setPower(1);
|
||||
// pid.setPIDF(p, i, d, f);
|
||||
double target = ticksperdegree * vision;
|
||||
double targetTicks = pid.calculate(robot.turret.getCurrentPosition(), target);
|
||||
pid.setPIDF(p, i, d, f);
|
||||
|
||||
//double targetTicks = turretLock.update(pose, result, vision, robot.turret.getCurrentPosition());
|
||||
// robot.turret.setTargetPosition((int) (robot.turret.getCurrentPosition() - targetTicks));
|
||||
// robot.turret.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||
|
||||
if((robot.turret.getCurrentPosition() > maxTicks) && (targetTicks > 0)){
|
||||
targetTicks = targetTicks;
|
||||
} else if((robot.turret.getCurrentPosition() < maxTicks) && (targetTicks < 0)){
|
||||
targetTicks = targetTicks;
|
||||
|
||||
} else if((robot.turret.getCurrentPosition() > maxTicks) || (robot.turret.getCurrentPosition() < minTicks) ){
|
||||
targetTicks = 0;
|
||||
}
|
||||
|
||||
robot.turret.setPower(- targetTicks);
|
||||
|
||||
// int turretPos = robot.turret.getCurrentPosition();
|
||||
// double power = pid.calculate(turretPos, targetTicks);
|
||||
// robot.turret.setPower(power);
|
||||
@@ -140,17 +157,18 @@ public class TeleOpTurretLock extends LinearOpMode {
|
||||
// telemetry.addData("target", targetTicks);
|
||||
// telemetry.addData("currentPos", turretPos);
|
||||
|
||||
telemetry.addData("VisionAngle", turretLock.getBearing());
|
||||
telemetry.addData("Angle", turretLock.getFinalDeg());
|
||||
telemetry.addData("Ticks", Math.round(turretLock.getFinalDeg() * 2.543));
|
||||
telemetry.addData("MODE", turretLock.getTYPE());
|
||||
telemetry.addData("Tag ID", turretLock.getObeliskID());
|
||||
// telemetry.addData("VisionAngle", turretLock.getBearing());
|
||||
// telemetry.addData("Angle", turretLock.getFinalDeg());
|
||||
// telemetry.addData("Ticks", Math.round(turretLock.getFinalDeg() * 2.543));
|
||||
// telemetry.addData("MODE", turretLock.getTYPE());
|
||||
// telemetry.addData("Tag ID", turretLock.getObeliskID());
|
||||
telemetry.addData("Does Lime have Result", result.isValid());
|
||||
telemetry.addData("pid", targetTicks);
|
||||
|
||||
telemetry.addData("Botpose", result.getBotpose());
|
||||
// telemetry.addData("Botpose", result.getBotpose());
|
||||
telemetry.addData("Yaw", result.getTx());
|
||||
|
||||
telemetry.addData("Status", turretLock.getStatus());
|
||||
// telemetry.addData("Status", turretLock.getStatus());
|
||||
|
||||
telemetry.addData("LimelightInfo", limelight.getStatus());
|
||||
|
||||
|
||||
@@ -88,7 +88,7 @@ public class TurretLock {
|
||||
this.webcam = cam;
|
||||
switch (side) {
|
||||
case RED:
|
||||
goalX = -75;
|
||||
goalX = 75;
|
||||
goalY = -75;
|
||||
break;
|
||||
case BLUE:
|
||||
|
||||
@@ -45,7 +45,7 @@ public class DriveConstants {
|
||||
* convenience. Make sure to exclude any gear ratio included in MOTOR_CONFIG from GEAR_RATIO.
|
||||
*/
|
||||
public static double WHEEL_RADIUS = 2.047; // in
|
||||
public static double GEAR_RATIO = (24 / 22); // output (wheel) speed / input (motor) speed
|
||||
public static double GEAR_RATIO = (24.0 / 22.0); // output (wheel) speed / input (motor) speed
|
||||
public static double TRACK_WIDTH = 12.3; // in
|
||||
|
||||
/*
|
||||
|
||||
Reference in New Issue
Block a user