From df6e892be2847b49809c2b0b97ae17c650b7b791 Mon Sep 17 00:00:00 2001 From: FTC16750 Date: Sat, 3 Oct 2026 14:49:04 -0700 Subject: [PATCH 1/5] ok i did the thing code isnt great didnt get a chance to clean up some of the turret stuff will review when i get home i just needed to get some stuff so we could test pid values and velocity --- .../sixteen750/commands/TeleCommands.java | 8 ++ .../sixteen750/controls/DriverController.java | 9 +- .../teamcode/sixteen750/opmodes/MainTele.java | 8 +- .../subsystems/TurretSubsystem.java | 113 ++++++++++++++---- 4 files changed, 114 insertions(+), 24 deletions(-) diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/commands/TeleCommands.java b/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/commands/TeleCommands.java index ecc1329..aa4c73f 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/commands/TeleCommands.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/commands/TeleCommands.java @@ -49,6 +49,14 @@ public static Command GateClose(Robot r) { public static Command Track(Robot r) { return Command.create(r.turretsubsystem::setTurretTarget); } + public static Command Increase (Robot r) { + return Command.create(r.turretsubsystem::up); + } + public static Command Decrease (Robot r) { + return Command.create(r.turretsubsystem::down); + } + public static Command Launch (Robot r) {return Command.create(r.turretsubsystem::Launch);} } + diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/controls/DriverController.java b/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/controls/DriverController.java index 4615569..725c110 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/controls/DriverController.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/controls/DriverController.java @@ -32,6 +32,8 @@ public class DriverController implements Loggable { public CommandButton trackButton; public CommandButton intakeTrigger; + public CommandButton upButton; + public CommandButton downButton; public CommandAxis autoAim; public PedroDriver pedroDriver; @@ -65,13 +67,15 @@ public void AssignNamedControllerButton() { driveRightStick = gamepad.rightStick; intakeTrigger = gamepad.rightTrigger.getAsButton(); + upButton = gamepad.dpadUp; + downButton = gamepad.dpadDown; snailButton = gamepad.ps_triangle; launchButton = gamepad.rightBumper; spitButton = gamepad.ps_square; gateButton = gamepad.ps_cross; holdButton = gamepad.ps_circle; RelocButton = gamepad.ps_share; - turretlockButton = gamepad.dpadUp; + turretlockButton = gamepad.dpadLeft; trackButton = gamepad.leftBumper; } @@ -107,6 +111,9 @@ public void bindDriveControls() { public void bindLaunchControls() { trackButton.whilePressed(TeleCommands.Track(robot)); + launchButton.whilePressed(TeleCommands.Launch(robot)); + upButton.whenPressed(TeleCommands.Increase(robot)); + downButton.whenPressed(TeleCommands.Decrease(robot)); } public void bindIntakeControls() { diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/opmodes/MainTele.java b/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/opmodes/MainTele.java index 51b236d..3a4df59 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/opmodes/MainTele.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/opmodes/MainTele.java @@ -3,6 +3,7 @@ import static org.firstinspires.ftc.teamcode.sixteen750.Setup.HardwareNames.AprilTag_Pipeline; import com.bylazar.telemetry.PanelsTelemetry; +import com.bylazar.telemetry.TelemetryManager; import com.pedropathing.math.Pose; import com.qualcomm.hardware.limelightvision.Limelight3A; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; @@ -20,6 +21,7 @@ import org.firstinspires.ftc.teamcode.sixteen750.controls.DriverController; import org.firstinspires.ftc.teamcode.sixteen750.controls.OperatorController; import org.firstinspires.ftc.teamcode.sixteen750.helpers.StartingPosition; +import org.firstinspires.ftc.teamcode.sixteen750.subsystems.TurretSubsystem; @TeleOp(name = "MainTele") @SuppressWarnings("unused") @@ -31,6 +33,7 @@ public class MainTele extends CommandOpMode implements Loggable { public Hardware hardware; private Limelight3A limelight; private PanelsTelemetry panelsTelemetry; + TelemetryManager ptel; @Override public void uponInit() { @@ -46,8 +49,7 @@ public void uponInit() { CommandScheduler.scheduleForState( new SequentialCommandGroup( HeadingHelper.RestorePreviousPosition(robot.follower), - DrivingCommands.ResetGyro(controlsDriver.pedroDriver), - Command.create(robot.turretsubsystem::Launch) + DrivingCommands.ResetGyro(controlsDriver.pedroDriver) ), OpModeState.INIT ); @@ -69,6 +71,8 @@ public void uponInit() { * Starts polling for data. If you neglect to call start(), getLatestResult() will return null. */ limelight.start(); + + ptel = PanelsTelemetry.INSTANCE.getTelemetry(); } if (Setup.Connected.TURRETSUBSYSTEM) { // CommandScheduler.register(robot.launcherSubsystem); diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/subsystems/TurretSubsystem.java b/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/subsystems/TurretSubsystem.java index 36f24dc..e8bc238 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/subsystems/TurretSubsystem.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/subsystems/TurretSubsystem.java @@ -4,9 +4,10 @@ import com.bylazar.configurables.annotations.Configurable; import com.pedropathing.math.Pose; -import com.pedropathing.utils.Angle; import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.Gamepad; +import com.qualcomm.robotcore.hardware.PIDFCoefficients; import com.technototes.library.command.CommandScheduler; import com.technototes.library.hardware.motor.EncodedMotor; import com.technototes.library.hardware.servo.Servo; @@ -14,8 +15,10 @@ import com.technototes.library.logger.Loggable; import com.technototes.library.subsystem.Subsystem; import com.technototes.library.util.MathUtils; +import com.technototes.library.util.PIDFController; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit; import org.firstinspires.ftc.teamcode.sixteen750.Hardware; import org.firstinspires.ftc.teamcode.sixteen750.Robot; import org.firstinspires.ftc.teamcode.sixteen750.Setup; @@ -41,50 +44,88 @@ public class TurretSubsystem implements Loggable, Subsystem { public static double ABSOLUTE_HEAD = 0; @Log.Number (name = "Turret Heading") public static double TURRET_HEADING = 0; + @Log.Number (name = "Target Velocity") + public static double TARGET_VELO = 0; + @Log.Number (name = "Current Velocity") + public static double CURRENT_VELO = 0; @Log.Number (name = "Target Pose") public static Pose TARGET_POSE = new Pose(0,0); public static double UP = 0.1; public static double DOWN = 0.5; + public static double INCREASE = 15; + public static double ACTUAL_TARGET = 0; - public static double LAUNCHER_VELOCITY = -0.5; + public static double LAUNCHER_VELOCITY = 2400; boolean hasHardware; + Gamepad gamepad; EncodedMotor launcher; Servo turret1, turret2, hood; + // This the PIDF controller that's used manage the power. + // The PIDF values are set in the Config class above. + private final PIDFController pidfController; + // The input value is the error of the target velocity that ranges from + // -2800 to +2800 for a goBilda motor. + // It's output is a power value in the -1 to +1 range. + // So, P is probably in the range of .001-ish. + // For a velocity-targeting PIDF, we probably want an I value, not a D value. + public static PIDFCoefficients launchPID = new PIDFCoefficients(0.015, 0.0, 0.0, 0); + private Hardware hardware; + // Stuff used for the Feed Forward function. + // This one is highly variable, based on the amount of friction in the system + + public static double kStaticFriction = 0.183; + + public static double kDynamicFriction = 0.168; + + // This one tends to be somewhere between 0.0035 to 0.005 or so. + public static double kVelocityConstant = 0.0043; + public static double MotorResistance = 12 / 9.2; + + public static double GetFrictionConstant(boolean inMotion) { + return inMotion ? kDynamicFriction : kStaticFriction; + } + + // GoBilda says stall current of 9.2A at 12V, so V = I * R, R = 12 / 9.2 (about 1.3 ohms) + // As the motor heats up, resistance also increase, so we could increase this a little bit + // or maybe increase it over time to counteract that, but this is probably good enough. public TurretSubsystem(Hardware h, Robot r) { hasHardware = Setup.Connected.TURRETSUBSYSTEM; robot = r; // Do stuff in here if (hasHardware) { + hardware = h; launcher = h.launcher; turret1 = h.turret1; turret2 = h.turret2; hood = h.hood; CommandScheduler.register(this); + gamepad = null; launcher.setDirection(DcMotorSimple.Direction.REVERSE); } else { launcher = null; turret1 = null; turret2 = null; } - // Create the array to hold past current values + pidfController = new PIDFController( + launchPID, + target -> + (Math.signum(target) * // signum(<0) = -1, signum(>0) = 1, signum(0) = *0* + (GetFrictionConstant(getActualVelocity() != 0) + + getMotor1Current() * MotorResistance) + + kVelocityConstant * target) / + hardware.voltage() + ); } public void Launch() { // Spin the motors - setLauncherVelocity(LAUNCHER_VELOCITY); + setVelocityTarget(LAUNCHER_VELOCITY); } - public void HoodUp() { - setHoodPosition(UP); - } - - public void HoodDown() { - setHoodPosition(DOWN); - } // takes the already decided upon target and takes the current robot pose and does some math to figure out what angle the turret needs to point to face the target i just guessed which direction is positive should be easy to flip @@ -113,6 +154,14 @@ public double getDistance() { return Distance; } + public double increase() { + ACTUAL_TARGET = LAUNCHER_VELOCITY +INCREASE; + return ACTUAL_TARGET; + } + public double decrease() { + ACTUAL_TARGET = LAUNCHER_VELOCITY - INCREASE; + return ACTUAL_TARGET; + } // determines which hive target to aim for based on what half of the field we are public Pose getTargetPose() { @@ -139,6 +188,12 @@ public double getTurretPos() { public void setTurretTarget() { setTurretPosition(getTurretPos()); } + public void up() { + LAUNCHER_VELOCITY = increase(); + } + public void down() { + LAUNCHER_VELOCITY = decrease(); + } @Override @@ -148,28 +203,44 @@ public void periodic() { TARGET_POSE = getTargetPose(); TURRET_HEADING = getTurretAngle(); ROBOT_HEAD = robot.follower.pose().heading(); + TARGET_VELO = getVelocityTarget(); + CURRENT_VELO = getActualVelocity(); // Add an item to the array and update the index for the next update to the 'circular' array + double power = pidfController.update(getActualVelocity()); + setLauncherPower(power); } - private void setLauncherVelocity(double velo) { - if (hasHardware) { - launcher.setPower(0.8); - // launcher.setVelocity(velo); - } + public double getMotor1Current() { + return hasHardware ? launcher.getAmperage(CurrentUnit.AMPS) : 0; } - private void setHoodPosition(double pos) { + public double getActualVelocity() { if (hasHardware) { - hood.setPosition(pos); + return launcher.getVelocity(); + } else { + return pidfController.getTarget(); // Not a Number } } - private void setTurretPosition(double pos) { + // Explicitly set the target velocity for the motors + public void setVelocityTarget(double speed) { + pidfController.setTarget(speed); + } + public void setTurretPosition(double pos) { + turret1.setPosition(pos); + turret2.setPosition(pos); + } + + // Returns the current target velocity (which may be set explicitly, or automatically) + public double getVelocityTarget() { + return pidfController.getTarget(); + } + + private void setLauncherPower(double power) { if (hasHardware) { - turret1.setPosition(pos); - turret2.setPosition(pos); + launcher.setPower(power); } } } From bdb04f1ea1df3cdbe45f8f42a4166b1be7595cb6 Mon Sep 17 00:00:00 2001 From: stedman Date: Mon, 5 Oct 2026 08:38:13 -0700 Subject: [PATCH 2/5] ok did all the hood comp auto hood angle and auto velocity math the hood comp angle and velocity constants will most likely need to be tuned the velocity regression is prob gonna be the most far off the other idk --- .../sixteen750/commands/TeleCommands.java | 1 + .../sixteen750/controls/DriverController.java | 58 ++-------- .../subsystems/IntakeSubsystem.java | 4 +- .../subsystems/TurretSubsystem.java | 105 +++++++++++++----- 4 files changed, 87 insertions(+), 81 deletions(-) diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/commands/TeleCommands.java b/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/commands/TeleCommands.java index aa4c73f..9b4c9e5 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/commands/TeleCommands.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/commands/TeleCommands.java @@ -55,6 +55,7 @@ public static Command Increase (Robot r) { public static Command Decrease (Robot r) { return Command.create(r.turretsubsystem::down); } + public static Command AutoHood (Robot r) {return Command.create(r.turretsubsystem::setHoodAutoPos);} public static Command Launch (Robot r) {return Command.create(r.turretsubsystem::Launch);} diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/controls/DriverController.java b/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/controls/DriverController.java index 725c110..512a204 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/controls/DriverController.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/controls/DriverController.java @@ -28,14 +28,13 @@ public class DriverController implements Loggable { public CommandButton override; public CommandButton RelocButton; public CommandButton holdButton; - public CommandButton turretlockButton; public CommandButton trackButton; - + public CommandButton flywheelButton; public CommandButton intakeTrigger; public CommandButton upButton; public CommandButton downButton; - public CommandAxis autoAim; public PedroDriver pedroDriver; + public CommandButton hoodButton; public static double triggerThreshold = 0.1; @@ -55,10 +54,6 @@ public DriverController(CommandGamepad g, Robot r) { if (Setup.Connected.INTAKESUBSYSTEM) { bindIntakeControls(); } - - if (Setup.Connected.AIMINGSUBSYSTEM) { - bindAimControls(); - } } public void AssignNamedControllerButton() { @@ -69,14 +64,15 @@ public void AssignNamedControllerButton() { upButton = gamepad.dpadUp; downButton = gamepad.dpadDown; - snailButton = gamepad.ps_triangle; + snailButton = gamepad.leftBumper; launchButton = gamepad.rightBumper; + flywheelButton = gamepad.dpadLeft; spitButton = gamepad.ps_square; gateButton = gamepad.ps_cross; holdButton = gamepad.ps_circle; RelocButton = gamepad.ps_share; - turretlockButton = gamepad.dpadLeft; - trackButton = gamepad.leftBumper; + trackButton = gamepad.ps_triangle; + hoodButton = gamepad.dpadRight; } public void bindDriveControls() { @@ -88,30 +84,19 @@ public void bindDriveControls() { ); CommandScheduler.scheduleJoystick(pedroDriver); - // turboButton.whenPressed(DrivingCommands.TurboDriving(robot.drivebase)); - // turboButton.whenReleased(DrivingCommands.NormalDriving(robot.drivebase)); snailButton.whenPressedReleased( DrivingCommands.SnailDriving(pedroDriver), DrivingCommands.NormalDriving(pedroDriver) ); resetGyroButton.whenPressed(DrivingCommands.ResetGyro(pedroDriver)); - //MotorDecrease.whenPressed(TeleCommands.DecreaseMotor(robot)); - //MotorIncrease.whenPressed(TeleCommands.IncreaseMotor(robot)); - // if (Setup.Connected.LIMELIGHTSUBSYSTEM) { - // autoAim.whenPressed(DrivingCommands.AutoOrient(pedroDriver)); - // autoAim.whenReleased(DrivingCommands.NoAutoOrient(pedroDriver)); - // RelocButton.whenPressed(TeleCommands.LLRelocCommand(robot)); - //AltAutoAlign.whenPressed(new AltAutoOrient(robot)); - //AltAutoAlign.whenReleased(DrivingCommands.NormalDriving(pedroDriver)); - // } - // autoAim.whilePressed(new LLPipelineChangeCommand(hardware.limelight, Setup.HardwareNames.AprilTag_Pipeline)); } public void bindLaunchControls() { - trackButton.whilePressed(TeleCommands.Track(robot)); - launchButton.whilePressed(TeleCommands.Launch(robot)); + trackButton.whileInverseToggled(TeleCommands.Track(robot)); + flywheelButton.whileInverseToggled(TeleCommands.Launch(robot)); + hoodButton.whileInverseToggled(TeleCommands.AutoHood(robot)); upButton.whenPressed(TeleCommands.Increase(robot)); downButton.whenPressed(TeleCommands.Decrease(robot)); } @@ -127,29 +112,4 @@ public void bindIntakeControls() { launchButton.whenReleased(TeleCommands.Hold(robot)); gateButton.whenPressedReleased(TeleCommands.GateOpen(robot), TeleCommands.GateClose(robot)); } - - // spitTrigger.whilePressed(TeleCommands.Spit(robot.intakeSubsystem)); - // spitTrigger.whileReleased(TeleCommands.Intake(robot.intakeSubsystem)); - - public void bindAimControls() { - // if(yippee) { - // leverButton.whenPressed( - // TeleCommands.LeverStop(robot.aimingSubsystem)); - // yippee = false; - // } else { - // leverButton.whenPressed( - // TeleCommands.LeverGo(robot.aimingSubsystem)); - // yippee = true; - // } - // gateButton.whenPressed(TeleCommands.GateDown(robot)); - gateButton.whenPressed(TeleCommands.Feed(robot)); - gateButton.whenReleased(TeleCommands.IntakeStop(robot)); - // gateButton.whenReleased(TeleCommands.GateUp(robot)); - - // - holdButton.whilePressed(TeleCommands.Intake(robot)); - // holdButton.whenPressed(TeleCommands.GateDown(robot)); - holdButton.whenReleased(TeleCommands.IntakeStop(robot)); - //holdButton.whenReleased(TeleCommands.GateUp(robot)); - } } diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/subsystems/IntakeSubsystem.java b/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/subsystems/IntakeSubsystem.java index 247edf7..21b1748 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/subsystems/IntakeSubsystem.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/subsystems/IntakeSubsystem.java @@ -20,9 +20,9 @@ public class IntakeSubsystem implements Loggable, Subsystem { public static double HOLD_VELOCITY = 0.4; - public static double FEED_VELOCITY = 0.95; + public static double FEED_VELOCITY = 1; - public static double BLOCK_POSITION = 0.5; + public static double BLOCK_POSITION = 0.53; public static double FIRE_POSITION = 0.3; diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/subsystems/TurretSubsystem.java b/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/subsystems/TurretSubsystem.java index e8bc238..73d4ff6 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/subsystems/TurretSubsystem.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/subsystems/TurretSubsystem.java @@ -29,18 +29,17 @@ public class TurretSubsystem implements Loggable, Subsystem { Pose HIVE_TARGET1 = new Pose(58,53); Pose HIVE_TARGET2 = new Pose(58,88 ); - public static double TARGET_SWITCH_THRESHOLD = 71; + public static double TARGET_SWITCH_THRESHOLD = 72; public static double TURRET_CENTER = 0.5; - public static double TURRET_MIN = 0.1; - public static double TURRET_MAX = 0.9; - + public static double TURRET_MIN = 0.08; + public static double TURRET_MAX = 0.92; public static double ROTATE_LEFT = 0.15; public static double ROTATE_RIGHT = 0.85; - @Log.Number (name = "target servo Pos") + // @Log.Number (name = "target servo Pos") public static double TARGET_SERVO_POS = 0; @Log.Number (name = "Robot Heading") public static double ROBOT_HEAD = 0; - @Log.Number (name = "Absolute heading") + // @Log.Number (name = "Absolute heading") public static double ABSOLUTE_HEAD = 0; @Log.Number (name = "Turret Heading") public static double TURRET_HEADING = 0; @@ -50,13 +49,24 @@ public class TurretSubsystem implements Loggable, Subsystem { public static double CURRENT_VELO = 0; @Log.Number (name = "Target Pose") public static Pose TARGET_POSE = new Pose(0,0); - - public static double UP = 0.1; - public static double DOWN = 0.5; - public static double INCREASE = 15; + public static double HOOD_AUTO_POS = 0.5; // final value we feed into the hood for position + public static double HOOD_TARGET_ANGLE = 21; // hood target angle in degrees + public static double HOOD_POSITION_TO_ANGLE_CONSTANT = 43.42105; // did math to get this it is how the position relates to the angle in degrees + public static double hoodCompScalar = 0.02; // the ratio between our error in velocity ~in the couple of hundreds and the change in hood angle 21-38 degrees; + public static double HOOD_MIN = 0.1; + public static double HOOD_MAX = 0.5; + public static double HOOD_DOWN_ANGLE = 21; // fully down hood angle in deg + public static double INCREASE = 15; // extremely jank increment and decrement + public static double HOOD_TARGET_POS = 0.5; public static double ACTUAL_TARGET = 0; - public static double LAUNCHER_VELOCITY = 2400; + public static double AUTO_VELOCITY = 2000; + public static double HOOD_REGRESSION_A = 0.5051; // slope of hood regression + public static double HOOD_REGRESSION_B = 7.5895; // offset to the slope (y intercept) + public static double LAUNCHER_REGRESSION_A = 33.86667; // slope of launcher regression + public static double LAUNCHER_REGRESSION_B = 916.66667; // offset to the launcher regression slope (y-intercept) + public static double DISTANCE_TO_TARGET = 0; // distance to the hive we are aiming at in inches + public static double error = 0; // error for launcher velo boolean hasHardware; Gamepad gamepad; @@ -71,7 +81,7 @@ public class TurretSubsystem implements Loggable, Subsystem { // It's output is a power value in the -1 to +1 range. // So, P is probably in the range of .001-ish. // For a velocity-targeting PIDF, we probably want an I value, not a D value. - public static PIDFCoefficients launchPID = new PIDFCoefficients(0.015, 0.0, 0.0, 0); + public static PIDFCoefficients launchPID = new PIDFCoefficients(0.015, 0.0, 0.0, 0); // 10/3/26 added a p value seems pretty decent private Hardware hardware; // Stuff used for the Feed Forward function. // This one is highly variable, based on the amount of friction in the system @@ -91,7 +101,6 @@ public static double GetFrictionConstant(boolean inMotion) { // GoBilda says stall current of 9.2A at 12V, so V = I * R, R = 12 / 9.2 (about 1.3 ohms) // As the motor heats up, resistance also increase, so we could increase this a little bit // or maybe increase it over time to counteract that, but this is probably good enough. - public TurretSubsystem(Hardware h, Robot r) { hasHardware = Setup.Connected.TURRETSUBSYSTEM; robot = r; @@ -123,41 +132,41 @@ public TurretSubsystem(Hardware h, Robot r) { public void Launch() { // Spin the motors - setVelocityTarget(LAUNCHER_VELOCITY); + setVelocityTarget(AUTO_VELOCITY); } - - - // takes the already decided upon target and takes the current robot pose and does some math to figure out what angle the turret needs to point to face the target i just guessed which direction is positive should be easy to flip + // returns the turret angle in radians relative to the robot public double getTurretAngle() { Pose targetPose = getTargetPose(); - double x, y, robotHead, dx, dy, absoluteAngle, angle; + double x, y, robotHead, dx, dy, absoluteAngle, angle; // - x = robot.follower.pose().x(); - y = robot.follower.pose().y(); - robotHead = MathUtils.normalizeDeltaAngle(robot.follower.pose().heading(), AngleUnit.RADIANS); // head is in radians cause math is nice + x = robot.follower.pose().x(); // sets x to robots current y position + y = robot.follower.pose().y(); // sets y to robots current y position + robotHead = MathUtils.normalizeDeltaAngle(robot.follower.pose().heading(), AngleUnit.RADIANS); // sets robotHead to robot heading and normalizes to delta radians - dx = targetPose.x() - x; - dy = targetPose.y() - y; + dx = targetPose.x() - x; // difference between target x and robot x + dy = targetPose.y() - y; // difference between target y and robot y absoluteAngle = Math.atan2(dy, dx); // in radians the target angle of the turret relative to the field angle = (absoluteAngle - robotHead); // still in radians....... return angle; } - //determines the distance from the robot to the current hive target in inches to be used for hood angle and flywheel speed + //returns the distance from the selected hive target in inches public double getDistance() { - Pose RobotPose = robot.follower.pose(); - Pose TargetPose = getTargetPose(); + Pose robotPose = robot.follower.pose(); + Pose targetPose = getTargetPose(); - double Distance = RobotPose.distance(TargetPose); + double Distance = robotPose.distance(targetPose); return Distance; } + // increment public double increase() { ACTUAL_TARGET = LAUNCHER_VELOCITY +INCREASE; return ACTUAL_TARGET; } + // decrement public double decrease() { ACTUAL_TARGET = LAUNCHER_VELOCITY - INCREASE; return ACTUAL_TARGET; @@ -178,12 +187,12 @@ public Pose getTargetPose() { return targetPose; } - // takes our turret target angle and turns it into a servo position also clamps it currently to not fry to wiring + // takes our final turret angle does some math and returns a value in servo position aka 0-1 public double getTurretPos() { double angle = getTurretAngle(); - double servoPos = TURRET_CENTER - (angle / (2 * Math.PI)); + double servoPos = TURRET_CENTER + (angle / (2 * Math.PI)); - return (clamp(servoPos, TURRET_MIN, TURRET_MAX)-1 ) *-1; + return (clamp(servoPos, TURRET_MIN, TURRET_MAX)); } public void setTurretTarget() { setTurretPosition(getTurretPos()); @@ -194,7 +203,25 @@ public void up() { public void down() { LAUNCHER_VELOCITY = decrease(); } + public double getHoodTargetAngle() { + double x = DISTANCE_TO_TARGET; + + HOOD_TARGET_ANGLE = HOOD_REGRESSION_A * x + HOOD_REGRESSION_B; + + return HOOD_TARGET_ANGLE; + } + public double getHoodTargetPos() { + HOOD_TARGET_POS = HOOD_MAX - (HOOD_TARGET_ANGLE-HOOD_DOWN_ANGLE) / HOOD_POSITION_TO_ANGLE_CONSTANT; + return clamp(HOOD_TARGET_POS, HOOD_MIN, HOOD_MAX); + } + public double getHoodAutoPos() { + HOOD_AUTO_POS = HOOD_TARGET_POS - (error * hoodCompScalar); + return clamp(HOOD_AUTO_POS, HOOD_MIN, HOOD_MAX); + } + public void setHoodAutoPos() { + setHoodPos(HOOD_AUTO_POS); + } @Override public void periodic() { @@ -205,6 +232,13 @@ public void periodic() { ROBOT_HEAD = robot.follower.pose().heading(); TARGET_VELO = getVelocityTarget(); CURRENT_VELO = getActualVelocity(); + AUTO_VELOCITY = getAutoVelocity(); + DISTANCE_TO_TARGET = getDistance(); + getHoodAutoPos(); + getHoodTargetPos(); + getHoodTargetAngle(); + error = pidfController.getLastError(); + // Add an item to the array and update the index for the next update to the 'circular' array @@ -223,6 +257,14 @@ public double getActualVelocity() { return pidfController.getTarget(); // Not a Number } } + // takes distance and returns AUTO_VELOCITY by plugging it into the regression + public double getAutoVelocity() { + double x = DISTANCE_TO_TARGET; + + AUTO_VELOCITY = LAUNCHER_REGRESSION_A * x + LAUNCHER_REGRESSION_B; + + return AUTO_VELOCITY; + } // Explicitly set the target velocity for the motors public void setVelocityTarget(double speed) { @@ -232,6 +274,9 @@ public void setTurretPosition(double pos) { turret1.setPosition(pos); turret2.setPosition(pos); } + private void setHoodPos(double pos) { + hood.setPosition(pos); + } // Returns the current target velocity (which may be set explicitly, or automatically) public double getVelocityTarget() { From a97531982155ddc9390a82c8259781cc28c5f500 Mon Sep 17 00:00:00 2001 From: stedman Date: Mon, 5 Oct 2026 17:04:46 -0700 Subject: [PATCH 3/5] ok shouldve fixed all of the stuff aswell as added a small bit to the 1 auto we have + a small intake addition! --- .../teamcode/sixteen750/PedroConstants.java | 4 +- .../sixteen750/commands/TeleCommands.java | 10 +- .../sixteen750/controls/DriverController.java | 6 +- .../teamcode/sixteen750/opmodes/MainTele.java | 2 +- .../opmodes/auto/PartnerPark12Park.java | 21 ++-- .../subsystems/IntakeSubsystem.java | 18 ++- .../subsystems/TurretSubsystem.java | 103 +++++++++--------- 7 files changed, 92 insertions(+), 72 deletions(-) diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/PedroConstants.java b/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/PedroConstants.java index f4eb137..800613e 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/PedroConstants.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/PedroConstants.java @@ -42,8 +42,8 @@ public static Follower create(HardwareMap h) { c.xPodPort.set(1); c.yPodPort.set(0); c.ticksPerUnit.set(505.316944406); - c.xPodOffset.set(2.92913); - c.yPodOffset.set(2.77717); + c.xPodOffset.set(2.92913);// from cad but i trust it more + c.yPodOffset.set(2.77717);// from cad but i trust it more c.xPodDirection.set(OctoQuad.EncoderDirection.FORWARD); c.yPodDirection.set(OctoQuad.EncoderDirection.REVERSE); c.globalDistanceUnit.set(DistanceUnit.INCH); diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/commands/TeleCommands.java b/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/commands/TeleCommands.java index 9b4c9e5..9d293f5 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/commands/TeleCommands.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/commands/TeleCommands.java @@ -46,14 +46,14 @@ public static Command GateClose(Robot r) { return Command.create(r.intakeSubsystem::GateClose); } - public static Command Track(Robot r) { + public static Command TrackHive(Robot r) { return Command.create(r.turretsubsystem::setTurretTarget); } - public static Command Increase (Robot r) { - return Command.create(r.turretsubsystem::up); + public static Command IncreaseFlywheel (Robot r) { + return Command.create(r.turretsubsystem::increaseVelocity); } - public static Command Decrease (Robot r) { - return Command.create(r.turretsubsystem::down); + public static Command DecreaseFlywheel (Robot r) { + return Command.create(r.turretsubsystem::decreaseVelocity); } public static Command AutoHood (Robot r) {return Command.create(r.turretsubsystem::setHoodAutoPos);} diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/controls/DriverController.java b/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/controls/DriverController.java index 512a204..fe8b6bb 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/controls/DriverController.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/controls/DriverController.java @@ -94,11 +94,11 @@ public void bindDriveControls() { } public void bindLaunchControls() { - trackButton.whileInverseToggled(TeleCommands.Track(robot)); + trackButton.whileInverseToggled(TeleCommands.TrackHive(robot)); flywheelButton.whileInverseToggled(TeleCommands.Launch(robot)); hoodButton.whileInverseToggled(TeleCommands.AutoHood(robot)); - upButton.whenPressed(TeleCommands.Increase(robot)); - downButton.whenPressed(TeleCommands.Decrease(robot)); + upButton.whenPressed(TeleCommands.IncreaseFlywheel(robot)); + downButton.whenPressed(TeleCommands.DecreaseFlywheel(robot)); } public void bindIntakeControls() { diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/opmodes/MainTele.java b/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/opmodes/MainTele.java index 3a4df59..f6fef74 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/opmodes/MainTele.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/opmodes/MainTele.java @@ -41,7 +41,7 @@ public void uponInit() { robot = new Robot(hardware, Alliance.RED, StartingPosition.Unspecified); // controlsOperator = new OperatorController(codriverGamepad, robot); panelsTelemetry = PanelsTelemetry.INSTANCE; - robot.follower.setPose(new Pose(6.75, 6.75, 0)); + robot.follower.setPose(new Pose(6.75, 6.75, 0)); // temporary starting pos for now // limelight = hardwareMap.get(Limelight3A.class, Setup.HardwareNames.LIMELIGHT); controlsDriver = new DriverController(driverGamepad, robot); if (Setup.Connected.DRIVEBASE) { diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/opmodes/auto/PartnerPark12Park.java b/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/opmodes/auto/PartnerPark12Park.java index 8fa2a2d..c008029 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/opmodes/auto/PartnerPark12Park.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/opmodes/auto/PartnerPark12Park.java @@ -5,6 +5,7 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.technototes.library.command.Command; import com.technototes.library.command.CommandScheduler; +import com.technototes.library.command.ParallelCommandGroup; import com.technototes.library.command.SequentialCommandGroup; import com.technototes.library.command.WaitCommand; import com.technototes.library.structure.CommandOpMode; @@ -20,7 +21,7 @@ import org.firstinspires.ftc.teamcode.sixteen750.pedro.meepmeep.Paths; import org.firstinspires.ftc.teamcode.sixteen750.pedro.meepmeep.Poses; -@Autonomous(name = "PartnerPark12Park", preselectTeleOp = "BlueTele") +@Autonomous(name = "PartnerPark12Park", preselectTeleOp = "MainTele") @SuppressWarnings("unused") public class PartnerPark12Park extends CommandOpMode { @@ -41,22 +42,28 @@ public void uponInit() { panelsTelemetry = PanelsTelemetry.INSTANCE; robot.follower.setPose(Poses.StartPoses.getStart()); CommandScheduler.scheduleForState( + new ParallelCommandGroup( + t.Launch(robot), + t.TrackHive(robot), + t.AutoHood(robot) + ).alongWith( new SequentialCommandGroup( - //t.Launch(robot) .alongWith( new PedroPathCommand(robot.follower, p.StartToPartnerPark()), //) new PedroPathCommand(robot.follower, p.PartnerParkToLaunch1()), - //t.Feed(robot), + t.Feed(robot), new WaitCommand(0.6), - new PedroPathCommand(robot.follower, p.Launch1ToGardenPreInt()), - //.alongWith( t.Intake(robot) ) + new PedroPathCommand(robot.follower, p.Launch1ToGardenPreInt()) + .alongWith( + t.Intake(robot)), new PedroPathCommand(robot.follower, p.GardenPreIntToGardenInt()), new WaitCommand(1), new PedroPathCommand(robot.follower, p.GardenIntToLaunch2()), - //t.Feed(robot), + t.Feed(robot), new WaitCommand(0.6), new PedroPathCommand(robot.follower, p.Launch2ToPark()), CommandScheduler::terminateOpMode - ), + ) + ), OpModeState.RUN ); } diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/subsystems/IntakeSubsystem.java b/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/subsystems/IntakeSubsystem.java index 21b1748..0007bbd 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/subsystems/IntakeSubsystem.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/subsystems/IntakeSubsystem.java @@ -18,7 +18,8 @@ public class IntakeSubsystem implements Loggable, Subsystem { public static double REJECT_VELOCITY = -0.4; - public static double HOLD_VELOCITY = 0.4; + public static double HOLD_VELOCITY1 = 0.45; + public static double HOLD_VELOCITY2 = 0.55; public static double FEED_VELOCITY = 1; @@ -72,7 +73,8 @@ public void Spit() { } public void Hold() { - setTransferPower(HOLD_VELOCITY); + setTransfer1Power(HOLD_VELOCITY2); + setTransfer2Power(HOLD_VELOCITY1); setGatePosition(BLOCK_POSITION); } @@ -107,6 +109,18 @@ private void setTransferPower(double power) { } } + private void setTransfer1Power(double power) { + if (hasHardware) { + _transfer1.setPower(power); + } + } + + private void setTransfer2Power(double power) { + if (hasHardware) { + _transfer2.setPower(power); + } + } + private void setGatePosition(double pos) { if (hasHardware) { _gate.setPosition(pos); diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/subsystems/TurretSubsystem.java b/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/subsystems/TurretSubsystem.java index 73d4ff6..4d18c1d 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/subsystems/TurretSubsystem.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/subsystems/TurretSubsystem.java @@ -36,36 +36,36 @@ public class TurretSubsystem implements Loggable, Subsystem { public static double ROTATE_LEFT = 0.15; public static double ROTATE_RIGHT = 0.85; // @Log.Number (name = "target servo Pos") - public static double TARGET_SERVO_POS = 0; + public static double targetServoPos = 0; @Log.Number (name = "Robot Heading") - public static double ROBOT_HEAD = 0; + public static double robotHead = 0; // @Log.Number (name = "Absolute heading") public static double ABSOLUTE_HEAD = 0; @Log.Number (name = "Turret Heading") - public static double TURRET_HEADING = 0; + public static double turretHead = 0; @Log.Number (name = "Target Velocity") - public static double TARGET_VELO = 0; + public static double targetVelo = 0; @Log.Number (name = "Current Velocity") - public static double CURRENT_VELO = 0; + public static double currentVelo = 0; @Log.Number (name = "Target Pose") - public static Pose TARGET_POSE = new Pose(0,0); - public static double HOOD_AUTO_POS = 0.5; // final value we feed into the hood for position - public static double HOOD_TARGET_ANGLE = 21; // hood target angle in degrees + public static Pose targetPose = new Pose(0,0); + public static double hoodAutoPos = 0.5; // final value we feed into the hood for position + public static double hoodTargetAngle = 21; // hood target angle in degrees public static double HOOD_POSITION_TO_ANGLE_CONSTANT = 43.42105; // did math to get this it is how the position relates to the angle in degrees public static double hoodCompScalar = 0.02; // the ratio between our error in velocity ~in the couple of hundreds and the change in hood angle 21-38 degrees; public static double HOOD_MIN = 0.1; public static double HOOD_MAX = 0.5; public static double HOOD_DOWN_ANGLE = 21; // fully down hood angle in deg public static double INCREASE = 15; // extremely jank increment and decrement - public static double HOOD_TARGET_POS = 0.5; - public static double ACTUAL_TARGET = 0; - public static double LAUNCHER_VELOCITY = 2400; - public static double AUTO_VELOCITY = 2000; + public static double hoodTargetPos = 0.5; + public static double actualTarget = 0; // this is very sus code yay! + public static double launcherVelocity = 2400; + public static double autoVelocity = 2000; public static double HOOD_REGRESSION_A = 0.5051; // slope of hood regression public static double HOOD_REGRESSION_B = 7.5895; // offset to the slope (y intercept) public static double LAUNCHER_REGRESSION_A = 33.86667; // slope of launcher regression public static double LAUNCHER_REGRESSION_B = 916.66667; // offset to the launcher regression slope (y-intercept) - public static double DISTANCE_TO_TARGET = 0; // distance to the hive we are aiming at in inches + public static double distanceToTarget = 0; // distance to the hive we are aiming at in inches public static double error = 0; // error for launcher velo boolean hasHardware; @@ -86,9 +86,9 @@ public class TurretSubsystem implements Loggable, Subsystem { // Stuff used for the Feed Forward function. // This one is highly variable, based on the amount of friction in the system - public static double kStaticFriction = 0.183; + public static double kStaticFriction = 0.364; // measured 10/3 - public static double kDynamicFriction = 0.168; + public static double kDynamicFriction = 0.360; // measured 10/3 // This one tends to be somewhere between 0.0035 to 0.005 or so. public static double kVelocityConstant = 0.0043; @@ -132,7 +132,7 @@ public TurretSubsystem(Hardware h, Robot r) { public void Launch() { // Spin the motors - setVelocityTarget(AUTO_VELOCITY); + setVelocityTarget(autoVelocity); } // returns the turret angle in radians relative to the robot @@ -161,15 +161,15 @@ public double getDistance() { return Distance; } - // increment - public double increase() { - ACTUAL_TARGET = LAUNCHER_VELOCITY +INCREASE; - return ACTUAL_TARGET; + // increment ignore how sus this implementation is btw + public double increaseVelo() { + actualTarget = launcherVelocity +INCREASE; + return actualTarget; } // decrement - public double decrease() { - ACTUAL_TARGET = LAUNCHER_VELOCITY - INCREASE; - return ACTUAL_TARGET; + public double decreaseVelo() { + actualTarget = launcherVelocity - INCREASE; + return actualTarget; } // determines which hive target to aim for based on what half of the field we are public Pose getTargetPose() { @@ -197,60 +197,59 @@ public double getTurretPos() { public void setTurretTarget() { setTurretPosition(getTurretPos()); } - public void up() { - LAUNCHER_VELOCITY = increase(); + public void increaseVelocity() { + launcherVelocity = increaseVelo(); } - public void down() { - LAUNCHER_VELOCITY = decrease(); + public void decreaseVelocity() { + launcherVelocity = decreaseVelo(); } + // returns the target angle we want our hood to be at in degrees before we compensate for velocity public double getHoodTargetAngle() { - double x = DISTANCE_TO_TARGET; + double x = distanceToTarget; // distance in inches - HOOD_TARGET_ANGLE = HOOD_REGRESSION_A * x + HOOD_REGRESSION_B; + hoodTargetAngle = HOOD_REGRESSION_A * x + HOOD_REGRESSION_B; // we run our distance into our regression formula - return HOOD_TARGET_ANGLE; + return hoodTargetAngle; } + //returns the target servo position for our hood before we compensate for velocity public double getHoodTargetPos() { - HOOD_TARGET_POS = HOOD_MAX - (HOOD_TARGET_ANGLE-HOOD_DOWN_ANGLE) / HOOD_POSITION_TO_ANGLE_CONSTANT; + hoodTargetPos = HOOD_MAX - (hoodTargetAngle-HOOD_DOWN_ANGLE) / HOOD_POSITION_TO_ANGLE_CONSTANT; // basically shifting and scaling it to work - return clamp(HOOD_TARGET_POS, HOOD_MIN, HOOD_MAX); + return clamp(hoodTargetPos, HOOD_MIN, HOOD_MAX); // clamping it so the servo doesnt rebel from the rest of the robot } + // returns the final position in servo position (0-1) that we want our hood servo to be at after velocity compensation public double getHoodAutoPos() { - HOOD_AUTO_POS = HOOD_TARGET_POS - (error * hoodCompScalar); - return clamp(HOOD_AUTO_POS, HOOD_MIN, HOOD_MAX); + hoodAutoPos = hoodTargetPos - (error * hoodCompScalar); // takes our target and subtracts our error times a scalar + return clamp(hoodAutoPos, HOOD_MIN, HOOD_MAX); // clamp it again so it doesnt try and unionize (i think you only need to clamp it once but by doing it twice both the compensated and uncompensated values are actually usable) } public void setHoodAutoPos() { - setHoodPos(HOOD_AUTO_POS); + setHoodPos(hoodAutoPos); } @Override public void periodic() { - getTurretPos(); - TARGET_SERVO_POS = getTurretPos(); - TARGET_POSE = getTargetPose(); - TURRET_HEADING = getTurretAngle(); - ROBOT_HEAD = robot.follower.pose().heading(); - TARGET_VELO = getVelocityTarget(); - CURRENT_VELO = getActualVelocity(); - AUTO_VELOCITY = getAutoVelocity(); - DISTANCE_TO_TARGET = getDistance(); + targetServoPos = getTurretPos(); + targetPose = getTargetPose(); + turretHead = getTurretAngle(); + robotHead = robot.follower.pose().heading(); + targetVelo = getVelocityTarget(); + currentVelo = getActualVelocity(); + autoVelocity = getAutoVelocity(); + distanceToTarget = getDistance(); getHoodAutoPos(); getHoodTargetPos(); getHoodTargetAngle(); error = pidfController.getLastError(); - - - // Add an item to the array and update the index for the next update to the 'circular' array double power = pidfController.update(getActualVelocity()); setLauncherPower(power); } - public double getMotor1Current() { + private double getMotor1Current() { return hasHardware ? launcher.getAmperage(CurrentUnit.AMPS) : 0; } - public double getActualVelocity() { + private double getActualVelocity() { if (hasHardware) { return launcher.getVelocity(); } else { @@ -259,11 +258,11 @@ public double getActualVelocity() { } // takes distance and returns AUTO_VELOCITY by plugging it into the regression public double getAutoVelocity() { - double x = DISTANCE_TO_TARGET; + double x = distanceToTarget; - AUTO_VELOCITY = LAUNCHER_REGRESSION_A * x + LAUNCHER_REGRESSION_B; + autoVelocity = LAUNCHER_REGRESSION_A * x + LAUNCHER_REGRESSION_B; - return AUTO_VELOCITY; + return autoVelocity; } // Explicitly set the target velocity for the motors From b1d40869322a4ccb2e2486d076754811ae91e021 Mon Sep 17 00:00:00 2001 From: Kevin Frei Date: Mon, 5 Oct 2026 10:01:58 -0700 Subject: [PATCH 4/5] Auto-formatted code --- .../sixteen750/commands/TeleCommands.java | 11 ++++--- .../sixteen750/controls/DriverController.java | 1 - .../subsystems/TurretSubsystem.java | 30 +++++++++++++------ 3 files changed, 28 insertions(+), 14 deletions(-) diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/commands/TeleCommands.java b/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/commands/TeleCommands.java index 9d293f5..8235c4c 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/commands/TeleCommands.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/commands/TeleCommands.java @@ -23,7 +23,7 @@ public static Command Feed(Robot r) { } public static Command IntakeStop(Robot r) { - return Command.create(r.intakeSubsystem::StopIntake); + return Command.create(r.intakeSubsystem::StopIntake); } public static Command Reject(Robot r) { @@ -55,9 +55,12 @@ public static Command IncreaseFlywheel (Robot r) { public static Command DecreaseFlywheel (Robot r) { return Command.create(r.turretsubsystem::decreaseVelocity); } - public static Command AutoHood (Robot r) {return Command.create(r.turretsubsystem::setHoodAutoPos);} + public static Command AutoHood(Robot r) { + return Command.create(r.turretsubsystem::setHoodAutoPos); + } - public static Command Launch (Robot r) {return Command.create(r.turretsubsystem::Launch);} + public static Command Launch(Robot r) { + return Command.create(r.turretsubsystem::Launch); + } } - diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/controls/DriverController.java b/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/controls/DriverController.java index fe8b6bb..272eb11 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/controls/DriverController.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/controls/DriverController.java @@ -90,7 +90,6 @@ public void bindDriveControls() { ); resetGyroButton.whenPressed(DrivingCommands.ResetGyro(pedroDriver)); - } public void bindLaunchControls() { diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/subsystems/TurretSubsystem.java b/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/subsystems/TurretSubsystem.java index 4d18c1d..ca0e323 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/subsystems/TurretSubsystem.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/subsystems/TurretSubsystem.java @@ -16,7 +16,6 @@ import com.technototes.library.subsystem.Subsystem; import com.technototes.library.util.MathUtils; import com.technototes.library.util.PIDFController; - import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit; import org.firstinspires.ftc.teamcode.sixteen750.Hardware; @@ -25,9 +24,10 @@ @Configurable public class TurretSubsystem implements Loggable, Subsystem { + final Robot robot; - Pose HIVE_TARGET1 = new Pose(58,53); - Pose HIVE_TARGET2 = new Pose(58,88 ); + Pose HIVE_TARGET1 = new Pose(58, 53); + Pose HIVE_TARGET2 = new Pose(58, 88); public static double TARGET_SWITCH_THRESHOLD = 72; public static double TURRET_CENTER = 0.5; @@ -143,15 +143,19 @@ public double getTurretAngle() { x = robot.follower.pose().x(); // sets x to robots current y position y = robot.follower.pose().y(); // sets y to robots current y position - robotHead = MathUtils.normalizeDeltaAngle(robot.follower.pose().heading(), AngleUnit.RADIANS); // sets robotHead to robot heading and normalizes to delta radians + robotHead = MathUtils.normalizeDeltaAngle( + robot.follower.pose().heading(), + AngleUnit.RADIANS + ); // sets robotHead to robot heading and normalizes to delta radians dx = targetPose.x() - x; // difference between target x and robot x dy = targetPose.y() - y; // difference between target y and robot y absoluteAngle = Math.atan2(dy, dx); // in radians the target angle of the turret relative to the field - angle = (absoluteAngle - robotHead); // still in radians....... + angle = absoluteAngle - robotHead; // still in radians....... return angle; } + //returns the distance from the selected hive target in inches public double getDistance() { Pose robotPose = robot.follower.pose(); @@ -166,16 +170,17 @@ public double increaseVelo() { actualTarget = launcherVelocity +INCREASE; return actualTarget; } + // decrement public double decreaseVelo() { actualTarget = launcherVelocity - INCREASE; return actualTarget; } + // determines which hive target to aim for based on what half of the field we are public Pose getTargetPose() { - double Y; - Pose targetPose; + Pose targetPose; Y = robot.follower.pose().y(); @@ -187,13 +192,15 @@ public Pose getTargetPose() { return targetPose; } + // takes our final turret angle does some math and returns a value in servo position aka 0-1 public double getTurretPos() { double angle = getTurretAngle(); - double servoPos = TURRET_CENTER + (angle / (2 * Math.PI)); + double servoPos = TURRET_CENTER + angle / (2 * Math.PI); - return (clamp(servoPos, TURRET_MIN, TURRET_MAX)); + return clamp(servoPos, TURRET_MIN, TURRET_MAX); } + public void setTurretTarget() { setTurretPosition(getTurretPos()); } @@ -222,6 +229,7 @@ public double getHoodAutoPos() { hoodAutoPos = hoodTargetPos - (error * hoodCompScalar); // takes our target and subtracts our error times a scalar return clamp(hoodAutoPos, HOOD_MIN, HOOD_MAX); // clamp it again so it doesnt try and unionize (i think you only need to clamp it once but by doing it twice both the compensated and uncompensated values are actually usable) } + public void setHoodAutoPos() { setHoodPos(hoodAutoPos); } @@ -241,6 +249,7 @@ public void periodic() { getHoodTargetAngle(); error = pidfController.getLastError(); + // Add an item to the array and update the index for the next update to the 'circular' array double power = pidfController.update(getActualVelocity()); setLauncherPower(power); } @@ -256,6 +265,7 @@ private double getActualVelocity() { return pidfController.getTarget(); // Not a Number } } + // takes distance and returns AUTO_VELOCITY by plugging it into the regression public double getAutoVelocity() { double x = distanceToTarget; @@ -269,10 +279,12 @@ public double getAutoVelocity() { public void setVelocityTarget(double speed) { pidfController.setTarget(speed); } + public void setTurretPosition(double pos) { turret1.setPosition(pos); turret2.setPosition(pos); } + private void setHoodPos(double pos) { hood.setPosition(pos); } From d8d62f7ae065d723d957e6751a71a2515d09f869 Mon Sep 17 00:00:00 2001 From: evandro <248197846+evandrospenceralves-star@users.noreply.github.com> Date: Tue, 6 Oct 2026 17:53:11 -0700 Subject: [PATCH 5/5] Evandro and Kyan , we added a reset position command, kevin said its good --- .../teamcode/sixteen750/PedroConstants.java | 6 ++-- .../sixteen750/commands/PedroDriver.java | 4 +++ .../commands/driving/DrivingCommands.java | 4 +++ .../sixteen750/controls/DriverController.java | 7 +++-- .../subsystems/TurretSubsystem.java | 31 ++++++++++++------- 5 files changed, 34 insertions(+), 18 deletions(-) diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/PedroConstants.java b/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/PedroConstants.java index 800613e..3943c03 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/PedroConstants.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/PedroConstants.java @@ -42,14 +42,14 @@ public static Follower create(HardwareMap h) { c.xPodPort.set(1); c.yPodPort.set(0); c.ticksPerUnit.set(505.316944406); - c.xPodOffset.set(2.92913);// from cad but i trust it more - c.yPodOffset.set(2.77717);// from cad but i trust it more + c.xPodOffset.set(2.598); // from cad but i trust it more + c.yPodOffset.set(2.264); // from cad but i trust it more c.xPodDirection.set(OctoQuad.EncoderDirection.FORWARD); c.yPodDirection.set(OctoQuad.EncoderDirection.REVERSE); c.globalDistanceUnit.set(DistanceUnit.INCH); c.offsetUnits.set(DistanceUnit.INCH); c.i2cRecoveryMode.set(OctoQuad.I2cRecoveryMode.MODE_1_PERIPH_RST_ON_FRAME_ERR); - c.headingScalar.set(1.0348099609138208); + c.headingScalar.set(1.01828); }); public static ForesightConfig foresightConfig = new ForesightConfig(c -> { Controller primaryTranslationalForward = Controller.proportional(0.28921569321161206); diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/commands/PedroDriver.java b/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/commands/PedroDriver.java index a64a809..e44d5d6 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/commands/PedroDriver.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/commands/PedroDriver.java @@ -51,6 +51,10 @@ public void ResetGyro() { headingOffset = follower.pose().heading(); } + public void ResetPosition() { + follower.setPose(new Pose(6.75, 6.75, 0)); + } + public void SetSnailSpeed() { // follower.setMaxPowerScaling(OtherSettings.SNAIL_SPEED); turnSpeed = OtherSettings.SNAIL_TURN; diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/commands/driving/DrivingCommands.java b/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/commands/driving/DrivingCommands.java index 27c679a..1cd74cc 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/commands/driving/DrivingCommands.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/commands/driving/DrivingCommands.java @@ -13,6 +13,10 @@ public static Command SnailDriving(PedroDriver pd) { return pd::SetSnailSpeed; } + public static Command ResetPosition(PedroDriver pd) { + return pd::ResetPosition; + } + public static Command TurboDriving(PedroDriver pd) { return pd::SetTurboSpeed; } diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/controls/DriverController.java b/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/controls/DriverController.java index 272eb11..c45fbc0 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/controls/DriverController.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/controls/DriverController.java @@ -1,7 +1,7 @@ package org.firstinspires.ftc.teamcode.sixteen750.controls; +import com.technototes.library.command.Command; import com.technototes.library.command.CommandScheduler; -import com.technototes.library.control.CommandAxis; import com.technototes.library.control.CommandButton; import com.technototes.library.control.CommandGamepad; import com.technototes.library.control.Stick; @@ -26,7 +26,7 @@ public class DriverController implements Loggable { public CommandButton spitButton; public CommandButton gateButton; public CommandButton override; - public CommandButton RelocButton; + public CommandButton relocButton; public CommandButton holdButton; public CommandButton trackButton; public CommandButton flywheelButton; @@ -70,7 +70,7 @@ public void AssignNamedControllerButton() { spitButton = gamepad.ps_square; gateButton = gamepad.ps_cross; holdButton = gamepad.ps_circle; - RelocButton = gamepad.ps_share; + relocButton = gamepad.ps_share; trackButton = gamepad.ps_triangle; hoodButton = gamepad.dpadRight; } @@ -90,6 +90,7 @@ public void bindDriveControls() { ); resetGyroButton.whenPressed(DrivingCommands.ResetGyro(pedroDriver)); + relocButton.whenPressed(DrivingCommands.ResetPosition(pedroDriver)); } public void bindLaunchControls() { diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/subsystems/TurretSubsystem.java b/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/subsystems/TurretSubsystem.java index ca0e323..78edf8a 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/subsystems/TurretSubsystem.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/subsystems/TurretSubsystem.java @@ -35,7 +35,7 @@ public class TurretSubsystem implements Loggable, Subsystem { public static double TURRET_MAX = 0.92; public static double ROTATE_LEFT = 0.15; public static double ROTATE_RIGHT = 0.85; - // @Log.Number (name = "target servo Pos") + // @Log.Number (name = "target servo Pos") public static double targetServoPos = 0; @Log.Number (name = "Robot Heading") public static double robotHead = 0; @@ -50,9 +50,10 @@ public class TurretSubsystem implements Loggable, Subsystem { @Log.Number (name = "Target Pose") public static Pose targetPose = new Pose(0,0); public static double hoodAutoPos = 0.5; // final value we feed into the hood for position + public static double hoodTargetAngle = 21; // hood target angle in degrees public static double HOOD_POSITION_TO_ANGLE_CONSTANT = 43.42105; // did math to get this it is how the position relates to the angle in degrees - public static double hoodCompScalar = 0.02; // the ratio between our error in velocity ~in the couple of hundreds and the change in hood angle 21-38 degrees; + public static double hoodCompScalar = 0.025; // the ratio between our error in velocity ~in the couple of hundreds and the change in hood angle 21-38 degrees; public static double HOOD_MIN = 0.1; public static double HOOD_MAX = 0.5; public static double HOOD_DOWN_ANGLE = 21; // fully down hood angle in deg @@ -61,11 +62,14 @@ public class TurretSubsystem implements Loggable, Subsystem { public static double actualTarget = 0; // this is very sus code yay! public static double launcherVelocity = 2400; public static double autoVelocity = 2000; - public static double HOOD_REGRESSION_A = 0.5051; // slope of hood regression - public static double HOOD_REGRESSION_B = 7.5895; // offset to the slope (y intercept) - public static double LAUNCHER_REGRESSION_A = 33.86667; // slope of launcher regression + public static double HOOD_REGRESSION_A = 0.4651; // slope of hood regression + public static double HOOD_REGRESSION_B = 6.5895; // offset to the slope (y intercept) + public static double LAUNCHER_REGRESSION_A = 32.86667; // slope of launcher regression public static double LAUNCHER_REGRESSION_B = 916.66667; // offset to the launcher regression slope (y-intercept) + + @Log.Number(name = "distance") public static double distanceToTarget = 0; // distance to the hive we are aiming at in inches + public static double error = 0; // error for launcher velo boolean hasHardware; @@ -167,7 +171,7 @@ public double getDistance() { } // increment ignore how sus this implementation is btw public double increaseVelo() { - actualTarget = launcherVelocity +INCREASE; + actualTarget = launcherVelocity + INCREASE; return actualTarget; } @@ -184,7 +188,7 @@ public Pose getTargetPose() { Y = robot.follower.pose().y(); - if (Y > TARGET_SWITCH_THRESHOLD) { + if (Y < TARGET_SWITCH_THRESHOLD) { targetPose = HIVE_TARGET1; } else { targetPose = HIVE_TARGET2; @@ -204,9 +208,11 @@ public double getTurretPos() { public void setTurretTarget() { setTurretPosition(getTurretPos()); } + public void increaseVelocity() { launcherVelocity = increaseVelo(); } + public void decreaseVelocity() { launcherVelocity = decreaseVelo(); } @@ -226,8 +232,8 @@ public double getHoodTargetPos() { } // returns the final position in servo position (0-1) that we want our hood servo to be at after velocity compensation public double getHoodAutoPos() { - hoodAutoPos = hoodTargetPos - (error * hoodCompScalar); // takes our target and subtracts our error times a scalar - return clamp(hoodAutoPos, HOOD_MIN, HOOD_MAX); // clamp it again so it doesnt try and unionize (i think you only need to clamp it once but by doing it twice both the compensated and uncompensated values are actually usable) + hoodAutoPos = hoodTargetPos - (error * hoodCompScalar) / HOOD_POSITION_TO_ANGLE_CONSTANT; // takes our target and subtracts our error times a scalar + return clamp(hoodAutoPos, HOOD_MIN, HOOD_MAX); // clamp it again so it doesnt try and unionize (i think you only need to clamp it once but by doing it twice both the compensated and uncompensated values are actually usable) } public void setHoodAutoPos() { @@ -244,6 +250,7 @@ public void periodic() { currentVelo = getActualVelocity(); autoVelocity = getAutoVelocity(); distanceToTarget = getDistance(); + hoodAutoPos = getHoodAutoPos(); getHoodAutoPos(); getHoodTargetPos(); getHoodTargetAngle(); @@ -268,11 +275,11 @@ private double getActualVelocity() { // takes distance and returns AUTO_VELOCITY by plugging it into the regression public double getAutoVelocity() { - double x = distanceToTarget; + double x = distanceToTarget; - autoVelocity = LAUNCHER_REGRESSION_A * x + LAUNCHER_REGRESSION_B; + autoVelocity = LAUNCHER_REGRESSION_A * x + LAUNCHER_REGRESSION_B; - return autoVelocity; + return autoVelocity; } // Explicitly set the target velocity for the motors