Improving sim and logging

misc/ControlsStuff/config.json

@@ -0,0 +1,46 @@
+{
+    "name": "Frontier",
+    "rotations": [
+    {
+        "axis": "x",
+        "degrees": 90
+    },
+    {
+        "axis": "y",
+        "degrees": 0
+    },
+    {
+        "axis": "z",
+        "degrees": 90
+    }
+    ],
+    "position": [
+    0,
+    0,
+    0.015
+    ],
+    "cameras": [],
+    "components": [
+        {
+        "zeroedRotations": [
+        {
+            "axis": "x",
+            "degrees": 90
+        },
+        {
+            "axis": "z",
+            "degrees": 90
+        },
+        {
+            "axis": "y",
+            "degrees": 0
+        }
+        ],
+        "zeroedPosition": [
+        -0.355,
+        0,
+        -0.193
+        ]
+    }
+    ]
+}

src/main/java/frc/robot/Robot.java

@@ -8,6 +8,7 @@
 import com.pathplanner.lib.commands.PathfindingCommand;
 import edu.wpi.first.cameraserver.CameraServer;
 import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.RobotBase;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import frc.robot.AutoManager.AutoManager;
 import frc.robot.Manager.Manager;
@@ -60,7 +61,9 @@ public void robotInit() {
 	@Override
 	public void robotPeriodic() {
 		manager.periodic();
-		vision.periodic();
+		if (!RobotBase.isSimulation()) {
+			vision.periodic(); // Vision sim is broken :(
+		}
 		CommandScheduler.getInstance().run();
 		Utilitys.controllers.clearCache();
 	}

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

@@ -3,7 +3,6 @@
 import static frc.robot.GlobalConstants.*;
 import static frc.robot.Subsystems.AlgaeCoraler.AlgaeCoralerConstants.*;
 
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import org.littletonrobotics.junction.Logger;
 import org.team7525.subsystem.Subsystem;
 
@@ -13,8 +12,8 @@ public class AlgaeCoraler extends Subsystem<AlgaeCoralerStates> {
 
 	private AlgaeCoralerIO io;
 	private AlgaeCoralerIOInputsAutoLogged inputs;
-	private boolean there;
-	private AlgaeCoralerStates past;
+	private boolean there; // If arm is at the target position
+	private AlgaeCoralerStates past; // Previous state of the algae bar to reset there variable
 
 	public static AlgaeCoraler getInstance() {
 		if (instance == null) {
@@ -46,7 +45,6 @@ public void runState() {
 		} else if (nearTarget()) {
 			there = true;
 		}
-		io.setThere(there);
 		if (there) {
 			io.setArmSpeed(getState().getThereSpeed());
 		} else {
@@ -55,8 +53,9 @@ public void runState() {
 
 		io.updateInputs(inputs);
 		Logger.processInputs(SUBSYSTEM_NAME, inputs);
-		SmartDashboard.putNumber("Coral Out", CORAL_OUT_SPEED);
 		Logger.recordOutput(SUBSYSTEM_NAME + "/State", getState().getStateString());
+		Logger.recordOutput(SUBSYSTEM_NAME + "/isThere", there);
+		Logger.recordOutput(SUBSYSTEM_NAME + "/Mechanism Position", io.getArmPosition());
 		past = getState();
 	}
 

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

@@ -32,8 +32,6 @@ public final class AlgaeCoralerConstants {
 	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);
-
 	//Wheel Speeds
 	public static final double ALGAE_IN_SPEED = -1;
 	public static final double ALGAE_OUT_SPEED = 1;

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

@@ -1,5 +1,6 @@
 package frc.robot.Subsystems.AlgaeCoraler;
 
+import edu.wpi.first.math.geometry.Pose3d;
 import org.littletonrobotics.junction.AutoLog;
 
 public interface AlgaeCoralerIO {
@@ -9,11 +10,10 @@ public static class AlgaeCoralerIOInputs {
 		public double ArmSpeed;
 		public double ArmSetpoint;
 
-		//Wheels
+		public Pose3d ArmPosition;
+
 		public double wheelSpeed;
 		public double wheelSpeedSetpoint;
-
-		public AlgaeCoralerStates state;
 	}
 
 	public void updateInputs(AlgaeCoralerIOInputs input);
@@ -24,7 +24,7 @@ public static class AlgaeCoralerIOInputs {
 
 	public boolean hasCoral();
 
-	public void setThere(boolean there);
-
 	public void setArmSpeed(double ArmSpeed);
+
+	public Pose3d getArmPosition();
 }

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

@@ -6,20 +6,19 @@
 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.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.DigitalInput;
 import org.littletonrobotics.junction.Logger;
 
-// TODO: Implement Current sensing for detection of algae
-
 public class AlgaeCoralerIOReal implements AlgaeCoralerIO {
 
 	private SparkMax wheelsMotor;
 	private SparkMax pivotMotor;
 
 	private double wheelSpeedSetpoint;
 	private DigitalInput beamBreak;
-	private boolean motorZeroed;
-	private Boolean there;
 	private Debouncer debounce;
 
 	public AlgaeCoralerIOReal() {
@@ -30,18 +29,14 @@ public AlgaeCoralerIOReal() {
 		debounce = new Debouncer(DEBOUNCE_TIME, DebounceType.kBoth);
 
 		pivotMotor.getEncoder().setPosition(0); // Zeroing the encoder
-
-		there = true;
 	}
 
 	@Override
 	public void updateInputs(AlgaeCoralerIOInputs inputs) {
 		inputs.wheelSpeed = (wheelsMotor.getEncoder().getVelocity()) / 60;
 		inputs.wheelSpeedSetpoint = wheelSpeedSetpoint;
 
-		Logger.recordOutput("Motors Zeroed", motorZeroed);
 		Logger.recordOutput("Beam Break Value", beamBreak.get());
-		Logger.recordOutput("Wheels Current", wheelsMotor.getOutputCurrent());
 		Logger.recordOutput("Pivot Current", pivotMotor.getOutputCurrent());
 	}
 
@@ -70,7 +65,16 @@ public boolean hasCoral() {
 	}
 
 	@Override
-	public void setThere(boolean there) {
-		this.there = there;
+	public Pose3d getArmPosition() {
+		return new Pose3d(
+			0,
+			0,
+			0,
+			new Rotation3d(
+				0,
+				Units.rotationsToDegrees(pivotMotor.getEncoder().getPosition()) / 25,
+				0
+			)
+		);
 	}
 }

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

@@ -8,10 +8,12 @@
 import com.revrobotics.sim.SparkMaxSim;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
 import com.revrobotics.spark.SparkMax;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.math.system.plant.LinearSystemId;
 import edu.wpi.first.math.util.Units;
-import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.wpilibj.simulation.DCMotorSim;
 import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
 import frc.robot.Subsystems.AlgaeCoraler.AlgaeCoralerConstants.Sim;
@@ -29,7 +31,6 @@ public class AlgaeCoralerIOSim implements AlgaeCoralerIO {
 	private SparkMaxSim wheelSparkSim;
 
 	private double wheelSpeedSetpoint;
-	private Angle pivotPosSetpoint;
 
 	public AlgaeCoralerIOSim() {
 		pivotSim = new SingleJointedArmSim(
@@ -58,8 +59,6 @@ public AlgaeCoralerIOSim() {
 		pivotSparkSim = new SparkMaxSim(dummyPivotSpark, DCMotor.getNEO(NUM_PIVOT_MOTORS));
 
 		wheelSparkSim = new SparkMaxSim(dummyWheelsSpark, DCMotor.getNEO(NUM_SPEED_MOTORS));
-
-		pivotPosSetpoint = Degrees.of(0);
 		wheelSpeedSetpoint = 0;
 	}
 
@@ -94,20 +93,32 @@ public void setArmSpeed(double armSpeed) {
 
 	@Override
 	public boolean nearTarget() {
-		return (
-			(Math.abs(
-					Units.radiansToDegrees(pivotSim.getAngleRads()) - pivotPosSetpoint.in(Degree)
-				) <
-				PIVOT_TOLERANCE.in(Degrees))
-		);
+		return true; // In simulation, we assume the arm is always near the target
 	}
 
 	@Override
 	public boolean hasCoral() {
 		return AlgaeCoraler.getInstance().getStateTime() > Sim.CORAL_TIME.in(Seconds);
-		// IDK how to sim this
 	}
 
 	@Override
-	public void setThere(boolean there) {}
+	public Pose3d getArmPosition() {
+		return new Pose3d(
+			new Translation3d(0.355, 0, 0.193),
+			new Rotation3d(
+				0,
+				Math.toRadians(
+					AlgaeCoraler.getInstance().getState() == AlgaeCoralerStates.IDLE ||
+						AlgaeCoraler.getInstance().getState() == AlgaeCoralerStates.HOLDING ||
+						AlgaeCoraler.getInstance().getState() == AlgaeCoralerStates.CORALOUT ||
+						AlgaeCoraler.getInstance().getState() == AlgaeCoralerStates.ALGAEOUT
+						? Math.floor(
+							Math.max(45 - (AlgaeCoraler.getInstance().getStateTime() * 60), -10)
+						)
+						: Math.floor(Math.min(AlgaeCoraler.getInstance().getStateTime() * 60, 45))
+				),
+				0
+			)
+		);
+	}
 }

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

@@ -1,3 +1,5 @@
+// Drive Subsystem
+// [Rouge Subsystem: Manages it's own state]
 package frc.robot.Subsystems.Drive;
 
 import static frc.robot.GlobalConstants.Controllers.DRIVER_CONTROLLER;
@@ -112,15 +114,11 @@ private void establishTriggers() {
 	public void runState() {
 		// Slow mode toggle
 		if (slow) {
-			if (Controllers.DRIVER_CONTROLLER.getLeftBumperButtonPressed()) {
-				slow = false;
-			}
+			if (Controllers.DRIVER_CONTROLLER.getLeftBumperButtonPressed()) slow = false;
 			swerveInputs.scaleTranslation(0.33);
 			swerveInputs.scaleRotation(0.33);
 		} else {
-			if (Controllers.DRIVER_CONTROLLER.getLeftBumperButtonPressed()) {
-				slow = true;
-			}
+			if (Controllers.DRIVER_CONTROLLER.getLeftBumperButtonPressed()) slow = true;
 			swerveInputs.scaleTranslation(1);
 			swerveInputs.scaleRotation(1);
 		}
@@ -138,9 +136,12 @@ public void runState() {
 			swerveDrive.drive(swerveInputs.get());
 		}
 
-		// Update SmartDashboard
-		SmartDashboard.putBoolean("SLOW MODE", slow);
-		SmartDashboard.putBoolean("FIELD RELATIVE", fieldRelative);
+		// Logging Stuff
+		field.setRobotPose(swerveDrive.getPose());
+		Logger.recordOutput(SUBSYSTEM_NAME + "/Pose", swerveDrive.getPose());
+		Logger.recordOutput(SUBSYSTEM_NAME + "/Slow Mode", slow);
+		Logger.recordOutput(SUBSYSTEM_NAME + "/Field Relative", fieldRelative);
+		Logger.recordOutput(SUBSYSTEM_NAME + "/Drive State", getState());
 		SmartDashboard.putData(field);
 	}
 

src/main/java/frc/robot/Subsystems/Drive/DriveConstants.java

@@ -15,6 +15,8 @@
  */
 public final class DriveConstants {
 
+	public static final String SUBSYSTEM_NAME = "DriveBase";
+
 	// PID constants for translation and rotation
 	public static final PIDConstants PPH_TRANSLATION_PID = new PIDConstants(5, 0, 0);
 	public static final PIDConstants PPH_ROTATION_PID = new PIDConstants(5, 0, 0);