Nitay - I am a god(the simulation shows perfect graphs despite it bei…

…ng imposible) - working on simulatin pid and feedforward
GreenBlitz · Aug 13, 2024 · 75aac6b · 75aac6b
1 parent 0774c12
commit 75aac6b
Show file tree

Hide file tree

Showing 14 changed files with 65 additions and 25 deletions.
diff --git a/src/main/java/training/commands/ArmCommands/elbowCommands/ElbowStayInPlace.java b/src/main/java/training/commands/ArmCommands/elbowCommands/ElbowStayInPlace.java
@@ -1,23 +1,25 @@
 package training.commands.ArmCommands.elbowCommands;
 
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj2.command.Command;
 import training.subsystems.ArmSubsystems.elbow.Elbow;
-import training.subsystems.ArmSubsystems.elbow.ElbowConstants;
+
+import java.lang.module.ModuleDescriptor;
 
 public class ElbowStayInPlace extends Command {
 
-	private final Elbow elbow;
+	private Elbow elbow;
+	private Rotation2d targetPosition;
 
 	public ElbowStayInPlace(Elbow elbow) {
 		this.elbow = elbow;
+		this.targetPosition = elbow.getTargetPosition();
 		addRequirements(elbow);
 	}
 
-
 	@Override
 	public void execute() {
-		if (elbow.isAtTargetPosition(elbow.getTargetPosition(), ElbowConstants.POSITION_TOLERANCE, ElbowConstants.VELOCITY_TOLERANCE)) {
-			elbow.stayAtPosition();
-		}
+		elbow.stayAtPosition();
 	}
+
 }
diff --git a/src/main/java/training/commands/ArmCommands/elbowCommands/MoveElbow.java b/src/main/java/training/commands/ArmCommands/elbowCommands/MoveElbow.java
@@ -17,14 +17,18 @@ public MoveElbow(Rotation2d targetPosition, Elbow elbow) {
 		addRequirements(elbow);
 	}
 
+	@Override
+	public void initialize() {
+		System.out.println("move");
+	}
+
 	@Override
 	public void execute() {
 		elbow.goToPosition(targetPosition);
 	}
 
 	@Override
 	public boolean isFinished() {
-		System.out.println("aaaaaaaaahhh");
 		return elbow.isAtTargetPosition(targetPosition, ElbowConstants.POSITION_TOLERANCE, ElbowConstants.VELOCITY_TOLERANCE)
 			&& elbow.getVelocity().getRotations() <= ElbowConstants.VELOCITY_TOLERANCE.getRotations();
 	}

diff --git a/src/main/java/training/commands/ArmCommands/rollerCommands/RollClockwise.java b/src/main/java/training/commands/ArmCommands/rollerCommands/RollClockwise.java
@@ -9,6 +9,8 @@ public class RollClockwise extends Command {
 
 	public RollClockwise(Roller roller) {
 		this.roller = roller;
+		addRequirements(roller);
+
 	}
 
 	@Override

diff --git a/src/main/java/training/commands/ArmCommands/rollerCommands/RollCounterClockwise.java b/src/main/java/training/commands/ArmCommands/rollerCommands/RollCounterClockwise.java
@@ -9,6 +9,7 @@ public class RollCounterClockwise extends Command {
 
 	public RollCounterClockwise(Roller roller) {
 		this.roller = roller;
+		addRequirements(roller);
 	}
 
 	public void initialize() {

diff --git a/src/main/java/training/commands/ArmCommands/wristCommands/MoveWrist.java b/src/main/java/training/commands/ArmCommands/wristCommands/MoveWrist.java
@@ -13,6 +13,8 @@ public class MoveWrist extends Command {
 	public MoveWrist(Rotation2d targetPosition, Wrist wrist) {
 		this.wrist = wrist;
 		this.targetPosition = targetPosition;
+		addRequirements(wrist);
+
 	}
 
 	@Override

diff --git a/src/main/java/training/commands/ArmCommands/wristCommands/StopWrist.java b/src/main/java/training/commands/ArmCommands/wristCommands/StopWrist.java
@@ -10,6 +10,7 @@ public class StopWrist extends Command {
 	public StopWrist(Wrist wrist) {
 		this.wrist = wrist;
 		addRequirements(wrist);
+
 	}
 
 	@Override

diff --git a/src/main/java/training/subsystems/ArmSubsystems/elbow/Elbow.java b/src/main/java/training/subsystems/ArmSubsystems/elbow/Elbow.java
@@ -8,6 +8,7 @@ public class Elbow extends GBSubsystem {
 
 	private final IElbow iElbow;
 	private final ElbowInputsAutoLogged inputs;
+	private Rotation2d targetPosition = ElbowConstants.STARTING_POSITION;
 
 	public Elbow() {
 		this.iElbow = ElbowFactory.create();
@@ -37,13 +38,14 @@ public boolean isAtTargetPosition(Rotation2d targetAngle, Rotation2d positionTol
 	}
 
 	public void goToPosition(Rotation2d targetPosition) {
-		iElbow.goToPosition(inputs.targetPosition);
-		Logger.recordOutput("target position:", targetPosition.getDegrees());
 		setTargetPosition(targetPosition);
+		iElbow.goToPosition(getTargetPosition());
+		Logger.recordOutput("target position:", targetPosition.getDegrees());
 	}
 
 	public void stayAtPosition() {
-		goToPosition(inputs.targetPosition);
+		iElbow.stayAtPosition();
+		System.out.println("staying");
 	}
 
 	public void setVoltage(double voltage) {
@@ -71,11 +73,11 @@ public double getCurrent() {
 	}
 
 	public Rotation2d getTargetPosition() {
-		return inputs.targetPosition;
+		return targetPosition;
 	}
 
 	public void setTargetPosition(Rotation2d targetPosition) {
-		inputs.targetPosition = targetPosition;
+		this.targetPosition = targetPosition;
 	}
 
 }
diff --git a/src/main/java/training/subsystems/ArmSubsystems/elbow/ElbowConstants.java b/src/main/java/training/subsystems/ArmSubsystems/elbow/ElbowConstants.java
@@ -6,12 +6,11 @@ public class ElbowConstants {
 
 
 	public static final Rotation2d STARTING_POSITION = Rotation2d.fromDegrees(-60);
-	public static final Rotation2d CLIMBING_POSITION = Rotation2d.fromDegrees(-20);
+	public static final Rotation2d CLIMBING_POSITION = Rotation2d.fromDegrees(-21.2);
 
-	public static final Rotation2d POSITION_TOLERANCE = Rotation2d.fromDegrees(5);
+	public static final Rotation2d POSITION_TOLERANCE = Rotation2d.fromDegrees(0.25);
 	public static final Rotation2d VELOCITY_TOLERANCE = Rotation2d.fromRotations(0.01);
 
-	public static final Rotation2d GEAR_RATIO = Rotation2d.fromDegrees(1 / (28.0 * (60.0 / 16.0)));
 
 	public static final double ARM_LENGTH = 0.44;
 	public static final double ARM_MASS_KG = 0.44;

diff --git a/src/main/java/training/subsystems/ArmSubsystems/elbow/ElbowInputs.java b/src/main/java/training/subsystems/ArmSubsystems/elbow/ElbowInputs.java
@@ -10,6 +10,5 @@ public class ElbowInputs {
 	public Rotation2d velocity = Rotation2d.fromRotations(0);
 	public double current = 0;
 	public double voltage = 0;
-	public Rotation2d targetPosition = ElbowConstants.STARTING_POSITION;
 
 }
diff --git a/src/main/java/training/subsystems/ArmSubsystems/elbow/IElbow.java b/src/main/java/training/subsystems/ArmSubsystems/elbow/IElbow.java
@@ -12,4 +12,6 @@ public interface IElbow {
 
 	void updateInputs(ElbowInputsAutoLogged inputs);
 
+	void stayAtPosition();
+
 }
diff --git a/src/main/java/training/subsystems/ArmSubsystems/elbow/neo/Neo.java b/src/main/java/training/subsystems/ArmSubsystems/elbow/neo/Neo.java
@@ -17,8 +17,8 @@ public class Neo implements IElbow {
 
 	public Neo() {
 		this.motor = new CANSparkMax(NeoConstants.ID, CANSparkLowLevel.MotorType.kBrushless);
-		motor.getEncoder().setPositionConversionFactor(ElbowConstants.GEAR_RATIO.getRotations());
-		motor.getEncoder().setVelocityConversionFactor(ElbowConstants.GEAR_RATIO.getRotations());
+		motor.getEncoder().setPositionConversionFactor(NeoConstants.GEAR_RATIO.getRotations());
+		motor.getEncoder().setVelocityConversionFactor(NeoConstants.GEAR_RATIO.getRotations());
 
 		motor.setSoftLimit(CANSparkBase.SoftLimitDirection.kForward, (float) ElbowConstants.FORWARD_ANGLE_LIMIT.getRotations());
 		motor.enableSoftLimit(CANSparkBase.SoftLimitDirection.kForward, true);
@@ -52,6 +52,10 @@ public void goToPosition(Rotation2d position) {
 			);
 	}
 
+	@Override
+	public void stayAtPosition() {
+		setVoltage(NeoConstants.FEEDFORWARD_CONTROLLER.calculate(getPosition().getRadians(), getVelocity().getRadians()));
+	}
 
 	@Override
 	public void updateInputs(ElbowInputsAutoLogged inputs) {
@@ -61,6 +65,7 @@ public void updateInputs(ElbowInputsAutoLogged inputs) {
 		inputs.voltage = motor.getBusVoltage() * motor.getAppliedOutput();
 	}
 
+
 	public Rotation2d getPosition() {
 		return Rotation2d.fromRotations(motor.getEncoder().getPosition());
 	}

diff --git a/src/main/java/training/subsystems/ArmSubsystems/elbow/neo/NeoConstants.java b/src/main/java/training/subsystems/ArmSubsystems/elbow/neo/NeoConstants.java
@@ -1,6 +1,7 @@
 package training.subsystems.ArmSubsystems.elbow.neo;
 
 import edu.wpi.first.math.controller.ArmFeedforward;
+import edu.wpi.first.math.geometry.Rotation2d;
 
 public class NeoConstants {
 
@@ -10,6 +11,8 @@ public class NeoConstants {
 	public static final double I_VALUE = 0;
 	public static final double D_VALUE = 0;
 
+	public static final Rotation2d GEAR_RATIO = Rotation2d.fromDegrees(1 / (28.0 * (60.0 / 16.0)));
+
 	public static final double KS_VALUE = 0;
 	public static final double KG_VALUE = 0;
 	public static final double KV_VALUE = 0;

diff --git a/src/main/java/training/subsystems/ArmSubsystems/elbow/simulation/Simulation.java b/src/main/java/training/subsystems/ArmSubsystems/elbow/simulation/Simulation.java
@@ -20,12 +20,12 @@ public class Simulation implements IElbow {
 	public Simulation() {
 		this.motor = new SingleJointedArmSim(
 			DCMotor.getNEO(1),
-			ElbowConstants.GEAR_RATIO.getDegrees(),
+			SimulationConstants.GEAR_RATIO.getDegrees(),
 			SingleJointedArmSim.estimateMOI(ElbowConstants.ARM_LENGTH, ElbowConstants.ARM_MASS_KG),
 			ElbowConstants.ARM_LENGTH,
 			ElbowConstants.BACKWARD_ANGLE_LIMIT.getRadians(),
 			ElbowConstants.FORWARD_ANGLE_LIMIT.getRadians(),
-			false,
+			true,
 			ElbowConstants.STARTING_POSITION.getRadians()
 		);
 		pidController = SimulationConstants.PID_CONTROLLER;
@@ -45,8 +45,19 @@ public void setPower(double power) {
 
 	public void goToPosition(Rotation2d targetPosition) {
 		pidController.setSetpoint(targetPosition.getRadians());
-		setVoltage(pidController.calculate(getPosition().getRadians())
-//				+ feedForwardController.calculate(getPosition().getDegrees(), getVelocity().getRotations())
+		setVoltage(
+			pidController.calculate(getPosition().getRadians())
+				+ feedForwardController.calculate(getPosition().getDegrees(), getVelocity().getRotations())
+		);
+	}
+
+	@Override
+	public void stayAtPosition() {
+		setVoltage(
+			SimulationConstants.FEEDFORWARD_CONTROLLER.calculate(
+				getPosition().getRadians(),
+				getVelocity().getRadians() + pidController.calculate(getPosition().getRadians())
+			)
 		);
 	}
 
@@ -71,4 +82,8 @@ public boolean isAtTargetAngle(Rotation2d targetAngle, Rotation2d tolerance) {
 		return (Math.abs(getPosition().minus(targetAngle).getDegrees()) <= tolerance.getDegrees());
 	}
 
+	public ArmFeedforward getFeedForwardController() {
+		return feedForwardController;
+	}
+
 }
diff --git a/src/main/java/training/subsystems/ArmSubsystems/elbow/simulation/SimulationConstants.java b/src/main/java/training/subsystems/ArmSubsystems/elbow/simulation/SimulationConstants.java
@@ -2,18 +2,21 @@
 
 import edu.wpi.first.math.controller.ArmFeedforward;
 import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.geometry.Rotation2d;
 
 public class SimulationConstants {
 
 	public static final int ID = 0;
 
-	public static final double P_VALUE = 0.15;
+	public static final double P_VALUE = 100;
 	public static final double I_VALUE = 0;
-	public static final double D_VALUE = 0.2;
+	public static final double D_VALUE = 0;
 	public static final PIDController PID_CONTROLLER = new PIDController(P_VALUE, I_VALUE, D_VALUE);
 
+	public static final Rotation2d GEAR_RATIO = Rotation2d.fromDegrees(1 / (1 / (28.0 * (60.0 / 16.0))));
+
 	public static final double KS_VALUE = 0;
-	public static final double KG_VALUE = 0;
+	public static final double KG_VALUE = 0.04169848;
 	public static final double KV_VALUE = 0;
 	public static final double KA_VALUE = 0;
Original file line number	Diff line number	Diff line change
Expand Up		@@ -12,4 +12,6 @@ public interface IElbow {

		void updateInputs(ElbowInputsAutoLogged inputs);

		void stayAtPosition();

		}