diff --git a/LearnBot/src/main/java/org/firstinspires/ftc/learnbot/component/Launcher.java b/LearnBot/src/main/java/org/firstinspires/ftc/learnbot/component/Launcher.java new file mode 100644 index 00000000..e2f122b4 --- /dev/null +++ b/LearnBot/src/main/java/org/firstinspires/ftc/learnbot/component/Launcher.java @@ -0,0 +1,728 @@ +package org.firstinspires.ftc.learnbot.component; + +import com.bylazar.configurables.annotations.Configurable; +import com.bylazar.telemetry.PanelsTelemetry; +import com.bylazar.telemetry.TelemetryManager; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.DcMotorSimple.Direction; +import com.qualcomm.robotcore.hardware.PIDFCoefficients; +import com.qualcomm.robotcore.util.ElapsedTime; +import com.qualcomm.robotcore.util.MovingStatistics; +import com.technototes.library.command.Command; +import com.technototes.library.command.CommandScheduler; +import com.technototes.library.hardware.motor.EncodedMotor; +import com.technototes.library.logger.Log; +import com.technototes.library.logger.Loggable; +import com.technototes.library.structure.ValidationOpMode; +import com.technototes.library.subsystem.Subsystem; +import com.technototes.library.subsystem.TargetAcquisition; +import com.technototes.library.util.PIDFController; +import java.util.Locale; +import java.util.function.DoubleSupplier; +import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit; + +public class Launcher { + + // *ALL* the configuration should go in here. I moved some things that had been constants up + // to here, as they are "bot build configuration": are the motors reversed. + @Configurable + public static class Config { + + // You can (should...) pull this from a Setup.* location: + public static String getMotorName() { + return "rr"; // return Setup.HardwareNames.MyLauncherMotor; for example + } + + // Is the primary intake motor reversed? + public static boolean PrimaryReversed = true; + // Is the secondary intake motor reversed? + public static boolean SecondaryReversed = false; + + public static double CloseTargetLaunchVelocity = 1400; + public static double FarTargetLaunchVelocity = 1850; + public static double FarTargetLaunchVelocityForAuto = 2300; + public static double TargetLaunchVelocityForAuto1 = 1950; + public static double TargetLaunchVelocityForAuto2 = 1850; + public static double additionDelta = 10; + + // TODO: Document this better + public static PIDFCoefficients launchPID = new PIDFCoefficients(0.004, 0.0002, 0.0, 0); + public static double kStaticFriction = 0.15; + public static double kVelocityConstant = 0.0043; + public static double PeakVoltage = 13.6; + // GoBilda says stall current of 9.2A at 12V, so V = I * R, R = 12 / 9.2 (about 1.3) + // 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 for + // now + public static double MotorResistance = 12 / 9.2; + + // TODO: Make this more configurable. Maybe just turn it into a little helper function that + // lives in this class? + public static double REGRESSION_A = 6.261; // multiplier for x for close zone launch speed formula + public static double REGRESSION_B = 1550; // minimum velocity for close zone launch speed formula + public static double REGRESSION_C = 20; // multiplier for x for far zone launch speed formula + public static double REGRESSION_D = 320; // minimum velocity for far zone launch speed formula - 130, 255 + public static double REGRESSION_C_TELEOP = 20; // multiplier for x for far zone launch speed formula + public static double REGRESSION_D_TELEOP = 130; // minimum velocity for far zone launch speed formula + public static double REGRESSION_C_AUTO = 17; // multiplier for x for far zone launch speed formula + public static double REGRESSION_D_AUTO = 115; // minimum velocity for far zone launch speed formula + } + + // *All* commands for the subsystem belong in here. It's easy for the simple "call a method" + // commands, but for more complicated commands, scroll down to see AutoVelocity/AutoVelocityImpl + public static class Commands { + + // This is a little strange: It's a place to tuck away a reference to the Launcher Subsystem, + // so that all the commands can get to it there. + // It let's us do this: + // button.whenPressed(Launcher.Commands.IncreaseMotor()); + // Instead of this: + // button.whenPressed(Launcher.Commands.IncreaseMotor(r.launcherComponent)); + // This doesn't work if we have *two* different launchers, but I *think* that's unlikely in an + // FTC game ;) + protected static Component component = null; + + public static Command Launch() { + return Command.create(component::Launch); + } + + public static Command SetFarShoot() { + return Command.create(component::FarShoot); + } + + public static Command SetCloseShoot() { + return Command.create(component::CloseShoot); + } + + public static Command AutoLaunch1() { + return Command.create(component::AutoLaunch1); + } + + public static Command AutoLaunch2() { + return Command.create(component::AutoLaunch2); + } + + public static Command FarAutoLaunch() { + return Command.create(component::FarAutoLaunch); + } + + public static Command StopLaunch() { + return Command.create(component::Stop); + } + + public static Command IncreaseMotor() { + return Command.create(component::IncreaseMotorVelocity); + } + + public static Command DecreaseMotor() { + return Command.create(component::DecreaseMotorVelocity); + } + + public static Command ReadVelocity() { + return Command.create(component::readVelocity); + } + + public static Command SetRegressionAuto() { + return Command.create(component::setRegressionAuto); + } + + public static Command SetRegressionTeleop() { + return Command.create(component::setRegressionTeleop); + } + + public static Command IncreaseRegressionDTeleop() { + return Command.create(component::increaseRegressionDTeleop); + } + + // This is just to make all commands look the same to the 'outside' user: + // You just call LauncherCommands.AutoVelocity() instead of needing to differentiate + // between simple Command.create's and more complex "class" commands. + public static Command AutoVelocity() { + return new AutoVelocityImpl(); + } + + // This class is protected to ensure that elsewhere, you can't ever use + // button.whenPressed(new AutoVelocityImpl()); + // but instead you have to use + // button.whenPressed(Launcher.Commands.AutoVelocity()); + protected static class AutoVelocityImpl implements Command { + + public AutoVelocityImpl() {} + + // This command is designed to *never* finish. + // It should be run in a parallel command group/alongWith/raceWith group. + @Override + public boolean isFinished() { + return false; + } + + @Override + public void execute() { + component.Launch(); + } + } + } + + @Configurable + public static class Component implements Loggable, Subsystem { + + @Log.Number(name = "Target Velocity") + public static double targetLaunchVelocity = 1150; + + @Log.Number(name = "Current Motor Velocity") + public static double currentLaunchVelocity = 0.0; + + public static double motorVelocity; + public static double additionAmount; + + public static double launcher1Current; + public static double launcher2Current; + + @Log(name = "Target Speed: ") + public static double targetSpeed; + + @Log(name = "Target Power: ") + public static double targetPower; + + private static PIDFController launcherPID; + public static double lastAutoVelocity = 0; + + @Log.Number(name = "AutoAim Velocity") + public static double autoVelocity; + + // External dependencies this component requires: + EncodedMotor launcher1; + EncodedMotor launcher2; + TargetAcquisition targetAcquisition; + DoubleSupplier voltage; + + public Component( + EncodedMotor primary, + EncodedMotor secondary, + TargetAcquisition targetSubsystem, + DoubleSupplier voltageSup + ) { + // Save this off for commands to use + Commands.component = this; + launcher1 = primary; + if (hasLaunch1()) { + launcher1.setDirection( + Config.PrimaryReversed ? Direction.REVERSE : Direction.FORWARD + ); + launcher1.coast(); + } + launcher2 = secondary; + if (hasLaunch2()) { + launcher2.setDirection( + Config.SecondaryReversed ? Direction.REVERSE : Direction.FORWARD + ); + launcher2.coast(); + } + targetAcquisition = targetSubsystem; + voltage = voltageSup != null ? voltageSup : () -> Config.PeakVoltage; + + // A quick wander around google comes up with something like this for motor feedfwd: + + // launcherMyPID = new PIDFController(Config.launcherPID, target -> + // (Config.kStaticFriction + Config.kVelocityConstant * target) / voltage.getAsDouble()); + + // The point is that motor RPM scales linearly with voltage, so to compensate, you should + // divide by voltage: Don't try to scale something by a delta from peak. Just divide. + + // To get solve that formula, get a fresh battery, run it at full power and measure the RPM. + // (Well, and figure out kStaticFriction, too: The lowest value that will still get the + // launcher barely moving) + launcherPID = new PIDFController(Config.launchPID, target -> { + if (target == 0) return 0.0; + return ( + (Math.copySign( + Config.kStaticFriction + getMotor1Current() * Config.MotorResistance, + target + ) + + Config.kVelocityConstant * target) / + voltage.getAsDouble() + ); + }); + + setTargetSpeed(0); + CommandScheduler.register(this); + } + + public Component() { + this(null, null, null, () -> 0); + } + + public Component( + EncodedMotor primary, + TargetAcquisition targetSubsystem, + DoubleSupplier voltageSup + ) { + this(primary, null, targetSubsystem, voltageSup); + } + + public void Launch() { + // Spin the motors pid goes here + setTargetSpeed(autoVelocity()); //change to auto aim velocity + } + + public void CloseShoot() { + // Spin the motors pid goes here + targetLaunchVelocity = Config.CloseTargetLaunchVelocity; + } + + public void FarShoot() { + // Spin the motors pid goes here + targetLaunchVelocity = Config.FarTargetLaunchVelocity; + } + + public void AutoLaunch1() { + // Spin the motors pid goes here + setTargetSpeed(Config.TargetLaunchVelocityForAuto1); //change to auto aim velocity + } + + public void AutoLaunch2() { + // Spin the motors pid goes here + setTargetSpeed(Config.TargetLaunchVelocityForAuto2); //change to auto aim velocity + } + + public void FarAutoLaunch() { + // Spin the motors pid goes here + setTargetSpeed(Config.FarTargetLaunchVelocityForAuto); //change to auto aim velocity + } + + public double readVelocity() { + if (hasLaunch1()) { + return launcher1.getVelocity(); + } else { + return Double.NaN; // Nan = "Not a Number" + } + } + + public void setTargetSpeed(double speed) { + targetSpeed = speed; + launcherPID.setTarget(speed); + } + + public double getTargetSpeed() { + return targetSpeed; + } + + protected void setMotorPower(double pow) { + double power = Math.clamp(pow, -1, 1); + targetPower = power; + if (hasLaunch1()) { + launcher1.setPower(power); + } + if (hasLaunch2()) { + launcher2.setPower(power); + } + } + + public double getMotorSpeed() { + if (hasLaunch1()) { + return launcher1.getVelocity(); + } + return -1; + } + + public double getMotor1Current() { + if (hasLaunch1()) { + return launcher1.getAmperage(CurrentUnit.AMPS); + } + return -1; + } + + public double getMotor2Current() { + if (hasLaunch2()) { + return launcher2.getAmperage(CurrentUnit.AMPS); + } + return -1; + } + + public void Stop() { + launcherPID.setTarget(0); + } + + public void IncreaseMotorVelocity() { + // Spin the motors pid goes here + additionAmount += Config.additionDelta; + } + + public void DecreaseMotorVelocity() { + // Spin the motors pid goes here + additionAmount -= Config.additionDelta; + } + + public double getMotor1Velocity() { + if (hasLaunch1()) { + return launcher1.getVelocity(); + } else { + return Double.NaN; // Not a Number + } + } + + public double getMotor2Velocity() { + if (hasLaunch2()) { + return launcher2.getVelocity(); + } else { + return Double.NaN; // Not a Number + } + } + + // This is wrong (and, as it turns out, unused), so I just commented it out + // TODO: We should discuss how to accomplish what this is attempting to do + /* + public void VelocityShoot() { + if ( + getMotor1Velocity() == targetLaunchVelocity && + getMotor2Velocity() == targetLaunchVelocity + ) { + // This doesn't do anything. It *creates* a command, but the command is never + // scheduled, so it won't ever actually do anything. + TeleCommands.GateDown(robot); + } + } + */ + + // TOOD: Turn this into a quadratic equation instead, since that's what it's mimicking. + public double autoVelocity() { + // x = distance in inches + double x = getTargetDistance(); + + // TODO: Clean this up, make it a formula + if (x < 0) { + return lastAutoVelocity; + } else if (x < 100) { + lastAutoVelocity = Config.REGRESSION_A * x + Config.REGRESSION_B; + return lastAutoVelocity; + } else { + return Config.REGRESSION_C * x + Config.REGRESSION_D; + } + } + + public void setRegressionAuto() { + // Spin the motors pid goes here + Config.REGRESSION_C = Config.REGRESSION_C_AUTO; + Config.REGRESSION_D = Config.REGRESSION_D_AUTO; + } + + public void setRegressionTeleop() { + // Spin the motors pid goes here + Config.REGRESSION_C = Config.REGRESSION_C_TELEOP; + Config.REGRESSION_D = Config.REGRESSION_D_TELEOP; + } + + public void increaseRegressionDTeleop() { + // Spin the motors pid goes here + Config.REGRESSION_D += 15; + } + + // This lets the 'no hardware' or 'subsystem disabled' thing still work without a functional + // target acquisition subsystem + private double getTargetDistance() { + if (targetAcquisition != null) { + return targetAcquisition.getDistance(); + } + return -1; + } + + @Override + public void periodic() { + autoVelocity = autoVelocity(); + currentLaunchVelocity = readVelocity(); + motorVelocity = getMotorSpeed(); + if (launcherPID.getTarget() != 0) { + setMotorPower(launcherPID.update(motorVelocity) + additionAmount); + } else { + setMotorPower(0); + // When we want to stop, reset the PID controller + launcherPID.update(motorVelocity); + launcherPID.reset(); + } + launcher1Current = getMotor1Current(); + launcher2Current = getMotor2Current(); + } + + // This should be used by a test opmode to check that the basics are working. + public String hardwareValidation(double power1, double power2) { + String res = ""; + if (hasLaunch1()) { + launcher1.setPower(power1); + } else { + res += "(no launcher1) "; + } + if (hasLaunch2()) { + launcher2.setPower(power2); + } else { + res += "(no launcher2) "; + } + res += String.format( + Locale.ENGLISH, + "Speed: %.2f, Current1: %.2f, Current2: %.2f", + getMotorSpeed(), + getMotor1Current(), + getMotor2Current() + ); + return res; + } + + private boolean hasLaunch1() { + return launcher1 != null; + } + + private boolean hasLaunch2() { + return launcher2 != null; + } + } + + /************************************************************************* + * Op modes: Can they be internal classes? I don't know! Let's find out! * + *************************************************************************/ + @Configurable + @SuppressWarnings("unused") + @TeleOp(name = "Launcher Validation", group = "Launcher") + public static class Validator extends ValidationOpMode { + + public Launcher.Component lc; + + @Override + public void init() { + super.init(); + EncodedMotor m = new EncodedMotor<>( + hardwareMap.get(DcMotorEx.class, Config.getMotorName()), + Config.getMotorName() + ); + lc = new Launcher.Component(m, null, this::getVoltage); + } + + @Override + public void loop() { + super.loop(); + telemetry.addLine(">>> Press left trigger for Launcher1 control"); + telemetry.addLine(">>> Press right trigger for Launcher2 control"); + telemetry.addLine(">>> Hit the dpad to stop"); + telemetry.addLine(lc.hardwareValidation(gamepad1.left_trigger, gamepad1.right_trigger)); + telemetry.update(); + if (anyDpadReleased()) { + terminateOpModeNow(); + } + } + } + + @TeleOp(name = "Launcher FeedFwd Helper", group = "Launcher") + public static class FeedFwdHelper extends ValidationOpMode { + + private enum State { + MeasureStaticFriction, + DoneWithFriction, + MeasureVelocity, + DoneWithVelocity, + Testing, + Abort, + } + + Launcher.Component lc = null; + State state = State.MeasureStaticFriction; + TelemetryManager panelsTelemetry = null; + + @Override + public void init() { + panelsTelemetry = PanelsTelemetry.INSTANCE.getTelemetry(); + super.init(); + EncodedMotor m = new EncodedMotor<>( + hardwareMap.get(DcMotorEx.class, Config.getMotorName()), + Config.getMotorName() + ); + lc = new Launcher.Component(m, null, this::getVoltage); + state = State.MeasureStaticFriction; + lc.setMotorPower(0); + } + + double staticFriction = 0.001; + double velocityConstant = 0; + + double staticFrictionStep = 0.001; + MovingStatistics velocityConstantStats = new MovingStatistics(50); + double vel = 0; + double targetVelocity = 0; + MovingStatistics error = new MovingStatistics(1000); + ElapsedTime lastUpdate = new ElapsedTime(); + + // Start slowly increasing power until we detect motion, then back off ever so slightly + private State MeasureStaticFriction() { + double v = getVoltage(); + double amps = lc.getMotor1Current(); + double power = (staticFriction + amps * Config.MotorResistance) / v; + lc.setMotorPower(power); + addLine("Press a button to abort (and just be patient)"); + addData("kStaticFriction", staticFriction); + addData("Voltage", v); + addData("Power", power); + if (lastUpdate.milliseconds() >= 100) { + lastUpdate.reset(); + // We update every 100 milliseconds, just to give it time to trigger the encoder + if (lc.getMotor1Velocity() != 0) { + lc.setMotorPower(0); + staticFriction -= staticFrictionStep; + return State.DoneWithFriction; + } + staticFriction += staticFrictionStep; + } + if (anyButtonsReleased()) { + lc.setMotorPower(0); + return State.Abort; + } + return State.MeasureStaticFriction; + } + + private State DoneWithFriction() { + // Display results of Static Friction calculator & wait for user + addData("kStaticFriction-->>", staticFriction); + addLine("Hit a button to start the velocity measurement"); + lastUpdate.reset(); + return (anyButtonsReleased() || anyDpadReleased()) + ? State.MeasureVelocity + : State.DoneWithFriction; + } + + private State MeasureVelocity() { + // If we're here, the system started moving. Stop the motors and set kStaticFriction to + // just below what was necessary to start the system moving. + // Display the kStaticFriction value we got: + // Next up: Velocity! + double vol = 0; + double amps; + if (anyButtonsReleased()) { + velocityConstant = velocityConstantStats.getMean(); + lc.setMotorPower(0); + return State.DoneWithVelocity; + } + lc.setMotorPower(1); + if (lastUpdate.milliseconds() >= 50) { + lastUpdate.reset(); + vel = lc.getMotor1Velocity(); + double curVol = getVoltage(); + if (curVol > 0) { + vol = curVol; + } + amps = lc.getMotor1Current(); + if (vel != 0) { + // power = (kStaticFriction + kVelocityConstant * RPM) / v + // So + // 1 = (kSF + kV * RPM + motorAmperage * motorResistance) / v; + // solve for kV: + // kV = (v - kSF - motorAmperage * motorResistance) / RPM + velocityConstant = (vol - staticFriction - amps * Config.MotorResistance) / vel; + velocityConstantStats.add(velocityConstant); + } + } + addData("kStaticFriction!", staticFriction); + addLine("Press a button to stop velocity measurement"); + addData("kVelocityConstant", velocityConstant); + addData("Average kV", velocityConstantStats.getMean()); + addData("Velocity", vel); + addData("Voltage", vol); + addData("Power", 1.0); + return State.MeasureVelocity; + } + + private State DoneWithVelocity() { + // Display results of Velocity Constant calculator & wait for user + addData("Measured kStaticFriction", staticFriction); + addData("Measured kVelocityConstant", velocityConstant); + addLine("Hit a button or move the dpad to move to testing"); + targetVelocity = vel * 0.5; + error.clear(); + error.add(0); + lastUpdate.reset(); + return (anyButtonsReleased() || anyDpadReleased()) + ? State.Testing + : State.DoneWithVelocity; + } + + private State Testing() { + double pow = + targetVelocity == 0 + ? 0 + : ((Math.copySign(staticFriction, targetVelocity) + + velocityConstant * targetVelocity) / + getVoltage()); + lc.setMotorPower(pow); + if (lastUpdate.milliseconds() > 250) { + lastUpdate.reset(); + vel = lc.getMotor1Velocity(); + error.add(targetVelocity - vel); + } + addData("kStaticFriction", staticFriction); + addData("kVelocityConstant", velocityConstant); + addLine("Press any button to stop"); + addLine("Press the dpad to change target velocity"); + addData("target", targetVelocity); + addData("measured", vel); + addData("Avg Err (after 0.25s)", error.getMean()); + addData("stddev", error.getStandardDeviation()); + addData("Power", pow); + if (gamepad1.dpadLeftWasPressed()) { + lastUpdate.reset(); + targetVelocity -= 100; + } else if (gamepad1.dpadRightWasPressed()) { + lastUpdate.reset(); + targetVelocity += 100; + } else if (gamepad1.dpadUpWasPressed()) { + lastUpdate.reset(); + targetVelocity += 10; + } else if (gamepad1.dpadDownWasPressed()) { + lastUpdate.reset(); + targetVelocity -= 10; + } + return anyButtonsReleased() ? State.Abort : State.Testing; + } + + public void loop() { + // Look at that, a silly little state machine... + switch (state) { + case MeasureStaticFriction: + state = MeasureStaticFriction(); + break; + case DoneWithFriction: + state = DoneWithFriction(); + break; + case MeasureVelocity: + state = MeasureVelocity(); + break; + case DoneWithVelocity: + state = DoneWithVelocity(); + break; + case Testing: + state = Testing(); + break; + case Abort: + default: + terminateOpModeNow(); + break; + } + + super.loop(); + panelsTelemetry.update(); + } + + @Override + public void addData(String caption, String data) { + super.addData(caption, data); + panelsTelemetry.addData(caption, data); + } + + @Override + public void addData(String caption, double d) { + super.addData(caption, d); + panelsTelemetry.addData(caption, d); + } + + @Override + public void addLine(String line) { + super.addLine(line); + panelsTelemetry.addLine(line); + } + } +} diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/Hardware.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/Hardware.java index 86c5dd37..02e5f904 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/Hardware.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/Hardware.java @@ -68,7 +68,7 @@ public Hardware(HardwareMap hwmap) { if (Setup.Connected.INTAKESUBSYSTEM) { intake = this.map.get(DcMotorEx.class, Setup.HardwareNames.INTAKE_MOTOR); } - if (Setup.Connected.LAUNCHERSUBSYSTEM) { + if (Setup.Connected.LAUNCHERCOMPONENT) { launcher1 = new EncodedMotor(Setup.HardwareNames.LAUNCHER_MOTOR1); launcher2 = new EncodedMotor(Setup.HardwareNames.LAUNCHER_MOTOR2); } diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/Robot.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/Robot.java index b4a2d105..08433bf9 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/Robot.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/Robot.java @@ -3,11 +3,11 @@ import com.pedropathing.follower.Follower; import com.technototes.library.logger.Loggable; import com.technototes.library.util.Alliance; +import org.firstinspires.ftc.sixteen750.component.LauncherComponent; import org.firstinspires.ftc.sixteen750.helpers.StartingPosition; import org.firstinspires.ftc.sixteen750.subsystems.AimingSubsystem; import org.firstinspires.ftc.sixteen750.subsystems.BrakeSubsystem; import org.firstinspires.ftc.sixteen750.subsystems.IntakeSubsystem; -import org.firstinspires.ftc.sixteen750.subsystems.LauncherSubsystem; import org.firstinspires.ftc.sixteen750.subsystems.LimelightSubsystem; import org.firstinspires.ftc.sixteen750.subsystems.SafetySubsystem; import org.firstinspires.ftc.sixteen750.subsystems.TestSubsystem; @@ -20,7 +20,7 @@ public class Robot implements Loggable { public double initialVoltage; public SafetySubsystem safetySubsystem; - public LauncherSubsystem launcherSubsystem; + public LauncherComponent launcherComponent; public IntakeSubsystem intakeSubsystem; public BrakeSubsystem brakeSubsystem; public AimingSubsystem aimingSubsystem; @@ -41,8 +41,23 @@ public Robot(Hardware hw, Alliance team, StartingPosition pos) { if (Setup.Connected.INTAKESUBSYSTEM) { this.intakeSubsystem = new IntakeSubsystem(hw); } - if (Setup.Connected.LAUNCHERSUBSYSTEM) { - this.launcherSubsystem = new LauncherSubsystem(hw); + if (Setup.Connected.LAUNCHERCOMPONENT) { + // For Review: + // The limelightSubsystem now implements the "TargetAcquisition" interface. This let's + // us completely decouple the LauncherComponent from the LimelightSubsystem, and instead + // just requires that we provide *anything* that will tell us the distance to target. + // We could build out a little thing that only uses Odometry to decide where the target + // is, instead, and wouldn't need to change anything in the LauncherComponent. + this.launcherComponent = new LauncherComponent( + hw.launcher1, + hw.launcher2, + limelightSubsystem, + hw::voltage + ); + } else { + // This should let us run code (scheduled commands, for instance...) without any + // hardware in the launcher subsystem + this.launcherComponent = new LauncherComponent(); } if (Setup.Connected.BRAKESUBSYSTEM) { this.brakeSubsystem = new BrakeSubsystem(hw); diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/Setup.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/Setup.java index 0705e557..209fcf00 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/Setup.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/Setup.java @@ -16,7 +16,7 @@ public static class Connected { public static boolean EXTERNAL_IMU = true; public static boolean OTOS = false; public static boolean INTAKESUBSYSTEM = true; - public static boolean LAUNCHERSUBSYSTEM = true; + public static boolean LAUNCHERCOMPONENT = true; public static boolean AIMINGSUBSYSTEM = true; public static boolean BRAKESUBSYSTEM = true; } diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/commands/AltAutoOrient.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/commands/AltAutoOrient.java index cf4ffc55..3590048f 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/commands/AltAutoOrient.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/commands/AltAutoOrient.java @@ -11,7 +11,6 @@ import com.technototes.library.util.PIDFController; import org.firstinspires.ftc.sixteen750.Robot; import org.firstinspires.ftc.sixteen750.Setup; -import org.firstinspires.ftc.sixteen750.subsystems.LauncherSubsystem; import org.firstinspires.ftc.sixteen750.subsystems.LimelightSubsystem; @Configurable @@ -73,7 +72,7 @@ public void execute() { // Math.toRadians(robot.limelightSubsystem.getTX()) //.getTX .getLimelightRotation() // ); // robot.follower.holdPoint(new BezierPoint(wantedPose), wantedPose.getHeading(), false); - // LauncherSubsystem.targetPower = 1; + // LauncherComponent.targetPower = 1; // firsttime = false; //} currentPose = robot.follower.getPose(); diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/commands/AltAutoOrientFar.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/commands/AltAutoOrientFar.java index 2b6df322..89ad3f37 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/commands/AltAutoOrientFar.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/commands/AltAutoOrientFar.java @@ -69,7 +69,7 @@ public void execute() { // Math.toRadians(robot.limelightSubsystem.getTX()) //.getTX .getLimelightRotation() // ); // robot.follower.holdPoint(new BezierPoint(wantedPose), wantedPose.getHeading(), false); - // LauncherSubsystem.targetPower = 1; + // LauncherComponent.targetPower = 1; // firsttime = false; //} currentPose = robot.follower.getPose(); diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/commands/AltAutoVelocity.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/commands/AltAutoVelocity.java deleted file mode 100644 index fcddc163..00000000 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/commands/AltAutoVelocity.java +++ /dev/null @@ -1,34 +0,0 @@ -package org.firstinspires.ftc.sixteen750.commands; - -import com.bylazar.configurables.annotations.Configurable; -import com.pedropathing.geometry.Pose; -import com.technototes.library.command.Command; -import org.firstinspires.ftc.sixteen750.Robot; -import org.firstinspires.ftc.sixteen750.Setup; -import org.firstinspires.ftc.sixteen750.subsystems.LimelightSubsystem; - -@Configurable -public class AltAutoVelocity implements Command { - - public Robot robot; - - public AltAutoVelocity(Robot r) { - robot = r; - } - - @Override - public boolean isFinished() { - //return !robot.follower.isBusy(); - return false; - } - - @Override - public void execute() { - robot.launcherSubsystem.Launch(); - } - - // @Override - // public void end(boolean s) { - // robot.follower.drivetrain.breakFollowing(); - // } -} diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/commands/TeleCommands.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/commands/TeleCommands.java index cb310130..43d21f89 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/commands/TeleCommands.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/commands/TeleCommands.java @@ -14,34 +14,6 @@ public static Command TurnTo90(Robot r) { ); //pose might need to be current pose? } - public static Command Launch(Robot r) { - return Command.create(r.launcherSubsystem::Launch); - } - - public static Command SetFarShoot(Robot r) { - return Command.create(r.launcherSubsystem::FarShoot); - } - - public static Command SetCloseShoot(Robot r) { - return Command.create(r.launcherSubsystem::CloseShoot); - } - - public static Command AutoLaunch1(Robot r) { - return Command.create(r.launcherSubsystem::AutoLaunch1); - } - - public static Command AutoLaunch2(Robot r) { - return Command.create(r.launcherSubsystem::AutoLaunch2); - } - - public static Command FarAutoLaunch(Robot r) { - return Command.create(r.launcherSubsystem::FarAutoLaunch); - } - - public static Command StopLaunch(Robot r) { - return Command.create(r.launcherSubsystem::Stop); - } - public static Command Rumble(Robot r) { return Command.create(r.intakeSubsystem::setRumble); } @@ -50,14 +22,6 @@ public static Command RumbleOff(Robot r) { return Command.create(r.intakeSubsystem::setRumbleOff); } - public static Command IncreaseMotor(Robot r) { - return Command.create(r.launcherSubsystem::IncreaseMotorVelocity); - } - - public static Command DecreaseMotor(Robot r) { - return Command.create(r.launcherSubsystem::DecreaseMotorVelocity); - } - public static Command Intake(Robot r) { return Command.create(r.intakeSubsystem::Intake); } @@ -117,28 +81,4 @@ public static Command MotorPowerTest(Robot r) { public static Command MotorVelocityTest(Robot r) { return Command.create(r.testSubsystem::setMotorVelocityTest); } - - public static Command ReadVelocity(Robot r) { - return Command.create(r.launcherSubsystem::readVelocity); - } - - public static Command SetRegressionCAuto(Robot r) { - return Command.create(r.launcherSubsystem::setRegressionCAuto); - } - - public static Command SetRegressionDAuto(Robot r) { - return Command.create(r.launcherSubsystem::setRegressionDAuto); - } - - public static Command SetRegressionCTeleop(Robot r) { - return Command.create(r.launcherSubsystem::setRegressionCTeleop); - } - - public static Command SetRegressionDTeleop(Robot r) { - return Command.create(r.launcherSubsystem::setRegressionDTeleop); - } - - public static Command IncreaseRegressionDTeleop(Robot r) { - return Command.create(r.launcherSubsystem::increaseRegressionDTeleop); - } } diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/commands/auto/Paths.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/commands/auto/Paths.java index 8f5ab6bb..cae8e313 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/commands/auto/Paths.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/commands/auto/Paths.java @@ -15,6 +15,7 @@ import org.firstinspires.ftc.sixteen750.commands.AltAutoOrient; import org.firstinspires.ftc.sixteen750.commands.AltAutoOrientFar; import org.firstinspires.ftc.sixteen750.commands.TeleCommands; +import org.firstinspires.ftc.sixteen750.component.LauncherComponent.LauncherCommand; @Configurable public class Paths { @@ -26,7 +27,7 @@ public static Command Launching3Balls(Robot r) { new ParallelCommandGroup( TeleCommands.Intake(r), TeleCommands.GateUp(r), - TeleCommands.AutoLaunch1(r) + LauncherCommand.AutoLaunch1() ), new WaitCommand(3), TeleCommands.GateDown(r), diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/component/LauncherComponent.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/component/LauncherComponent.java new file mode 100644 index 00000000..858825f6 --- /dev/null +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/component/LauncherComponent.java @@ -0,0 +1,626 @@ +package org.firstinspires.ftc.sixteen750.component; + +import com.bylazar.configurables.annotations.Configurable; +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.qualcomm.robotcore.util.ElapsedTime; +import com.qualcomm.robotcore.util.MovingStatistics; +import com.technototes.library.command.Command; +import com.technototes.library.command.CommandScheduler; +import com.technototes.library.hardware.motor.EncodedMotor; +import com.technototes.library.logger.Log; +import com.technototes.library.logger.Loggable; +import com.technototes.library.subsystem.Subsystem; +import com.technototes.library.subsystem.TargetAcquisition; +import com.technototes.library.util.PIDFController; +import java.util.Locale; +import java.util.function.DoubleSupplier; +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit; + +@Configurable +public class LauncherComponent implements Loggable, Subsystem { + + // This is a little strange: It's a place to tuck away a reference to the Launcher Subsystem, + // so that all the commands can get to it there. + // It let's us do this: + // button.whenPressed(LauncherComponent.Commands.IncreaseMotor()); + // Instead of this: + // button.whenPressed(LauncherComponent.Commands.IncreaseMotor(r.launcherComponent)); + private static LauncherComponent self = null; + + // *ALL* the configuration should go in here. I moved some things that had been constants up + // to here, as they are "bot build configuration": are the motors reversed. + // TODO: Make "are there 1 or 2 motors" a configuration setting + public static class Config { + + // Is the primary intake motor reversed? + public static boolean PrimaryReversed = true; + // Is the secondary intake motor reversed? + public static boolean SecondaryReversed = false; + + // @Log.Number(name = "Motor Power") + // public static double MOTOR_POWER = 0.65; // 0.5 1.0 + public static double CloseTargetLaunchVelocity = 1400; + public static double FarTargetLaunchVelocity = 1850; + public static double FarTargetLaunchVelocityForAuto = 2300; + public static double TargetLaunchVelocityForAuto1 = 1950; + public static double TargetLaunchVelocityForAuto2 = 1850; + + // TODO: Document this better + public static PIDFCoefficients launcherPI = new PIDFCoefficients(0.004, 0.0002, 0.0, 0); + public static double Near_P = 0.004; + public static double Near_I = 0.0002; + public static double Far_P = 0.004; + public static double Far_I = 0.0002; + public static double SPIN_F_SCALE = 0.00021; + public static double SPIN_VOLT_COMP = 0.0216; + public static double DIFFERENCE = 0.0046; + public static double PEAK_VOLTAGE = 13; + + // TODO: Make this more configurable. Maybe just turn it into a little helper function that + // lives in this class? + public static double REGRESSION_A = 6.261; // multiplier for x for close zone launch speed formula + public static double REGRESSION_B = 1550; // minimum velocity for close zone launch speed formula + public static double REGRESSION_C = 20; // multiplier for x for far zone launch speed formula + public static double REGRESSION_D = 320; // minimum velocity for far zone launch speed formula - 130, 255 + public static double REGRESSION_C_TELEOP = 20; // multiplier for x for far zone launch speed formula + public static double REGRESSION_D_TELEOP = 130; // minimum velocity for far zone launch speed formula + public static double REGRESSION_C_AUTO = 17; // multiplier for x for far zone launch speed formula + public static double REGRESSION_D_AUTO = 115; // minimum velocity for far zone launch speed formula + } + + // *All* commands for the subsystem belong in here. It's easy for the simple "call a method" + // commands, but for more complicated commands, scroll down to see AutoVelocity/AutoVelocityImpl + public static class LauncherCommand { + + public static Command Launch() { + return Command.create(self::Launch); + } + + public static Command SetFarShoot() { + return Command.create(self::FarShoot); + } + + public static Command SetCloseShoot() { + return Command.create(self::CloseShoot); + } + + public static Command AutoLaunch1() { + return Command.create(self::AutoLaunch1); + } + + public static Command AutoLaunch2() { + return Command.create(self::AutoLaunch2); + } + + public static Command FarAutoLaunch() { + return Command.create(self::FarAutoLaunch); + } + + public static Command StopLaunch() { + return Command.create(self::Stop); + } + + public static Command IncreaseMotor() { + return Command.create(self::IncreaseMotorVelocity); + } + + public static Command DecreaseMotor() { + return Command.create(self::DecreaseMotorVelocity); + } + + public static Command ReadVelocity() { + return Command.create(self::readVelocity); + } + + public static Command SetRegressionAuto() { + return Command.create(self::setRegressionAuto); + } + + public static Command SetRegressionTeleop() { + return Command.create(self::setRegressionTeleop); + } + + public static Command IncreaseRegressionDTeleop() { + return Command.create(self::increaseRegressionDTeleop); + } + + // This is just to make all commands look the same to the 'outside' user: + // You just call LauncherCommands.AutoVelocity() instead of needing to differentiate + // between simple Command.create's and more complex "class" commands. + public static Command AutoVelocity() { + return new AutoVelocityImpl(); + } + + // This class is protected to ensure that elsewhere, you can't ever use + // button.whenPressed(new AutoVelocityImpl()); + // but instead you have to use + // button.whenPressed(LaunchCommand.AutoVelocity()); + protected static class AutoVelocityImpl implements Command { + + public AutoVelocityImpl() {} + + // This command is designed to *never* finish. + // It should be run in a parallel command group/alongWith/raceWith group. + @Override + public boolean isFinished() { + return false; + } + + @Override + public void execute() { + LauncherComponent.self.Launch(); + } + } + } + + @Log.Number(name = "Target Velocity") + public static double targetLaunchVelocity = 1150; + + @Log.Number(name = "Current Motor Velocity") + public static double currentLaunchVelocity = 0.0; + + public static double motorVelocity; + public static double additionAmount; + public static double additionDelta = 5; + + //@Log(name = "Launcher Power: ") + public static double power; + + //@Log(name = "Error") + public static double err; + public static double launcher1Current; + public static double launcher2Current; + + @Log(name = "Target Speed: ") + public static double targetSpeed; + + @Log(name = "Target Power: ") + public static double targetPower; + + private static PIDFController launcherPID; + public static double lastAutoVelocity = 0; + + @Log.Number(name = "AutoAim Velocity") + public static double autoVelocity; + + // External dependencies this component requires: + EncodedMotor launcher1; + EncodedMotor launcher2; + TargetAcquisition targetAcquisition; + DoubleSupplier voltage; + + public LauncherComponent( + EncodedMotor primary, + EncodedMotor secondary, + TargetAcquisition targetSubsystem, + DoubleSupplier voltageSup + ) { + self = this; + launcher1 = primary; + if (hasLaunch1()) { + launcher1.setDirection( + Config.PrimaryReversed + ? DcMotorSimple.Direction.REVERSE + : DcMotorSimple.Direction.FORWARD + ); + launcher1.coast(); + } + launcher2 = secondary; + if (hasLaunch2()) { + launcher2.setDirection( + Config.SecondaryReversed + ? DcMotorSimple.Direction.REVERSE + : DcMotorSimple.Direction.FORWARD + ); + launcher2.coast(); + } + targetAcquisition = targetSubsystem; + voltage = voltageSup != null ? voltageSup : () -> Config.PEAK_VOLTAGE; + // All the Feedfwd stuff below seems...confused. + // Notes: Addition will *increase* as voltage decreases (I think this is expected) + // but it likely won't ever be zero, and it could (with a new battery) be *negative* + // I've tried to model it here: https://www.desmos.com/calculator/gmzlhkyz5a + + // (I is the initial voltage delta (ADDITION), and V is the voltage as the bot is runs. + // Don't animate I, just V, and you'll see that as voltage decreases, so does the output + // power. Drag I around to change the initial voltage delta) + + // The thing that saves us is I, which does what I believe we're expecting: It increase + // power as initial voltage is lower. But then the FF function drops it. + + // My suggestion: rip out all the 'initial' stuff and make the core F function return higher + // values as voltage *decreases*, which is I think what we're really looking for anyway. + + double ADDITION = (Config.PEAK_VOLTAGE - voltage.getAsDouble()); + if (ADDITION == 0) { + // I'd be stunned if this code runs...ever. Not sure what's supposed to happen here. + Config.SPIN_VOLT_COMP = Config.SPIN_VOLT_COMP + 0.001; + } else { + // This seems like a sensible thing: We're adding some amount of voltage delta to + // compensate for a lower initial voltage + Config.SPIN_VOLT_COMP = Config.SPIN_VOLT_COMP + (ADDITION * Config.DIFFERENCE); + } + launcherPID = new PIDFController(Config.launcherPI, target -> + target == 0 + ? 0 + : (Config.SPIN_F_SCALE * target) + + // This is weird: We're going to *reduce* the output power slightly as voltage + // decreases over time. I don't think this is what we're trying to accomplish. + (Config.SPIN_VOLT_COMP * Math.min(Config.PEAK_VOLTAGE, voltage.getAsDouble())) + ); + // A quick wander around google comes up with something like this: + + // launcherMyPID = new PIDFController(Config.launcherPID, target -> + // (Config.kStaticFriction + Config.kVelocityConstant * target) / voltage.getAsDouble()); + + // The point is that motor RPM scales linearly with voltage, so to compensate, you should + // divide by voltage: Don't try to scale something by a delta from peak. Just divide. + + // To get solve that formula, get a fresh battery, run it at full power and measure the RPM. + // (Well, and figure out kStaticFriction, too: The lowest value that will still get the + // launcher barely moving) + + // These capabilities should probably go in the "validation" function at the bottom :D + + setTargetSpeed(0); + CommandScheduler.register(this); + } + + public LauncherComponent() { + this(null, null, null, () -> 0); + } + + public LauncherComponent( + EncodedMotor primary, + TargetAcquisition targetSubsystem, + DoubleSupplier voltageSup + ) { + this(primary, null, targetSubsystem, voltageSup); + } + + public void Launch() { + // Spin the motors pid goes here + setTargetSpeed(autoVelocity()); //change to auto aim velocity + } + + public void CloseShoot() { + // Spin the motors pid goes here + targetLaunchVelocity = Config.CloseTargetLaunchVelocity; + } + + public void FarShoot() { + // Spin the motors pid goes here + targetLaunchVelocity = Config.FarTargetLaunchVelocity; + } + + public void AutoLaunch1() { + // Spin the motors pid goes here + setTargetSpeed(Config.TargetLaunchVelocityForAuto1); //change to auto aim velocity + } + + public void AutoLaunch2() { + // Spin the motors pid goes here + setTargetSpeed(Config.TargetLaunchVelocityForAuto2); //change to auto aim velocity + } + + public void FarAutoLaunch() { + // Spin the motors pid goes here + setTargetSpeed(Config.FarTargetLaunchVelocityForAuto); //change to auto aim velocity + } + + public double readVelocity() { + if (hasLaunch1()) { + return launcher1.getVelocity(); + } else { + return Double.NaN; // Nan = "Not a Number" + } + } + + public void setTargetSpeed(double speed) { + targetSpeed = speed; + launcherPID.setTarget(speed); + } + + public double getTargetSpeed() { + return targetSpeed; + } + + private void setMotorPower(double pow) { + double power = Math.clamp(pow, -1, 1); + targetPower = power; + if (hasLaunch1()) { + launcher1.setPower(power); + } + if (hasLaunch2()) { + launcher2.setPower(power); + } + } + + public double getMotorSpeed() { + if (hasLaunch1()) { + return launcher1.getVelocity(); + } + return -1; + } + + public double getMotor1Current() { + if (hasLaunch1()) { + return launcher1.getAmperage(CurrentUnit.AMPS); + } + return -1; + } + + public double getMotor2Current() { + if (hasLaunch2()) { + return launcher2.getAmperage(CurrentUnit.AMPS); + } + return -1; + } + + public void Stop() { + launcherPID.setTarget(0); + } + + public void IncreaseMotorVelocity() { + // Spin the motors pid goes here + additionAmount += additionDelta; + } + + public void DecreaseMotorVelocity() { + // Spin the motors pid goes here + additionAmount -= additionDelta; + } + + public double getMotor1Velocity() { + if (hasLaunch1()) { + return launcher1.getVelocity(); + } else { + return Double.NaN; // Not a Number + } + } + + public double getMotor2Velocity() { + if (hasLaunch2()) { + return launcher2.getVelocity(); + } else { + return Double.NaN; // Not a Number + } + } + + // This is wrong (and, as it turns out, unused), so I just commented it out + // TODO: We should discuss how to accomplish what this is attempting to do + /* + public void VelocityShoot() { + if ( + getMotor1Velocity() == targetLaunchVelocity && + getMotor2Velocity() == targetLaunchVelocity + ) { + // This doesn't do anything. It *creates* a command, but the command is never + // scheduled, so it won't ever actually do anything. + TeleCommands.GateDown(robot); + } + } + */ + + // TOOD: Turn this into a quadratic equation instead, since that's what it's mimicking. + public double autoVelocity() { + // x = distance in inches + double x = getTargetDistance(); + + if (x < 0) { + return lastAutoVelocity; + } else if (x < 100) { + // launcherPI.p = 0.0015; + // launcherPI.i = 0; + lastAutoVelocity = Config.REGRESSION_A * x + Config.REGRESSION_B; + return lastAutoVelocity; + } else { + // NOTE: These two lines don't appear to do anything, because we're not using the + // Near_P and Near_I values anywhere (they were commented out above) + // TODO: Assigning values to something in Config is 'bad form'. Instead, we should + // have the coefficients copied into the Launcher itself in the constructor, and change + // that here. + Config.launcherPI.p = Config.Far_P; + Config.launcherPI.i = Config.Far_I; + return Config.REGRESSION_C * x + Config.REGRESSION_D; + } + + //return ((RPM_PER_FOOT * ls.getDistance()) / 12 + MINIMUM_VELOCITY) + addtionamount; + } + + public void setRegressionAuto() { + // Spin the motors pid goes here + Config.REGRESSION_C = Config.REGRESSION_C_AUTO; + Config.REGRESSION_D = Config.REGRESSION_D_AUTO; + } + + public void setRegressionTeleop() { + // Spin the motors pid goes here + Config.REGRESSION_C = Config.REGRESSION_C_TELEOP; + Config.REGRESSION_D = Config.REGRESSION_D_TELEOP; + } + + public void increaseRegressionDTeleop() { + // Spin the motors pid goes here + Config.REGRESSION_D += 15; + } + + // This lets the 'no hardware' or 'subsystem disabled' thing still work without a functional + // target acquisition subsystem + private double getTargetDistance() { + if (targetAcquisition != null) { + return targetAcquisition.getDistance(); + } + return -1; + } + + @Override + public void periodic() { + autoVelocity = autoVelocity(); + currentLaunchVelocity = readVelocity(); + if (launcherPID.getTarget() != 0) { + setMotorPower(launcherPID.update(getMotorSpeed())); + } else { + setMotorPower(0); + launcherPID.update(getMotorSpeed()); + launcherPID.reset(); + } + + err = launcherPID.getLastError(); + motorVelocity = getMotorSpeed(); + if (hasLaunch1()) { + power = launcher1.getPower(); + } else { + power = Double.NaN; + } + launcher1Current = getMotor1Current(); + launcher2Current = getMotor2Current(); + } + + // This should be used by a test opmode to check that the basics are working. + public String hardwareValidation(double power1, double power2) { + String res = ""; + if (hasLaunch1()) { + launcher1.setPower(power1); + } else { + res += "(no launcher1)"; + } + if (hasLaunch2()) { + launcher2.setPower(power2); + } else { + res += "(no launcher2)"; + } + res += String.format( + Locale.ENGLISH, + "Launcher Speed: %.2f, Current1: %.2f, Current2: %.2f", + getMotorSpeed(), + getMotor1Current(), + getMotor2Current() + ); + return res; + } + + // TODO: Code that measures kStaticFriction and kVelocityConstant for a FeedFwd function + public void feedFwdHelper(Telemetry tel, Gamepad gamepad) { + // Wait until a button's been pressed, then first identify kStaticFriction, by watching to + // see when the motor just barely starts moving (then back off by a couple percent) + motorHelperPower(0); + while (!anyButtonsPressed(gamepad)) { + tel.addLine("Please press a button on the gamepad to begin test"); + tel.update(); + } + while (anyButtonsPressed(gamepad)) { + tel.addLine("Release the button to begin the test"); + tel.update(); + } + // Okay, slowly raise the power applied to launcher1 until w + double staticFriction = 0.001; + double staticFrictionStep = 0.001; + ElapsedTime lastUpdate = new ElapsedTime(); + while (true) { + double v = voltage.getAsDouble(); + motorHelperPower(staticFriction / v); + tel.addLine("Press a button to abort"); + tel.addData("kStaticFriction", staticFriction); + tel.addData("Voltage", v); + tel.update(); + if (anyButtonsPressed(gamepad)) { + motorHelperPower(0); + return; + } + if (lastUpdate.milliseconds() >= 50) { + // We update every 25 milliseconds, just to give it time to trigger + if (getMotor1Velocity() != 0) { + break; + } + lastUpdate.reset(); + staticFriction += staticFrictionStep; + } + } + // If we're here, the system started moving. Stop the motors and set kStaticFriction to + // just below what was necessary to start the system moving. + staticFriction -= staticFrictionStep; + motorHelperPower(0); + // Display the kStaticFriction value we got: + while (!anyButtonsPressed(gamepad)) { + tel.addData("kStaticFriction-->>", staticFriction); + tel.addLine("Press a button to start the velocity measurement"); + tel.update(); + } + // Next up: Velocity! + while (anyButtonsPressed(gamepad)) { + tel.addData("kStaticFriction!", staticFriction); + tel.addLine("Release the button to start the velocity measurement"); + tel.update(); + } + lastUpdate.reset(); + double velocityConstant = 0; + MovingStatistics velocityConstantStats = new MovingStatistics(50); + while (!anyButtonsPressed(gamepad)) { + motorHelperPower(1); + if (lastUpdate.milliseconds() >= 50) { + double vel = getMotor1Velocity(); + double vol = voltage.getAsDouble(); + if (vel != 0) { + // power = (kStaticFriction + kVelocity * RPM) / v + // So + // 1 = (kSF + kV * RPM) / v; + // Multiply both sides by v + // v = kSF + kV * RPM + // move kStaticFriction over: + // v - KSF = kV * RPM + // swap sides + // kv * RPM = v - kSF + // and divide both sides by RPM + // kv = (v - kSF) / RPM + velocityConstant = (vol - staticFriction) / vel; + velocityConstantStats.add(velocityConstant); + } + } + tel.addData("kStaticFriction!", staticFriction); + tel.addLine("Press a button to stop velocity measurement"); + tel.addData("kVelocityConstant", velocityConstant); + tel.addData("Average kV", velocityConstantStats.getMean()); + tel.update(); + } + } + + private static boolean anyButtonsPressed(Gamepad g) { + return ( + g.a || + g.b || + g.x || + g.y || + g.dpad_down || + g.dpad_up || + g.dpad_left || + g.dpad_right || + g.options || + g.share || + g.left_bumper || + g.right_bumper || + g.start + ); + } + + private void motorHelperPower(double p) { + if (hasLaunch1()) { + launcher1.setPower(p); + } + if (hasLaunch2()) { + launcher2.setPower(p); + } + } + + private boolean hasLaunch1() { + return launcher1 != null; + } + + private boolean hasLaunch2() { + return launcher2 != null; + } +} diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/controls/DriverController.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/controls/DriverController.java index 82326e7a..bbf36409 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/controls/DriverController.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/controls/DriverController.java @@ -8,12 +8,10 @@ import org.firstinspires.ftc.sixteen750.Hardware; import org.firstinspires.ftc.sixteen750.Robot; import org.firstinspires.ftc.sixteen750.Setup; -import org.firstinspires.ftc.sixteen750.commands.AltAutoOrient; import org.firstinspires.ftc.sixteen750.commands.PedroDriver; import org.firstinspires.ftc.sixteen750.commands.TeleCommands; -import org.firstinspires.ftc.sixteen750.commands.auto.Paths; import org.firstinspires.ftc.sixteen750.commands.driving.DrivingCommands; -import org.firstinspires.ftc.sixteen750.subsystems.LimelightSubsystem; +import org.firstinspires.ftc.sixteen750.component.LauncherComponent.LauncherCommand; public class DriverController { @@ -60,7 +58,7 @@ public DriverController(CommandGamepad g, Robot r) { if (Setup.Connected.DRIVEBASE) { bindDriveControls(); } - if (Setup.Connected.LAUNCHERSUBSYSTEM) { + if (Setup.Connected.LAUNCHERCOMPONENT) { bindLaunchControls(); } if (Setup.Connected.INTAKESUBSYSTEM) { @@ -129,13 +127,13 @@ public void bindDriveControls() { } public void bindLaunchControls() { - launchButton.whilePressed(TeleCommands.Launch(robot)); - launchButton.whenReleased(TeleCommands.StopLaunch(robot)); - // CloseShoot.whenPressed(TeleCommands.SetCloseShoot(robot)); - // FarShoot.whenPressed(TeleCommands.SetFarShoot(robot)); - MotorIncrease.whenPressed(robot.launcherSubsystem::IncreaseMotorVelocity); - MotorDecrease.whenPressed(robot.launcherSubsystem::DecreaseMotorVelocity); - increaseD.whenPressed(robot.launcherSubsystem::increaseRegressionDTeleop); + launchButton.whilePressed(LauncherCommand.Launch()); + launchButton.whenReleased(LauncherCommand.StopLaunch()); + // CloseShoot.whenPressed(LauncherCommand.SetCloseShoot()); + // FarShoot.whenPressed(LauncherCommand.SetFarShoot()); + MotorIncrease.whenPressed(LauncherCommand.IncreaseMotor()); + MotorDecrease.whenPressed(LauncherCommand.DecreaseMotor()); + increaseD.whenPressed(LauncherCommand.IncreaseRegressionDTeleop()); } public void bindIntakeControls() { diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/DualTeleOp.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/DualTeleOp.java index 5b8efe57..71d60805 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/DualTeleOp.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/DualTeleOp.java @@ -3,31 +3,22 @@ import static org.firstinspires.ftc.sixteen750.Setup.HardwareNames.AprilTag_Pipeline; import com.bylazar.telemetry.PanelsTelemetry; -import com.qualcomm.hardware.limelightvision.LLResult; -import com.qualcomm.hardware.limelightvision.LLResultTypes; -import com.qualcomm.hardware.limelightvision.LLStatus; import com.qualcomm.hardware.limelightvision.Limelight3A; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import com.technototes.library.command.Command; import com.technototes.library.command.CommandScheduler; import com.technototes.library.command.SequentialCommandGroup; import com.technototes.library.structure.CommandOpMode; import com.technototes.library.util.Alliance; -import java.util.Arrays; -import java.util.List; -import org.firstinspires.ftc.robotcore.external.navigation.Pose3D; import org.firstinspires.ftc.sixteen750.Hardware; import org.firstinspires.ftc.sixteen750.Robot; import org.firstinspires.ftc.sixteen750.Setup; -import org.firstinspires.ftc.sixteen750.commands.TeleCommands; import org.firstinspires.ftc.sixteen750.commands.auto.Paths; import org.firstinspires.ftc.sixteen750.commands.driving.DrivingCommands; +import org.firstinspires.ftc.sixteen750.component.LauncherComponent.LauncherCommand; import org.firstinspires.ftc.sixteen750.controls.DriverController; import org.firstinspires.ftc.sixteen750.controls.OperatorController; import org.firstinspires.ftc.sixteen750.helpers.HeadingHelper; import org.firstinspires.ftc.sixteen750.helpers.StartingPosition; -import org.firstinspires.ftc.sixteen750.subsystems.LauncherSubsystem; -import org.firstinspires.ftc.sixteen750.subsystems.LimelightSubsystem; @TeleOp(name = "Dual Control") @SuppressWarnings("unused") @@ -56,8 +47,7 @@ public void uponInit() { new SequentialCommandGroup( HeadingHelper.RestorePreviousPosition(robot.follower), DrivingCommands.ResetGyro(controlsDriver.pedroDriver), - TeleCommands.SetRegressionCTeleop(robot), - TeleCommands.SetRegressionDTeleop(robot) + LauncherCommand.SetRegressionTeleop() ), OpModeState.INIT ); @@ -80,9 +70,6 @@ public void uponInit() { */ limelight.start(); } - if (Setup.Connected.LAUNCHERSUBSYSTEM) { - CommandScheduler.register(robot.launcherSubsystem); - } } @Override @@ -97,20 +84,20 @@ public void runLoop() { .getTelemetry() .addData( "currentLaunchVelocity", - String.valueOf(LauncherSubsystem.currentLaunchVelocity) + String.valueOf(LauncherComponent.currentLaunchVelocity) ); panelsTelemetry .getTelemetry() - .addData("launcherError", String.valueOf(LauncherSubsystem.err)); + .addData("launcherError", String.valueOf(LauncherComponent.err)); panelsTelemetry .getTelemetry() - .addData("launcherTargetVelocity", String.valueOf(LauncherSubsystem.targetSpeed)); + .addData("launcherTargetVelocity", String.valueOf(LauncherComponent.targetSpeed)); panelsTelemetry .getTelemetry() - .addData("launcher1Current", String.valueOf(LauncherSubsystem.launcher1Current)); + .addData("launcher1Current", String.valueOf(LauncherComponent.launcher1Current)); panelsTelemetry .getTelemetry() - .addData("launcher2Current", String.valueOf(LauncherSubsystem.launcher2Current)); + .addData("launcher2Current", String.valueOf(LauncherComponent.launcher2Current)); panelsTelemetry.getTelemetry().update(telemetry); } */ diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/Blue12BallFar.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/Blue12BallFar.java index f24e7ff2..8366d21a 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/Blue12BallFar.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/Blue12BallFar.java @@ -12,6 +12,7 @@ import org.firstinspires.ftc.sixteen750.commands.PedroPathCommand; import org.firstinspires.ftc.sixteen750.commands.TeleCommands; import org.firstinspires.ftc.sixteen750.commands.auto.Paths; +import org.firstinspires.ftc.sixteen750.component.LauncherComponent.LauncherCommand; import org.firstinspires.ftc.sixteen750.controls.DriverController; import org.firstinspires.ftc.sixteen750.helpers.HeadingHelper; import org.firstinspires.ftc.sixteen750.helpers.StartingPosition; @@ -33,12 +34,11 @@ public void uponInit() { CommandScheduler.register(robot.limelightSubsystem); CommandScheduler.scheduleForState( new SequentialCommandGroup( - TeleCommands.SetRegressionCAuto(robot), - TeleCommands.SetRegressionDAuto(robot), + LauncherCommand.SetRegressionAuto(), new LLSetup(robot), TeleCommands.GateUp(robot), - TeleCommands.SetFarShoot(robot), - TeleCommands.FarAutoLaunch(robot), + LauncherCommand.SetFarShoot(), + LauncherCommand.FarAutoLaunch(), TeleCommands.HoldIntake(robot), TeleCommands.HoodUp(robot), new PedroPathCommand(robot.follower, p.StartFartolaunchfar), @@ -76,7 +76,7 @@ public void uponInit() { TeleCommands.Intake(robot) ), new WaitCommand(1), - TeleCommands.StopLaunch(robot), + LauncherCommand.StopLaunch(), TeleCommands.IntakeStop(robot), CommandScheduler::terminateOpMode ), diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/Blue9BallFar.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/Blue9BallFar.java index 04e868ec..42dd146b 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/Blue9BallFar.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/Blue9BallFar.java @@ -11,6 +11,7 @@ import org.firstinspires.ftc.sixteen750.commands.PedroPathCommand; import org.firstinspires.ftc.sixteen750.commands.TeleCommands; import org.firstinspires.ftc.sixteen750.commands.auto.Paths; +import org.firstinspires.ftc.sixteen750.component.LauncherComponent.LauncherCommand; import org.firstinspires.ftc.sixteen750.controls.DriverController; import org.firstinspires.ftc.sixteen750.helpers.HeadingHelper; import org.firstinspires.ftc.sixteen750.helpers.StartingPosition; @@ -32,8 +33,8 @@ public void uponInit() { CommandScheduler.scheduleForState( new SequentialCommandGroup( TeleCommands.GateUp(robot), - TeleCommands.SetFarShoot(robot), - TeleCommands.Launch(robot), + LauncherCommand.SetFarShoot(), + LauncherCommand.Launch(), TeleCommands.Intake(robot), TeleCommands.HoodUp(robot), new PedroPathCommand(robot.follower, p.StartFartolaunchfar), @@ -55,7 +56,7 @@ public void uponInit() { Paths.AutoLaunching3Balls(robot), new PedroPathCommand(robot.follower, p.launchfartopark), new WaitCommand(1), - TeleCommands.StopLaunch(robot), + LauncherCommand.StopLaunch(), TeleCommands.IntakeStop(robot), CommandScheduler::terminateOpMode ), diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/Blue9BallFarNew.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/Blue9BallFarNew.java index 6b9d8677..bb007dd6 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/Blue9BallFarNew.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/Blue9BallFarNew.java @@ -12,6 +12,7 @@ import org.firstinspires.ftc.sixteen750.commands.PedroPathCommand; import org.firstinspires.ftc.sixteen750.commands.TeleCommands; import org.firstinspires.ftc.sixteen750.commands.auto.Paths; +import org.firstinspires.ftc.sixteen750.component.LauncherComponent.LauncherCommand; import org.firstinspires.ftc.sixteen750.controls.DriverController; import org.firstinspires.ftc.sixteen750.helpers.HeadingHelper; import org.firstinspires.ftc.sixteen750.helpers.StartingPosition; @@ -33,12 +34,11 @@ public void uponInit() { CommandScheduler.register(robot.limelightSubsystem); CommandScheduler.scheduleForState( new SequentialCommandGroup( - TeleCommands.SetRegressionCAuto(robot), - TeleCommands.SetRegressionDAuto(robot), + LauncherCommand.SetRegressionAuto(), new LLSetup(robot), TeleCommands.GateUp(robot), - TeleCommands.SetFarShoot(robot), - TeleCommands.FarAutoLaunch(robot), + LauncherCommand.SetFarShoot(), + LauncherCommand.FarAutoLaunch(), TeleCommands.HoldIntake(robot), TeleCommands.HoodUp(robot), new PedroPathCommand(robot.follower, p.StartFartolaunchfar), @@ -73,7 +73,7 @@ public void uponInit() { TeleCommands.Intake(robot) ), new WaitCommand(1), - TeleCommands.StopLaunch(robot), + LauncherCommand.StopLaunch(), TeleCommands.IntakeStop(robot), CommandScheduler::terminateOpMode ), diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/BlueNearFirstandSecondLever.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/BlueNearFirstandSecondLever.java index 7d98e915..240fad0a 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/BlueNearFirstandSecondLever.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/BlueNearFirstandSecondLever.java @@ -13,14 +13,14 @@ import org.firstinspires.ftc.sixteen750.Hardware; import org.firstinspires.ftc.sixteen750.Robot; import org.firstinspires.ftc.sixteen750.Setup; -import org.firstinspires.ftc.sixteen750.commands.AltAutoVelocity; import org.firstinspires.ftc.sixteen750.commands.PedroPathCommand; import org.firstinspires.ftc.sixteen750.commands.TeleCommands; import org.firstinspires.ftc.sixteen750.commands.auto.Paths; +import org.firstinspires.ftc.sixteen750.component.LauncherComponent; +import org.firstinspires.ftc.sixteen750.component.LauncherComponent.LauncherCommand; import org.firstinspires.ftc.sixteen750.controls.DriverController; import org.firstinspires.ftc.sixteen750.helpers.HeadingHelper; import org.firstinspires.ftc.sixteen750.helpers.StartingPosition; -import org.firstinspires.ftc.sixteen750.subsystems.LauncherSubsystem; @Autonomous(name = "BlueNearLever1️⃣2️⃣", preselectTeleOp = "Dual Control") @SuppressWarnings("unused") @@ -40,7 +40,7 @@ public void uponInit() { panelsTelemetry = PanelsTelemetry.INSTANCE; robot.follower.setStartingPose(p.getBSegmentedCurveStart()); CommandScheduler.scheduleForState( - new AltAutoVelocity(robot).alongWith( + LauncherCommand.AutoVelocity().alongWith( new SequentialCommandGroup( //TeleCommands.AutoLaunch1(robot), TeleCommands.GateUp(robot), @@ -106,9 +106,6 @@ public void uponInit() { */ limelight.start(); } - if (Setup.Connected.LAUNCHERSUBSYSTEM) { - CommandScheduler.register(robot.launcherSubsystem); - } } public void uponStart() { @@ -120,20 +117,20 @@ public void runLoop() { .getTelemetry() .addData( "currentLaunchVelocity", - String.valueOf(LauncherSubsystem.currentLaunchVelocity) + String.valueOf(LauncherComponent.currentLaunchVelocity) ); panelsTelemetry .getTelemetry() - .addData("launcherError", String.valueOf(LauncherSubsystem.err)); + .addData("launcherError", String.valueOf(LauncherComponent.err)); panelsTelemetry .getTelemetry() - .addData("launcherTargetVelocity", String.valueOf(LauncherSubsystem.targetSpeed)); + .addData("launcherTargetVelocity", String.valueOf(LauncherComponent.targetSpeed)); panelsTelemetry .getTelemetry() - .addData("launcher1Current", String.valueOf(LauncherSubsystem.launcher1Current)); + .addData("launcher1Current", String.valueOf(LauncherComponent.launcher1Current)); panelsTelemetry .getTelemetry() - .addData("launcher2Current", String.valueOf(LauncherSubsystem.launcher2Current)); + .addData("launcher2Current", String.valueOf(LauncherComponent.launcher2Current)); panelsTelemetry.getTelemetry().update(telemetry); } diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/BlueNearLever.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/BlueNearLever.java index 06e1c039..29773916 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/BlueNearLever.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/BlueNearLever.java @@ -8,10 +8,10 @@ import com.technototes.library.util.Alliance; import org.firstinspires.ftc.sixteen750.Hardware; import org.firstinspires.ftc.sixteen750.Robot; -import org.firstinspires.ftc.sixteen750.commands.AltAutoVelocity; import org.firstinspires.ftc.sixteen750.commands.PedroPathCommand; import org.firstinspires.ftc.sixteen750.commands.TeleCommands; import org.firstinspires.ftc.sixteen750.commands.auto.Paths; +import org.firstinspires.ftc.sixteen750.component.LauncherComponent.LauncherCommand; import org.firstinspires.ftc.sixteen750.controls.DriverController; import org.firstinspires.ftc.sixteen750.helpers.HeadingHelper; import org.firstinspires.ftc.sixteen750.helpers.StartingPosition; @@ -31,7 +31,7 @@ public void uponInit() { Paths p = new Paths(robot.follower); robot.follower.setStartingPose(p.getBSegmentedCurveStart()); CommandScheduler.scheduleForState( - new AltAutoVelocity(robot).alongWith( + LauncherCommand.AutoVelocity().alongWith( new SequentialCommandGroup( //TeleCommands.AutoLaunch1(robot), TeleCommands.GateUp(robot), diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/BlueNearLeverNoThird.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/BlueNearLeverNoThird.java index a14a0ea5..28585e1d 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/BlueNearLeverNoThird.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/BlueNearLeverNoThird.java @@ -8,10 +8,10 @@ import com.technototes.library.util.Alliance; import org.firstinspires.ftc.sixteen750.Hardware; import org.firstinspires.ftc.sixteen750.Robot; -import org.firstinspires.ftc.sixteen750.commands.AltAutoVelocity; import org.firstinspires.ftc.sixteen750.commands.PedroPathCommand; import org.firstinspires.ftc.sixteen750.commands.TeleCommands; import org.firstinspires.ftc.sixteen750.commands.auto.Paths; +import org.firstinspires.ftc.sixteen750.component.LauncherComponent.LauncherCommand; import org.firstinspires.ftc.sixteen750.controls.DriverController; import org.firstinspires.ftc.sixteen750.helpers.HeadingHelper; import org.firstinspires.ftc.sixteen750.helpers.StartingPosition; @@ -31,7 +31,7 @@ public void uponInit() { Paths p = new Paths(robot.follower); robot.follower.setStartingPose(p.getBSegmentedCurveStart()); CommandScheduler.scheduleForState( - new AltAutoVelocity(robot).alongWith( + LauncherCommand.AutoVelocity().alongWith( new SequentialCommandGroup( //TeleCommands.AutoLaunch1(robot), TeleCommands.GateUp(robot), diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/BlueNearSecondLever.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/BlueNearSecondLever.java index a6e8f38b..86624bda 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/BlueNearSecondLever.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/BlueNearSecondLever.java @@ -8,10 +8,10 @@ import com.technototes.library.util.Alliance; import org.firstinspires.ftc.sixteen750.Hardware; import org.firstinspires.ftc.sixteen750.Robot; -import org.firstinspires.ftc.sixteen750.commands.AltAutoVelocity; import org.firstinspires.ftc.sixteen750.commands.PedroPathCommand; import org.firstinspires.ftc.sixteen750.commands.TeleCommands; import org.firstinspires.ftc.sixteen750.commands.auto.Paths; +import org.firstinspires.ftc.sixteen750.component.LauncherComponent.LauncherCommand; import org.firstinspires.ftc.sixteen750.controls.DriverController; import org.firstinspires.ftc.sixteen750.helpers.HeadingHelper; import org.firstinspires.ftc.sixteen750.helpers.StartingPosition; @@ -31,7 +31,7 @@ public void uponInit() { Paths p = new Paths(robot.follower); robot.follower.setStartingPose(p.getBSegmentedCurveStart()); CommandScheduler.scheduleForState( - new AltAutoVelocity(robot).alongWith( + LauncherCommand.AutoVelocity().alongWith( new SequentialCommandGroup( //TeleCommands.AutoLaunch1(robot), TeleCommands.GateUp(robot), diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/FarZoneDriveForward.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/FarZoneDriveForward.java index fe1b49f8..aaabd232 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/FarZoneDriveForward.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/FarZoneDriveForward.java @@ -13,6 +13,7 @@ import org.firstinspires.ftc.sixteen750.commands.TeleCommands; import org.firstinspires.ftc.sixteen750.commands.auto.DriveAutoCommand; import org.firstinspires.ftc.sixteen750.commands.auto.Paths; +import org.firstinspires.ftc.sixteen750.component.LauncherComponent.LauncherCommand; import org.firstinspires.ftc.sixteen750.controls.DriverController; import org.firstinspires.ftc.sixteen750.helpers.HeadingHelper; import org.firstinspires.ftc.sixteen750.helpers.StartingPosition; @@ -38,12 +39,12 @@ public void uponInit() { CommandScheduler.scheduleForState( new SequentialCommandGroup( new LLSetup(robot), - TeleCommands.SetFarShoot(robot), - TeleCommands.Launch(robot), + LauncherCommand.SetFarShoot(), + LauncherCommand.Launch(), TeleCommands.HoodUp(robot), new WaitCommand(2), Paths.AutoLaunching3Balls(robot), - TeleCommands.StopLaunch(robot), + LauncherCommand.StopLaunch(), TeleCommands.IntakeStop(robot), new WaitCommand(18), new DriveAutoCommand(robot.follower, 0.5), diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/Red12BallFar.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/Red12BallFar.java index b68eec7e..a268eb53 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/Red12BallFar.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/Red12BallFar.java @@ -9,11 +9,11 @@ import com.technototes.library.util.Alliance; import org.firstinspires.ftc.sixteen750.Hardware; import org.firstinspires.ftc.sixteen750.Robot; -import org.firstinspires.ftc.sixteen750.commands.AltAutoVelocity; import org.firstinspires.ftc.sixteen750.commands.LLSetup; import org.firstinspires.ftc.sixteen750.commands.PedroPathCommand; import org.firstinspires.ftc.sixteen750.commands.TeleCommands; import org.firstinspires.ftc.sixteen750.commands.auto.Paths; +import org.firstinspires.ftc.sixteen750.component.LauncherComponent.LauncherCommand; import org.firstinspires.ftc.sixteen750.controls.DriverController; import org.firstinspires.ftc.sixteen750.helpers.HeadingHelper; import org.firstinspires.ftc.sixteen750.helpers.StartingPosition; @@ -39,13 +39,12 @@ public void uponInit() { //new AltAutoVelocity(robot).alongWith( new SequentialCommandGroup( new LLSetup(robot), - TeleCommands.SetRegressionCAuto(robot), - TeleCommands.SetRegressionDAuto(robot), + LauncherCommand.SetRegressionAuto(), TeleCommands.GateUp(robot), TeleCommands.Intake(robot), - TeleCommands.SetFarShoot(robot), + LauncherCommand.SetFarShoot(), TeleCommands.HoldIntake(robot), - TeleCommands.FarAutoLaunch(robot), + LauncherCommand.FarAutoLaunch(), TeleCommands.HoodUp(robot), new PedroPathCommand(robot.follower, p.RStartFartolaunchfar), new WaitCommand(0.5), diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/Red9BallFar.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/Red9BallFar.java index c67cce5d..f56e4a4e 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/Red9BallFar.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/Red9BallFar.java @@ -12,6 +12,7 @@ import org.firstinspires.ftc.sixteen750.commands.PedroPathCommand; import org.firstinspires.ftc.sixteen750.commands.TeleCommands; import org.firstinspires.ftc.sixteen750.commands.auto.Paths; +import org.firstinspires.ftc.sixteen750.component.LauncherComponent.LauncherCommand; import org.firstinspires.ftc.sixteen750.controls.DriverController; import org.firstinspires.ftc.sixteen750.helpers.HeadingHelper; import org.firstinspires.ftc.sixteen750.helpers.StartingPosition; @@ -35,8 +36,8 @@ public void uponInit() { new SequentialCommandGroup( new LLSetup(robot), TeleCommands.GateUp(robot), - TeleCommands.SetFarShoot(robot), - TeleCommands.Launch(robot), + LauncherCommand.SetFarShoot(), + LauncherCommand.Launch(), TeleCommands.Intake(robot), TeleCommands.HoodUp(robot), new PedroPathCommand(robot.follower, p.RStartFartolaunchfar), @@ -57,7 +58,7 @@ public void uponInit() { Paths.AutoLaunching3Balls(robot), new PedroPathCommand(robot.follower, p.Rlaunchfartopark), new WaitCommand(1), - TeleCommands.StopLaunch(robot), + LauncherCommand.StopLaunch(), TeleCommands.IntakeStop(robot), CommandScheduler::terminateOpMode ), diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/RedNearFirstandSecondLever.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/RedNearFirstandSecondLever.java index 149c9f9b..afd69791 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/RedNearFirstandSecondLever.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/RedNearFirstandSecondLever.java @@ -13,14 +13,14 @@ import org.firstinspires.ftc.sixteen750.Hardware; import org.firstinspires.ftc.sixteen750.Robot; import org.firstinspires.ftc.sixteen750.Setup; -import org.firstinspires.ftc.sixteen750.commands.AltAutoVelocity; import org.firstinspires.ftc.sixteen750.commands.PedroPathCommand; import org.firstinspires.ftc.sixteen750.commands.TeleCommands; import org.firstinspires.ftc.sixteen750.commands.auto.Paths; +import org.firstinspires.ftc.sixteen750.component.LauncherComponent; +import org.firstinspires.ftc.sixteen750.component.LauncherComponent.LauncherCommand; import org.firstinspires.ftc.sixteen750.controls.DriverController; import org.firstinspires.ftc.sixteen750.helpers.HeadingHelper; import org.firstinspires.ftc.sixteen750.helpers.StartingPosition; -import org.firstinspires.ftc.sixteen750.subsystems.LauncherSubsystem; @Autonomous(name = "RedNearLever1️⃣2️⃣", preselectTeleOp = "Dual Control") @SuppressWarnings("unused") @@ -40,7 +40,7 @@ public void uponInit() { panelsTelemetry = PanelsTelemetry.INSTANCE; robot.follower.setStartingPose(p.getRSegmentedCurveStart()); CommandScheduler.scheduleForState( - new AltAutoVelocity(robot).alongWith( + LauncherCommand.AutoVelocity().alongWith( new SequentialCommandGroup( //TeleCommands.AutoLaunch1(robot), TeleCommands.GateUp(robot), @@ -106,9 +106,6 @@ public void uponInit() { */ limelight.start(); } - if (Setup.Connected.LAUNCHERSUBSYSTEM) { - CommandScheduler.register(robot.launcherSubsystem); - } } public void uponStart() { @@ -120,20 +117,20 @@ public void runLoop() { .getTelemetry() .addData( "currentLaunchVelocity", - String.valueOf(LauncherSubsystem.currentLaunchVelocity) + String.valueOf(LauncherComponent.currentLaunchVelocity) ); panelsTelemetry .getTelemetry() - .addData("launcherError", String.valueOf(LauncherSubsystem.err)); + .addData("launcherError", String.valueOf(LauncherComponent.err)); panelsTelemetry .getTelemetry() - .addData("launcherTargetVelocity", String.valueOf(LauncherSubsystem.targetSpeed)); + .addData("launcherTargetVelocity", String.valueOf(LauncherComponent.targetSpeed)); panelsTelemetry .getTelemetry() - .addData("launcher1Current", String.valueOf(LauncherSubsystem.launcher1Current)); + .addData("launcher1Current", String.valueOf(LauncherComponent.launcher1Current)); panelsTelemetry .getTelemetry() - .addData("launcher2Current", String.valueOf(LauncherSubsystem.launcher2Current)); + .addData("launcher2Current", String.valueOf(LauncherComponent.launcher2Current)); panelsTelemetry.getTelemetry().update(telemetry); } diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/RedNearLever.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/RedNearLever.java index 034fb350..1ce88804 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/RedNearLever.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/RedNearLever.java @@ -13,14 +13,14 @@ import org.firstinspires.ftc.sixteen750.Hardware; import org.firstinspires.ftc.sixteen750.Robot; import org.firstinspires.ftc.sixteen750.Setup; -import org.firstinspires.ftc.sixteen750.commands.AltAutoVelocity; import org.firstinspires.ftc.sixteen750.commands.PedroPathCommand; import org.firstinspires.ftc.sixteen750.commands.TeleCommands; import org.firstinspires.ftc.sixteen750.commands.auto.Paths; +import org.firstinspires.ftc.sixteen750.component.LauncherComponent; +import org.firstinspires.ftc.sixteen750.component.LauncherComponent.LauncherCommand; import org.firstinspires.ftc.sixteen750.controls.DriverController; import org.firstinspires.ftc.sixteen750.helpers.HeadingHelper; import org.firstinspires.ftc.sixteen750.helpers.StartingPosition; -import org.firstinspires.ftc.sixteen750.subsystems.LauncherSubsystem; @Autonomous(name = "RedNearLever1️⃣", preselectTeleOp = "Dual Control") @SuppressWarnings("unused") @@ -40,7 +40,7 @@ public void uponInit() { panelsTelemetry = PanelsTelemetry.INSTANCE; robot.follower.setStartingPose(p.getRSegmentedCurveStart()); CommandScheduler.scheduleForState( - new AltAutoVelocity(robot).alongWith( + LauncherCommand.AutoVelocity().alongWith( new SequentialCommandGroup( //TeleCommands.AutoLaunch1(robot), TeleCommands.GateUp(robot), @@ -103,9 +103,6 @@ public void uponInit() { */ limelight.start(); } - if (Setup.Connected.LAUNCHERSUBSYSTEM) { - CommandScheduler.register(robot.launcherSubsystem); - } } public void uponStart() { @@ -117,20 +114,20 @@ public void runLoop() { .getTelemetry() .addData( "currentLaunchVelocity", - String.valueOf(LauncherSubsystem.currentLaunchVelocity) + String.valueOf(LauncherComponent.currentLaunchVelocity) ); panelsTelemetry .getTelemetry() - .addData("launcherError", String.valueOf(LauncherSubsystem.err)); + .addData("launcherError", String.valueOf(LauncherComponent.err)); panelsTelemetry .getTelemetry() - .addData("launcherTargetVelocity", String.valueOf(LauncherSubsystem.targetSpeed)); + .addData("launcherTargetVelocity", String.valueOf(LauncherComponent.targetSpeed)); panelsTelemetry .getTelemetry() - .addData("launcher1Current", String.valueOf(LauncherSubsystem.launcher1Current)); + .addData("launcher1Current", String.valueOf(LauncherComponent.launcher1Current)); panelsTelemetry .getTelemetry() - .addData("launcher2Current", String.valueOf(LauncherSubsystem.launcher2Current)); + .addData("launcher2Current", String.valueOf(LauncherComponent.launcher2Current)); panelsTelemetry.getTelemetry().update(telemetry); } diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/RedNearLeverNoThird.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/RedNearLeverNoThird.java index 0dbaafe6..7ccee326 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/RedNearLeverNoThird.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/RedNearLeverNoThird.java @@ -13,14 +13,14 @@ import org.firstinspires.ftc.sixteen750.Hardware; import org.firstinspires.ftc.sixteen750.Robot; import org.firstinspires.ftc.sixteen750.Setup; -import org.firstinspires.ftc.sixteen750.commands.AltAutoVelocity; import org.firstinspires.ftc.sixteen750.commands.PedroPathCommand; import org.firstinspires.ftc.sixteen750.commands.TeleCommands; import org.firstinspires.ftc.sixteen750.commands.auto.Paths; +import org.firstinspires.ftc.sixteen750.component.LauncherComponent; +import org.firstinspires.ftc.sixteen750.component.LauncherComponent.LauncherCommand; import org.firstinspires.ftc.sixteen750.controls.DriverController; import org.firstinspires.ftc.sixteen750.helpers.HeadingHelper; import org.firstinspires.ftc.sixteen750.helpers.StartingPosition; -import org.firstinspires.ftc.sixteen750.subsystems.LauncherSubsystem; @Autonomous(name = "RedNearLever1️⃣NoThird", preselectTeleOp = "Dual Control") @SuppressWarnings("unused") @@ -40,7 +40,7 @@ public void uponInit() { panelsTelemetry = PanelsTelemetry.INSTANCE; robot.follower.setStartingPose(p.getRSegmentedCurveStart()); CommandScheduler.scheduleForState( - new AltAutoVelocity(robot).alongWith( + LauncherCommand.AutoVelocity().alongWith( new SequentialCommandGroup( //TeleCommands.AutoLaunch1(robot), TeleCommands.GateUp(robot), @@ -103,9 +103,6 @@ public void uponInit() { */ limelight.start(); } - if (Setup.Connected.LAUNCHERSUBSYSTEM) { - CommandScheduler.register(robot.launcherSubsystem); - } } public void uponStart() { @@ -117,20 +114,20 @@ public void runLoop() { .getTelemetry() .addData( "currentLaunchVelocity", - String.valueOf(LauncherSubsystem.currentLaunchVelocity) + String.valueOf(LauncherComponent.currentLaunchVelocity) ); panelsTelemetry .getTelemetry() - .addData("launcherError", String.valueOf(LauncherSubsystem.err)); + .addData("launcherError", String.valueOf(LauncherComponent.err)); panelsTelemetry .getTelemetry() - .addData("launcherTargetVelocity", String.valueOf(LauncherSubsystem.targetSpeed)); + .addData("launcherTargetVelocity", String.valueOf(LauncherComponent.targetSpeed)); panelsTelemetry .getTelemetry() - .addData("launcher1Current", String.valueOf(LauncherSubsystem.launcher1Current)); + .addData("launcher1Current", String.valueOf(LauncherComponent.launcher1Current)); panelsTelemetry .getTelemetry() - .addData("launcher2Current", String.valueOf(LauncherSubsystem.launcher2Current)); + .addData("launcher2Current", String.valueOf(LauncherComponent.launcher2Current)); panelsTelemetry.getTelemetry().update(telemetry); } diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/RedNearSecondLever.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/RedNearSecondLever.java index ec629bad..b27a9612 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/RedNearSecondLever.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/RedNearSecondLever.java @@ -13,14 +13,14 @@ import org.firstinspires.ftc.sixteen750.Hardware; import org.firstinspires.ftc.sixteen750.Robot; import org.firstinspires.ftc.sixteen750.Setup; -import org.firstinspires.ftc.sixteen750.commands.AltAutoVelocity; import org.firstinspires.ftc.sixteen750.commands.PedroPathCommand; import org.firstinspires.ftc.sixteen750.commands.TeleCommands; import org.firstinspires.ftc.sixteen750.commands.auto.Paths; +import org.firstinspires.ftc.sixteen750.component.LauncherComponent; +import org.firstinspires.ftc.sixteen750.component.LauncherComponent.LauncherCommand; import org.firstinspires.ftc.sixteen750.controls.DriverController; import org.firstinspires.ftc.sixteen750.helpers.HeadingHelper; import org.firstinspires.ftc.sixteen750.helpers.StartingPosition; -import org.firstinspires.ftc.sixteen750.subsystems.LauncherSubsystem; @Autonomous(name = "RedNearLever2️⃣", preselectTeleOp = "Dual Control") @SuppressWarnings("unused") @@ -40,7 +40,7 @@ public void uponInit() { panelsTelemetry = PanelsTelemetry.INSTANCE; robot.follower.setStartingPose(p.getRSegmentedCurveStart()); CommandScheduler.scheduleForState( - new AltAutoVelocity(robot).alongWith( + LauncherCommand.AutoVelocity().alongWith( new SequentialCommandGroup( //TeleCommands.AutoLaunch1(robot), TeleCommands.GateUp(robot), @@ -103,9 +103,6 @@ public void uponInit() { */ limelight.start(); } - if (Setup.Connected.LAUNCHERSUBSYSTEM) { - CommandScheduler.register(robot.launcherSubsystem); - } } public void uponStart() { @@ -117,20 +114,20 @@ public void runLoop() { .getTelemetry() .addData( "currentLaunchVelocity", - String.valueOf(LauncherSubsystem.currentLaunchVelocity) + String.valueOf(LauncherComponent.currentLaunchVelocity) ); panelsTelemetry .getTelemetry() - .addData("launcherError", String.valueOf(LauncherSubsystem.err)); + .addData("launcherError", String.valueOf(LauncherComponent.err)); panelsTelemetry .getTelemetry() - .addData("launcherTargetVelocity", String.valueOf(LauncherSubsystem.targetSpeed)); + .addData("launcherTargetVelocity", String.valueOf(LauncherComponent.targetSpeed)); panelsTelemetry .getTelemetry() - .addData("launcher1Current", String.valueOf(LauncherSubsystem.launcher1Current)); + .addData("launcher1Current", String.valueOf(LauncherComponent.launcher1Current)); panelsTelemetry .getTelemetry() - .addData("launcher2Current", String.valueOf(LauncherSubsystem.launcher2Current)); + .addData("launcher2Current", String.valueOf(LauncherComponent.launcher2Current)); panelsTelemetry.getTelemetry().update(telemetry); } diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/TestOpmode.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/TestOpmode.java index 57c8bb0d..2ce0fd70 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/TestOpmode.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/TestOpmode.java @@ -4,15 +4,12 @@ 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; import com.technototes.library.util.Alliance; import org.firstinspires.ftc.sixteen750.Hardware; import org.firstinspires.ftc.sixteen750.Robot; import org.firstinspires.ftc.sixteen750.commands.AltAutoOrient; -import org.firstinspires.ftc.sixteen750.commands.AltAutoVelocity; import org.firstinspires.ftc.sixteen750.commands.LLSetup; -import org.firstinspires.ftc.sixteen750.commands.TeleCommands; import org.firstinspires.ftc.sixteen750.commands.auto.Paths; import org.firstinspires.ftc.sixteen750.controls.DriverController; import org.firstinspires.ftc.sixteen750.helpers.HeadingHelper; diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/subsystems/LauncherSubsystem.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/subsystems/LauncherSubsystem.java deleted file mode 100644 index 64d98636..00000000 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/subsystems/LauncherSubsystem.java +++ /dev/null @@ -1,369 +0,0 @@ -package org.firstinspires.ftc.sixteen750.subsystems; - -import com.bylazar.configurables.annotations.Configurable; -import com.qualcomm.robotcore.hardware.DcMotorEx; -import com.qualcomm.robotcore.hardware.DcMotorSimple; -import com.qualcomm.robotcore.hardware.PIDFCoefficients; -import com.technototes.library.command.CommandScheduler; -import com.technototes.library.hardware.motor.EncodedMotor; -import com.technototes.library.logger.Log; -import com.technototes.library.logger.Loggable; -import com.technototes.library.subsystem.Subsystem; -import com.technototes.library.util.PIDFController; -import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit; -import org.firstinspires.ftc.sixteen750.Hardware; -import org.firstinspires.ftc.sixteen750.Robot; -import org.firstinspires.ftc.sixteen750.Setup; -import org.firstinspires.ftc.sixteen750.commands.TeleCommands; - -@Configurable -public class LauncherSubsystem implements Loggable, Subsystem { - - // @Log.Number(name = "Motor Power") - // public static double MOTOR_POWER = 0.65; // 0.5 1.0 - @Log.Number(name = "Target Velocity") - public static double targetLaunchVelocity = 1150; - - public static double closetargetLaunchVelocity = 1400; - public static double fartargetLaunchVelocity = 1850; - public static double fartargetLaunchVelocityforAuto = 2300; - public static double targetLaunchVelocityforAuto1 = 1950; - public static double targetLaunchVelocityforAuto2 = 1850; - - @Log.Number(name = "Current Motor Velocity") - public static double currentLaunchVelocity = 0.0; - - public static double motorVelocity; - public static double additionAmount; - public static double additionDelta = 5; - - //@Log(name = "Launcher Power: ") - public static double power; - - //@Log(name = "Error") - public static double err; - public static double launcher1Current; - public static double launcher2Current; - - @Log(name = "Target Speed: ") - public static double targetSpeed; - - @Log(name = "Target Power: ") - public static double targetPower; - - public static PIDFCoefficients launcherPI = new PIDFCoefficients(0.004, 0.0002, 0.0, 0); - public static PIDFCoefficients launcherPI_ForAuto = new PIDFCoefficients( - 0.0015, - 0.0000, - 0.0, - 0 - ); //p = 0.004, i = 0.00020 - public static double SPIN_F_SCALE = 0.00021; - public static double SPIN_VOLT_COMP = 0.0216; - public static double DIFFERENCE = 0.0046; - public static double PEAK_VOLTAGE = 13; - private static PIDFController launcherPID; - public static double lastAutoVelocity = 0; - - boolean hasHardware; - public Robot robot; - public PIDFCoefficients launcherPIDF = new PIDFCoefficients(0, 0.0, 0.0, 0); - public PIDFController launcherPIDFController; - public static double FEEDFORWARD_COEFFICIENT = 0.0; - - public static double MINIMUM_VELOCITY = 1140; - public static double RPM_PER_FOOT = 62.3; - public static double REGRESSION_A = 6.261; // multiplier for x for close zone launch speed formula - public static double REGRESSION_B = 1550; // minimum velocity for close zone launch speed formula - public static double REGRESSION_C = 20; // multiplier for x for far zone launch speed formula - public static double REGRESSION_D = 320; // minimum velocity for far zone launch speed formula - 130, 255 - public static double REGRESSION_C_TELEOP = 20; // multiplier for x for far zone launch speed formula - public static double REGRESSION_D_TELEOP = 130; // minimum velocity for far zone launch speed formula - public static double REGRESSION_C_AUTO = 17; // multiplier for x for far zone launch speed formula - public static double REGRESSION_D_AUTO = 115; // minimum velocity for far zone launch speed formula - - @Log.Number(name = "AutoAim Velocity") - public static double autoVelocity; - - public double launcherPow; - // not tested just placeholder but should be used - EncodedMotor launcher1; - EncodedMotor launcher2; - LimelightSubsystem ls; - - // @Log(name = "Flywheel at Velocity") - // public static boolean ready; - - public LauncherSubsystem(Hardware h) { - hasHardware = Setup.Connected.LAUNCHERSUBSYSTEM; - // Do stuff in here - if (hasHardware) { - launcher1 = h.launcher1; - launcher2 = h.launcher2; - launcher1.setDirection(DcMotorSimple.Direction.REVERSE); - launcher2.setDirection(DcMotorSimple.Direction.FORWARD); - launcher1.coast(); - launcher2.coast(); - launcher1.setPIDFCoefficients(launcherPIDF); - launcher2.setPIDFCoefficients(launcherPIDF); - //ready = false; - ls = new LimelightSubsystem(h); - double ADDITION = (PEAK_VOLTAGE - h.voltage()); - if (ADDITION == 0) { - SPIN_VOLT_COMP = SPIN_VOLT_COMP + 0.001; - } else { - SPIN_VOLT_COMP = SPIN_VOLT_COMP + (ADDITION * DIFFERENCE); - } - launcherPID = new PIDFController(launcherPI, target -> - target == 0 - ? 0 - : (SPIN_F_SCALE * target) + - (SPIN_VOLT_COMP * Math.min(PEAK_VOLTAGE, h.voltage())) - ); - // top.setPIDFCoefficients(launcherP); - setTargetSpeed(0); - } else { - launcher1 = null; - launcher2 = null; - } - CommandScheduler.register(this); - } - - public void Launch() { - // Spin the motors pid goes here - if (hasHardware) { - setTargetSpeed(autoVelocity()); //change to auto aim velocity - // launcher1.setVelocity(TargetLaunchVelocity); - // launcher2.setVelocity(TargetLaunchVelocity); - } - } - - public void CloseShoot() { - // Spin the motors pid goes here - if (hasHardware) { - targetLaunchVelocity = closetargetLaunchVelocity; - } - } - - public void FarShoot() { - // Spin the motors pid goes here - if (hasHardware) { - targetLaunchVelocity = fartargetLaunchVelocity; - } - } - - public void AutoLaunch1() { - // Spin the motors pid goes here - if (hasHardware) { - setTargetSpeed(targetLaunchVelocityforAuto1); //change to auto aim velocity - //launcher1.setVelocity(targetLaunchVelocityforAuto1); - //launcher2.setVelocity(targetLaunchVelocityforAuto1); - } - } - - public void AutoLaunch2() { - // Spin the motors pid goes here - if (hasHardware) { - setTargetSpeed(targetLaunchVelocityforAuto2); //change to auto aim velocity - // launcher1.setVelocity(TargetLaunchVelocity); - // launcher2.setVelocity(TargetLaunchVelocity); - } - } - - public void FarAutoLaunch() { - // Spin the motors pid goes here - if (hasHardware) { - setTargetSpeed(fartargetLaunchVelocityforAuto); //change to auto aim velocity - // launcher1.setVelocity(TargetLaunchVelocity); - // launcher2.setVelocity(TargetLaunchVelocity); - } - } - - public double readVelocity() { - // if(launcher1.getVelocity() == TARGET_LAUNCH_VELOCITY && launcher2.getVelocity() == TARGET_LAUNCH_VELOCITY) { - // return ready = true; - // } else { - // return ready = false; - // } - return launcher1.getVelocity(); - - // 12.25 stationary voltage - had to decrease velocity by 150 (trial one: true, trial two: true) - // 11.84 stationary voltage - had to decrease velocity by 100? (trial one: - } - - public void setTargetSpeed(double speed) { - targetSpeed = speed; - // top.setVelocity(speed); - launcherPID.setTarget(speed); - } - - public double getTargetSpeed() { - return targetSpeed; - } - - private void setMotorPower(double pow) { - double power = Math.clamp(pow, -1, 1); - targetPower = power; - if (launcher1 != null && launcher2 != null) { - launcher1.setPower(power); - launcher2.setPower(power); - } - } - - public double getMotorSpeed() { - if (launcher1 != null) { - return launcher1.getVelocity(); - } - return -1; - } - - public double getMotor1Current() { - if (launcher1 != null) { - return launcher1.getAmperage(CurrentUnit.AMPS); - } - return -1; - } - - public double getMotor2Current() { - if (launcher2 != null) { - return launcher2.getAmperage(CurrentUnit.AMPS); - } - return -1; - } - - public void Stop() { - if (hasHardware) { - // launcher1.setVelocity(0); - // launcher2.setVelocity(0); - //launcher1.setPower(0); - //launcher2.setPower(0); - launcherPID.setTarget(0); - } - } - - public void IncreaseMotorVelocity() { - // Spin the motors pid goes here - if (hasHardware) { - additionAmount += additionDelta; - } - } - - public void DecreaseMotorVelocity() { - // Spin the motors pid goes here - if (hasHardware) { - additionAmount -= additionDelta; - } - } - - public void setMotorVelocityTest() { - launcher1.setVelocity(targetLaunchVelocity); - } - - // public void setMotorPowerTest() { - // launcher1.setPower(MOTOR_POWER); - // CurrentLaunchVelocity = getMotor1Velocity(); - // } - - public double getMotor1Velocity() { - return launcher1.getVelocity(); - } - - public double getMotor2Velocity() { - return launcher2.getVelocity(); - } - - public void VelocityShoot() { - if ( - getMotor1Velocity() == targetLaunchVelocity && - getMotor2Velocity() == targetLaunchVelocity - ) { - TeleCommands.GateDown(robot); - } - } - - public double autoVelocity() { - // x = distance in feet - double x = ls.getDistance(); - - if (x < 100 && x > 0) { - //launcherPI.p = 0.0015; - //launcherPI.i = 0; - lastAutoVelocity = REGRESSION_A * x + REGRESSION_B; - return lastAutoVelocity; - } - if (x < 0) { - return lastAutoVelocity; - } else { - launcherPI.p = 0.004; - launcherPI.i = 0.0002; - return REGRESSION_C * x + REGRESSION_D; - } - - //return ((RPM_PER_FOOT * ls.getDistance()) / 12 + MINIMUM_VELOCITY) + addtionamount; - } - - public double autoVelocityForAuto() { - // x = distance in feet - double x = ls.getDistance(); - - if (x < 100 && x > 0) { - //launcherPI.p = 0.0015; - //launcherPI.i = 0; - lastAutoVelocity = REGRESSION_A * x + REGRESSION_B; - return lastAutoVelocity; - } - if (x < 0) { - return lastAutoVelocity; - } else { - launcherPI.p = 0.004; - launcherPI.i = 0.0002; - return REGRESSION_C_AUTO * x + REGRESSION_D_AUTO; - } - - //return ((RPM_PER_FOOT * ls.getDistance()) / 12 + MINIMUM_VELOCITY) + addtionamount; - } - - public void setRegressionCAuto() { - // Spin the motors pid goes here - REGRESSION_C = REGRESSION_C_AUTO; - } - - public void setRegressionDAuto() { - // Spin the motors pid goes here - REGRESSION_D = REGRESSION_D_AUTO; - } - - public void setRegressionCTeleop() { - // Spin the motors pid goes here - REGRESSION_C = REGRESSION_C_TELEOP; - } - - public void setRegressionDTeleop() { - // Spin the motors pid goes here - REGRESSION_D = REGRESSION_D_TELEOP; - } - - public void increaseRegressionDTeleop() { - // Spin the motors pid goes here - REGRESSION_D += 15; - } - - @Override - public void periodic() { - autoVelocity = autoVelocity(); - currentLaunchVelocity = readVelocity(); - if (launcherPID.getTarget() != 0) { - setMotorPower(launcherPID.update(getMotorSpeed())); - } else { - setMotorPower(0); - launcherPID.update(getMotorSpeed()); - launcherPID.reset(); - } - - err = launcherPID.getLastError(); - motorVelocity = getMotorSpeed(); - power = launcher1.getPower(); - launcher1Current = getMotor1Current(); - launcher2Current = getMotor2Current(); - } -} diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/subsystems/LimelightSubsystem.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/subsystems/LimelightSubsystem.java index 13b6c1e5..705b3c9a 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/subsystems/LimelightSubsystem.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/subsystems/LimelightSubsystem.java @@ -6,18 +6,16 @@ import com.qualcomm.hardware.limelightvision.LLResult; import com.qualcomm.hardware.limelightvision.LLResultTypes; import com.qualcomm.hardware.limelightvision.Limelight3A; -import com.qualcomm.robotcore.hardware.DcMotorEx; -import com.qualcomm.robotcore.hardware.DcMotorSimple; -import com.technototes.library.hardware.motor.EncodedMotor; import com.technototes.library.logger.Log; import com.technototes.library.logger.Loggable; import com.technototes.library.subsystem.Subsystem; +import com.technototes.library.subsystem.TargetAcquisition; import java.util.List; import org.firstinspires.ftc.sixteen750.Hardware; import org.firstinspires.ftc.sixteen750.Setup; @Configurable -public class LimelightSubsystem implements Loggable, Subsystem { +public class LimelightSubsystem implements Loggable, Subsystem, TargetAcquisition { boolean hasHardware; @@ -101,10 +99,15 @@ public double getLimelightRotation() { // if it start turning away from the apriltag } - public double getTX() { + // Don't know if the signs for these two things are correct or not... + public double getHorizontalPosition() { return Xangle; } + public double getVerticalPosition() { + return Yangle; + } + public void LimelightStartup() { limelight.setPollRateHz(100); limelight.pipelineSwitch(AprilTag_Pipeline); diff --git a/TechnoLib/RobotLibrary/src/main/java/com/technototes/library/structure/ValidationOpMode.java b/TechnoLib/RobotLibrary/src/main/java/com/technototes/library/structure/ValidationOpMode.java new file mode 100644 index 00000000..56af6fc1 --- /dev/null +++ b/TechnoLib/RobotLibrary/src/main/java/com/technototes/library/structure/ValidationOpMode.java @@ -0,0 +1,70 @@ +package com.technototes.library.structure; + +import com.qualcomm.hardware.lynx.LynxModule; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import java.util.List; +import org.firstinspires.ftc.robotcore.external.navigation.VoltageUnit; + +public abstract class ValidationOpMode extends OpMode { + + public List hubs; + + public final double getVoltage() { + double volts = 0; + for (LynxModule lm : hubs) { + volts += lm.getInputVoltage(VoltageUnit.VOLTS); + } + return volts / hubs.size(); + } + + protected final void clearBulkCache() { + for (LynxModule hub : hubs) { + hub.clearBulkCache(); + } + } + + public final boolean anyDpadReleased() { + return ( + gamepad1.dpadUpWasReleased() || + gamepad1.dpadDownWasReleased() || + gamepad1.dpadLeftWasReleased() || + gamepad1.dpadRightWasReleased() + ); + } + + public final boolean anyButtonsReleased() { + return ( + gamepad1.aWasReleased() || gamepad1.bWasReleased() || gamepad1.xWasReleased() || gamepad1.yWasReleased() + ); + } + + public ValidationOpMode() { + super(); + } + + @Override + public void init() { + hubs = hardwareMap.getAll(LynxModule.class); + for (LynxModule hub : hubs) { + hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL); + } + } + + @Override + public void loop() { + clearBulkCache(); + telemetry.update(); + } + + public void addData(String caption, String data) { + telemetry.addData(caption, data); + } + + public void addData(String caption, double d) { + telemetry.addData(caption, d); + } + + public void addLine(String line) { + telemetry.addLine(line); + } +} diff --git a/TechnoLib/RobotLibrary/src/main/java/com/technototes/library/subsystem/TargetAcquisition.java b/TechnoLib/RobotLibrary/src/main/java/com/technototes/library/subsystem/TargetAcquisition.java new file mode 100644 index 00000000..7eb6fbff --- /dev/null +++ b/TechnoLib/RobotLibrary/src/main/java/com/technototes/library/subsystem/TargetAcquisition.java @@ -0,0 +1,13 @@ +package com.technototes.library.subsystem; + +// This should be promoted to a library interface, so it's available for anything to use/implement +public interface TargetAcquisition { + // This should be in *inches* + double getDistance(); + // Returns the location, in degrees, of the target relative to the current bot perspective + // Clockwise is negative, Counter-clockwise is positive + double getHorizontalPosition(); + // Returns the location, in degrees, of the target relative to the current bot perspective. + // "Above perpsective" is positive, "Below perspective" is negative + double getVerticalPosition(); +}