Algae corraler

src/main/java/frc/robot/Subsystems/AlgaeCoraler/AlgaeCoraler.java

@@ -40,14 +40,18 @@ public AlgaeCoraler() {
 
 	@Override
 	public void runState() {
-		io.setPivotSetpoint(getState().getPivotSetpoint());
 		io.setWheelSpeed(getState().getWheelSpeed());
 		if (past != getState()) {
 			there = false;
 		} else if (nearTarget()) {
 			there = true;
 		}
 		io.setThere(there);
+		if (there) {
+			io.setArmSpeed(getState().getThereSpeed());
+		} else {
+			io.setArmSpeed(getState().getNotThereSpeed());
+		}
 
 		io.updateInputs(inputs);
 		Logger.processInputs(SUBSYSTEM_NAME, inputs);

src/main/java/frc/robot/Subsystems/AlgaeCoraler/AlgaeCoralerConstants.java

@@ -1,13 +1,11 @@
 package frc.robot.Subsystems.AlgaeCoraler;
 
-import static edu.wpi.first.units.Units.Amps;
 import static edu.wpi.first.units.Units.Degrees;
 import static edu.wpi.first.units.Units.KilogramSquareMeters;
 import static edu.wpi.first.units.Units.Meters;
 import static edu.wpi.first.units.Units.Second;
 
 import edu.wpi.first.units.measure.Angle;
-import edu.wpi.first.units.measure.Current;
 import edu.wpi.first.units.measure.Distance;
 import edu.wpi.first.units.measure.MomentOfInertia;
 import edu.wpi.first.units.measure.Time;
@@ -22,35 +20,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 double NEAR_TARGET_AMPS = 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 {
 

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,13 +18,13 @@ public static class AlgaeCoralerIOInputs {
 
 	public void updateInputs(AlgaeCoralerIOInputs input);
 
-	public void setPivotSetpoint(Angle pivotSetpoint);
-
 	public void setWheelSpeed(double wheelSpeed);
 
 	public boolean nearTarget();
 
 	public boolean hasCoral();
 
 	public void setThere(boolean there);
+
+	public void setArmSpeed(double ArmSpeed);
 }

src/main/java/frc/robot/Subsystems/AlgaeCoraler/AlgaeCoralerIOReal.java

@@ -8,7 +8,6 @@
 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 +18,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 +29,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 +38,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)) {
 			return true;
 		}
 		return false;

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

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;
 	}
 }