diff --git a/src/main/java/frc/robot/Subsystems/AlgaeCoraler/AlgaeCoraler.java b/src/main/java/frc/robot/Subsystems/AlgaeCoraler/AlgaeCoraler.java index c956e2e..73280ec 100644 --- a/src/main/java/frc/robot/Subsystems/AlgaeCoraler/AlgaeCoraler.java +++ b/src/main/java/frc/robot/Subsystems/AlgaeCoraler/AlgaeCoraler.java @@ -40,7 +40,6 @@ public AlgaeCoraler() { @Override public void runState() { - io.setPivotSetpoint(getState().getPivotSetpoint()); io.setWheelSpeed(getState().getWheelSpeed()); if (past != getState()) { there = false; @@ -48,6 +47,11 @@ public void runState() { there = true; } io.setThere(there); + if (there) { + io.setArmSpeed(getState().getThereSpeed()); + } else { + io.setArmSpeed(getState().getNotThereSpeed()); + } io.updateInputs(inputs); Logger.processInputs(SUBSYSTEM_NAME, inputs); diff --git a/src/main/java/frc/robot/Subsystems/AlgaeCoraler/AlgaeCoralerConstants.java b/src/main/java/frc/robot/Subsystems/AlgaeCoraler/AlgaeCoralerConstants.java index 6fef329..b22473e 100644 --- a/src/main/java/frc/robot/Subsystems/AlgaeCoraler/AlgaeCoralerConstants.java +++ b/src/main/java/frc/robot/Subsystems/AlgaeCoraler/AlgaeCoralerConstants.java @@ -22,35 +22,37 @@ public final class AlgaeCoralerConstants { public static final double WHEEL_GEARING = 25; public static final double PIVOT_GEARING = 25; + public static final double DEBOUNCE_TIME = 0.25; + + public static final Current NEAR_TARGET_AMPS = Amps.of(11.5); + + public static final int MAX_VOLTS = 12; + //CAN IDs - Correct Values public static final int PIVOT_MOTOR_CANID = 15; public static final int SPEED_MOTOR_CANID = 5; public static final Angle PIVOT_TOLERANCE = Degrees.of(1); - //Speeds + //Wheel Speeds public static final double ALGAE_IN_SPEED = -1; public static final double ALGAE_OUT_SPEED = 1; public static final double CORAL_OUT_SPEED = -0.25; public static final double HOLDING_SPEED = 0; - public static final double AUTO_SPEED = -0.43; - - //Currents - public static final Current ALGAE_CURRENT_LIMIT = Amps.of(12); - - //Angles - public static final Angle IDLE_ANGLE = Degrees.of(20); - public static final Angle ALGAE_IN_ANGLE = Degrees.of(-115); - public static final Angle ALGAE_HOLDING_ANGLE = Degrees.of(-25); - public static final Angle ALGAE_OUT_ANGLE = Degrees.of(-25); - public static final Angle CORAL_BLOCK_ANGLE = Degrees.of(-74.05); - - public static class Real { - - public static final PIDConstants DOWN_PIVOT_PID = new PIDConstants(0.6, 0, 0.025); - public static final PIDConstants UP_PIVOT_PID = new PIDConstants(0.9, 0, 0.025); - public static final PIDConstants PIVOT_PID = new PIDConstants(0.0, 0, 0); - } + public static final double AUTO_SHOOT_SPEED = -0.25; + + //Pivot speeds + //t and nt are slang for there and not there + public static final double IDLE_THERE_SPEED = 0.08; + public static final double IDLE_NOT_THERE_SPEED = 0.18; + public static final double ALGAE_IN_NT_SPEED = -0.4; + public static final double ALGAE_IN_T_SPEED = -0.1; + public static final double ALGAE_OUT_T_SPEED = 0.8; + public static final double ALGAE_OUT_NT_SPEED = 0.18; + public static final double CORAL_BLOCK_T_SPEED = 0; + public static final double CORAL_BLOCK_NT_SPEED = -0.2; + + public static class Real {} public static class Sim { diff --git a/src/main/java/frc/robot/Subsystems/AlgaeCoraler/AlgaeCoralerIO.java b/src/main/java/frc/robot/Subsystems/AlgaeCoraler/AlgaeCoralerIO.java index 3b9c41c..1f23e5d 100644 --- a/src/main/java/frc/robot/Subsystems/AlgaeCoraler/AlgaeCoralerIO.java +++ b/src/main/java/frc/robot/Subsystems/AlgaeCoraler/AlgaeCoralerIO.java @@ -1,15 +1,13 @@ package frc.robot.Subsystems.AlgaeCoraler; -import edu.wpi.first.units.measure.Angle; import org.littletonrobotics.junction.AutoLog; public interface AlgaeCoralerIO { @AutoLog public static class AlgaeCoralerIOInputs { - //Pivot - public double pivotPosition; - public double pivotSetpoint; + public double ArmSpeed; + public double ArmSetpoint; //Wheels public double wheelSpeed; @@ -20,8 +18,6 @@ public static class AlgaeCoralerIOInputs { public void updateInputs(AlgaeCoralerIOInputs input); - public void setPivotSetpoint(Angle pivotSetpoint); - public void setWheelSpeed(double wheelSpeed); public boolean nearTarget(); @@ -29,4 +25,6 @@ public static class AlgaeCoralerIOInputs { public boolean hasCoral(); public void setThere(boolean there); + + public void setArmSpeed(double ArmSpeed); } diff --git a/src/main/java/frc/robot/Subsystems/AlgaeCoraler/AlgaeCoralerIOReal.java b/src/main/java/frc/robot/Subsystems/AlgaeCoraler/AlgaeCoralerIOReal.java index bbbfc91..cc8e693 100644 --- a/src/main/java/frc/robot/Subsystems/AlgaeCoraler/AlgaeCoralerIOReal.java +++ b/src/main/java/frc/robot/Subsystems/AlgaeCoraler/AlgaeCoralerIOReal.java @@ -1,14 +1,11 @@ package frc.robot.Subsystems.AlgaeCoraler; -import static edu.wpi.first.units.Units.*; import static frc.robot.Subsystems.AlgaeCoraler.AlgaeCoralerConstants.*; import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.SparkMax; import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.filter.Debouncer.DebounceType; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj.DigitalInput; import org.littletonrobotics.junction.Logger; @@ -19,7 +16,6 @@ public class AlgaeCoralerIOReal implements AlgaeCoralerIO { private SparkMax wheelsMotor; private SparkMax pivotMotor; - private Angle pivotPosSetpoint; private double wheelSpeedSetpoint; private DigitalInput beamBreak; private boolean motorZeroed; @@ -31,7 +27,7 @@ public AlgaeCoralerIOReal() { wheelsMotor = new SparkMax(SPEED_MOTOR_CANID, MotorType.kBrushless); pivotMotor = new SparkMax(PIVOT_MOTOR_CANID, MotorType.kBrushless); beamBreak = new DigitalInput(DIO_PORT); - debounce = new Debouncer(0.25, DebounceType.kBoth); + debounce = new Debouncer(DEBOUNCE_TIME, DebounceType.kBoth); pivotMotor.getEncoder().setPosition(0); // Zeroing the encoder @@ -40,61 +36,29 @@ public AlgaeCoralerIOReal() { @Override public void updateInputs(AlgaeCoralerIOInputs inputs) { - inputs.pivotPosition = Units.rotationsToDegrees( - pivotMotor.getEncoder().getPosition() / PIVOT_GEARING - ); - inputs.pivotSetpoint = pivotPosSetpoint.in(Degree); inputs.wheelSpeed = (wheelsMotor.getEncoder().getVelocity()) / 60; inputs.wheelSpeedSetpoint = wheelSpeedSetpoint; - Logger.recordOutput("Pivot Position (Deg)", inputs.pivotPosition / 360); Logger.recordOutput("Motors Zeroed", motorZeroed); Logger.recordOutput("Beam Break Value", beamBreak.get()); Logger.recordOutput("Wheels Current", wheelsMotor.getOutputCurrent()); Logger.recordOutput("Pivot Current", pivotMotor.getOutputCurrent()); } - @Override - public void setPivotSetpoint(Angle pivotSetpoint) { - this.pivotPosSetpoint = pivotSetpoint; - - //Why did we return to these smh (leave the magic numbers shhh) - if (this.pivotPosSetpoint.in(Degree) == IDLE_ANGLE.in(Degree)) { - if (there) { - pivotMotor.set(0.08); - } else { - pivotMotor.set(0.18); - } - } else if (this.pivotPosSetpoint.in(Degree) == ALGAE_IN_ANGLE.in(Degree)) { - if (there) { - pivotMotor.set(-0.1); - } else { - pivotMotor.set(-0.4); - } - } else if (this.pivotPosSetpoint.in(Degree) == ALGAE_OUT_ANGLE.in(Degree)) { - if (there) { - pivotMotor.set(0.8); - } else { - pivotMotor.set(0.18); - } - } else if (this.pivotPosSetpoint.in(Degree) == CORAL_BLOCK_ANGLE.in(Degree)) { - if (there) { - pivotMotor.set(0); - } else { - pivotMotor.set(-0.2); - } - } - } - @Override public void setWheelSpeed(double wheelSpeed) { this.wheelSpeedSetpoint = wheelSpeed; wheelsMotor.set(wheelSpeed); } + @Override + public void setArmSpeed(double armSpeed) { + pivotMotor.set(armSpeed); + } + @Override public boolean nearTarget() { - if (debounce.calculate(pivotMotor.getOutputCurrent() >= 11.5)) { + if (debounce.calculate(pivotMotor.getOutputCurrent() >= NEAR_TARGET_AMPS.magnitude())) { return true; } return false; diff --git a/src/main/java/frc/robot/Subsystems/AlgaeCoraler/AlgaeCoralerIOSim.java b/src/main/java/frc/robot/Subsystems/AlgaeCoraler/AlgaeCoralerIOSim.java index 68bce7d..462c7e6 100644 --- a/src/main/java/frc/robot/Subsystems/AlgaeCoraler/AlgaeCoralerIOSim.java +++ b/src/main/java/frc/robot/Subsystems/AlgaeCoraler/AlgaeCoralerIOSim.java @@ -8,7 +8,6 @@ import com.revrobotics.sim.SparkMaxSim; import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.SparkMax; -import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.math.util.Units; @@ -29,8 +28,6 @@ public class AlgaeCoralerIOSim implements AlgaeCoralerIO { private SparkMaxSim pivotSparkSim; private SparkMaxSim wheelSparkSim; - private PIDController pivotController; - private double wheelSpeedSetpoint; private Angle pivotPosSetpoint; @@ -62,8 +59,6 @@ public AlgaeCoralerIOSim() { wheelSparkSim = new SparkMaxSim(dummyWheelsSpark, DCMotor.getNEO(NUM_SPEED_MOTORS)); - pivotController = new PIDController(PIVOT_PID.kP, PIVOT_PID.kI, PIVOT_PID.kD); - pivotPosSetpoint = Degrees.of(0); wheelSpeedSetpoint = 0; } @@ -73,10 +68,8 @@ public void updateInputs(AlgaeCoralerIOInputs inputs) { pivotSim.update(SIMULATION_PERIOD); wheelMotorSim.update(SIMULATION_PERIOD); - inputs.pivotPosition = Units.radiansToDegrees(pivotSim.getAngleRads()); inputs.wheelSpeed = Units.radiansToDegrees(wheelMotorSim.getAngularVelocityRPM() / 60); - inputs.pivotSetpoint = pivotPosSetpoint.in(Degree); inputs.wheelSpeedSetpoint = wheelSpeedSetpoint; pivotSparkSim.setPosition(pivotSim.getAngleRads()); @@ -89,20 +82,14 @@ public void updateInputs(AlgaeCoralerIOInputs inputs) { } @Override - public void setPivotSetpoint(Angle pivotSetpoint) { - this.pivotPosSetpoint = pivotSetpoint; - double voltage = pivotController.calculate( - Units.radiansToDegrees(pivotSim.getAngleRads()), - pivotSetpoint.in(Degrees) - ); - - pivotSim.setInputVoltage(voltage); + public void setWheelSpeed(double wheelSpeedSetpoint) { + this.wheelSpeedSetpoint = wheelSpeedSetpoint; + wheelMotorSim.setInputVoltage(wheelSpeedSetpoint * MAX_VOLTS); } @Override - public void setWheelSpeed(double wheelSpeedSetpoint) { - this.wheelSpeedSetpoint = wheelSpeedSetpoint; - wheelMotorSim.setInputVoltage(wheelSpeedSetpoint * 12); + public void setArmSpeed(double armSpeed) { + pivotSim.setInputVoltage(armSpeed * MAX_VOLTS); } @Override diff --git a/src/main/java/frc/robot/Subsystems/AlgaeCoraler/AlgaeCoralerStates.java b/src/main/java/frc/robot/Subsystems/AlgaeCoraler/AlgaeCoralerStates.java index 86f8efb..f61c6b2 100644 --- a/src/main/java/frc/robot/Subsystems/AlgaeCoraler/AlgaeCoralerStates.java +++ b/src/main/java/frc/robot/Subsystems/AlgaeCoraler/AlgaeCoralerStates.java @@ -2,26 +2,32 @@ import static frc.robot.Subsystems.AlgaeCoraler.AlgaeCoralerConstants.*; -import edu.wpi.first.units.measure.*; import org.team7525.subsystem.SubsystemStates; public enum AlgaeCoralerStates implements SubsystemStates { - IDLE("IDLE", (double) 0, IDLE_ANGLE), - CORALOUT("CORALOUT", CORAL_OUT_SPEED, IDLE_ANGLE), - ALGAEIN("ALGAEIN", ALGAE_IN_SPEED, ALGAE_IN_ANGLE), - HOLDING("HOLDING", HOLDING_SPEED, IDLE_ANGLE), - ALGAEOUT("ALGAEOUT", ALGAE_OUT_SPEED, IDLE_ANGLE), - AUTO_SHOOT("Auto Shoot", AUTO_SPEED, IDLE_ANGLE), - CORALBLOCK("CORALBLOCK", (double) 0, CORAL_BLOCK_ANGLE); + IDLE("IDLE", (double) 0, IDLE_THERE_SPEED, IDLE_NOT_THERE_SPEED), + CORALOUT("CORALOUT", CORAL_OUT_SPEED, IDLE_THERE_SPEED, IDLE_NOT_THERE_SPEED), + ALGAEIN("ALGAEIN", ALGAE_IN_SPEED, ALGAE_IN_T_SPEED, ALGAE_IN_NT_SPEED), + HOLDING("HOLDING", HOLDING_SPEED, IDLE_THERE_SPEED, IDLE_NOT_THERE_SPEED), + AUTO_SHOOT("AUTO_SHOOT", AUTO_SHOOT_SPEED, IDLE_THERE_SPEED, IDLE_NOT_THERE_SPEED), + ALGAEOUT("ALGAEOUT", ALGAE_OUT_SPEED, ALGAE_OUT_T_SPEED, ALGAE_OUT_NT_SPEED), + CORALBLOCK("CORALBLOCK", (double) 0, CORAL_BLOCK_T_SPEED, CORAL_BLOCK_NT_SPEED); private String stateString; private double wheelSpeed; - private Angle pivotSetpoint; - - AlgaeCoralerStates(String stateString, Double wheelSpeed, Angle pivotSetpoint) { + private double thereSpeed; + private double notThereSpeed; + + AlgaeCoralerStates( + String stateString, + Double wheelSpeed, + Double thereSpeed, + Double notThereSpeed + ) { this.stateString = stateString; - this.pivotSetpoint = pivotSetpoint; this.wheelSpeed = wheelSpeed; + this.thereSpeed = thereSpeed; + this.notThereSpeed = notThereSpeed; } @Override @@ -33,7 +39,11 @@ public double getWheelSpeed() { return wheelSpeed; } - public Angle getPivotSetpoint() { - return pivotSetpoint; + public double getThereSpeed() { + return thereSpeed; + } + + public double getNotThereSpeed() { + return notThereSpeed; } }