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..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); - c.yPodOffset.set(2.77717); + 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/TeleCommands.java b/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/commands/TeleCommands.java index ecc1329..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) { @@ -46,9 +46,21 @@ 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 IncreaseFlywheel (Robot r) { + return Command.create(r.turretsubsystem::increaseVelocity); + } + public static Command DecreaseFlywheel (Robot r) { + return Command.create(r.turretsubsystem::decreaseVelocity); + } - public static Command Launch (Robot r) {return Command.create(r.turretsubsystem::Launch);} + 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/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 4615569..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,14 +26,15 @@ 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 turretlockButton; public CommandButton trackButton; - + public CommandButton flywheelButton; public CommandButton intakeTrigger; - public CommandAxis autoAim; + public CommandButton upButton; + public CommandButton downButton; public PedroDriver pedroDriver; + public CommandButton hoodButton; public static double triggerThreshold = 0.1; @@ -53,10 +54,6 @@ public DriverController(CommandGamepad g, Robot r) { if (Setup.Connected.INTAKESUBSYSTEM) { bindIntakeControls(); } - - if (Setup.Connected.AIMINGSUBSYSTEM) { - bindAimControls(); - } } public void AssignNamedControllerButton() { @@ -65,14 +62,17 @@ public void AssignNamedControllerButton() { driveRightStick = gamepad.rightStick; intakeTrigger = gamepad.rightTrigger.getAsButton(); - snailButton = gamepad.ps_triangle; + upButton = gamepad.dpadUp; + downButton = gamepad.dpadDown; + 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.dpadUp; - trackButton = gamepad.leftBumper; + relocButton = gamepad.ps_share; + trackButton = gamepad.ps_triangle; + hoodButton = gamepad.dpadRight; } public void bindDriveControls() { @@ -84,29 +84,21 @@ 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)); + relocButton.whenPressed(DrivingCommands.ResetPosition(pedroDriver)); } public void bindLaunchControls() { - trackButton.whilePressed(TeleCommands.Track(robot)); + trackButton.whileInverseToggled(TeleCommands.TrackHive(robot)); + flywheelButton.whileInverseToggled(TeleCommands.Launch(robot)); + hoodButton.whileInverseToggled(TeleCommands.AutoHood(robot)); + upButton.whenPressed(TeleCommands.IncreaseFlywheel(robot)); + downButton.whenPressed(TeleCommands.DecreaseFlywheel(robot)); } public void bindIntakeControls() { @@ -120,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/opmodes/MainTele.java b/Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/opmodes/MainTele.java index 51b236d..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 @@ -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() { @@ -38,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) { @@ -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/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 247edf7..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,11 +18,12 @@ 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 = 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; @@ -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 36f24dc..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 @@ -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,114 +15,180 @@ 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; @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 = 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") - public static double TARGET_SERVO_POS = 0; + // @Log.Number (name = "target servo Pos") + public static double targetServoPos = 0; @Log.Number (name = "Robot Heading") - public static double ROBOT_HEAD = 0; - @Log.Number (name = "Absolute heading") + 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 targetVelo = 0; + @Log.Number (name = "Current Velocity") + public static double currentVelo = 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 LAUNCHER_VELOCITY = -0.5; + 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.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 + public static double INCREASE = 15; // extremely jank increment and decrement + 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.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; + 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); // 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 + + public static double kStaticFriction = 0.364; // measured 10/3 + + 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; + 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(autoVelocity); } - 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 + // 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....... + 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 ignore how sus this implementation is btw + 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(); - if (Y > TARGET_SWITCH_THRESHOLD) { + if (Y < TARGET_SWITCH_THRESHOLD) { targetPose = HIVE_TARGET1; } else { targetPose = HIVE_TARGET2; @@ -129,47 +196,114 @@ 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()); } + public void increaseVelocity() { + launcherVelocity = increaseVelo(); + } + + 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 = distanceToTarget; // distance in inches + + hoodTargetAngle = HOOD_REGRESSION_A * x + HOOD_REGRESSION_B; // we run our distance into our regression formula + + return hoodTargetAngle; + } + //returns the target servo position for our hood before we compensate for velocity + public double getHoodTargetPos() { + hoodTargetPos = HOOD_MAX - (hoodTargetAngle-HOOD_DOWN_ANGLE) / HOOD_POSITION_TO_ANGLE_CONSTANT; // basically shifting and scaling it to work + + 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() { + 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() { + setHoodPos(hoodAutoPos); + } @Override public void periodic() { - getTurretPos(); - TARGET_SERVO_POS = getTurretPos(); - TARGET_POSE = getTargetPose(); - TURRET_HEADING = getTurretAngle(); - ROBOT_HEAD = robot.follower.pose().heading(); - + targetServoPos = getTurretPos(); + targetPose = getTargetPose(); + turretHead = getTurretAngle(); + robotHead = robot.follower.pose().heading(); + targetVelo = getVelocityTarget(); + currentVelo = getActualVelocity(); + autoVelocity = getAutoVelocity(); + distanceToTarget = getDistance(); + hoodAutoPos = getHoodAutoPos(); + 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); } - private void setLauncherVelocity(double velo) { - if (hasHardware) { - launcher.setPower(0.8); - // launcher.setVelocity(velo); - } + private double getMotor1Current() { + return hasHardware ? launcher.getAmperage(CurrentUnit.AMPS) : 0; } - private void setHoodPosition(double pos) { + private double getActualVelocity() { if (hasHardware) { - hood.setPosition(pos); + return launcher.getVelocity(); + } else { + return pidfController.getTarget(); // Not a Number } } - private void setTurretPosition(double pos) { + // takes distance and returns AUTO_VELOCITY by plugging it into the regression + public double getAutoVelocity() { + double x = distanceToTarget; + + autoVelocity = LAUNCHER_REGRESSION_A * x + LAUNCHER_REGRESSION_B; + + return autoVelocity; + } + + // 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); + } + + private void setHoodPos(double pos) { + hood.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); } } }