yotam - fixing shit

GreenBlitz · Aug 7, 2024 · a5c7760 · a5c7760
1 parent 4ac8871
commit a5c7760
Show file tree

Hide file tree

Showing 6 changed files with 31 additions and 27 deletions.
diff --git a/src/main/java/training/TrainingRobotManager.java b/src/main/java/training/TrainingRobotManager.java
@@ -1,6 +1,8 @@
 package training;
 
+import edu.wpi.first.math.geometry.Rotation2d;
 import training.commands.MoveAngularByPosition;
+import training.commands.MoveElbowByPosition;
 import training.commands.MoveLinearMotor;
 import training.subsystems.Module;
 import utils.DefaultRobotManager;
@@ -15,9 +17,8 @@ public class TrainingRobotManager extends DefaultRobotManager {
     public void trainingInit() {
         this.robot = new Robot();
         joystick = new SmartJoystick(JoystickPorts.MAIN);
-        joystick.A.whileTrue(new MoveLinearMotor(robot,joystick));
-        joystick.B.whileTrue(new MoveAngularByPosition(robot, joystick, 2));
-        joystick.X.whileTrue(new MoveAngularByPosition(robot, joystick, 1.5));
+        joystick.A.onTrue(new MoveElbowByPosition(robot, Rotation2d.fromDegrees(90)));
+        joystick.B.onTrue(new MoveElbowByPosition(robot, Rotation2d.fromDegrees(180)));
     }
 
 	@Override

diff --git a/src/main/java/training/commands/MoveElbowByPosition.java b/src/main/java/training/commands/MoveElbowByPosition.java
@@ -9,6 +9,7 @@
 
 public class MoveElbowByPosition extends Command {
     private final Elbow elbow;
+
     private final Rotation2d position;
     public MoveElbowByPosition(Robot robot, Rotation2d position) {
         this.elbow = robot.getElbow();
@@ -24,16 +25,17 @@ public void initialize() {
     @Override
     public void execute() {
         elbow.moveToAngle(position);
+        Logger.recordOutput("difference", Math.abs(elbow.getPosition().getRotations() - position.getRotations()));
     }
 
     @Override
     public boolean isFinished() {
-        return Math.abs(elbow.getPosition().getRotations() - position.getRotations()) <= ElbowConstants.ANGULAR_TOLERANCE.getRotations();
+        return Math.abs(elbow.getPosition().getRotations() - position.getRotations()) % 1 <= ElbowConstants.ANGULAR_TOLERANCE.getRotations();
     }
 
     @Override
     public void end(boolean interrupted) {
         elbow.stopMotor();
-        Logger.recordOutput(elbow.getPosition().toString());
+        elbow.standInPlace();
     }
 }
diff --git a/src/main/java/training/subsystems/Elbow/Elbow.java b/src/main/java/training/subsystems/Elbow/Elbow.java
@@ -1,6 +1,7 @@
 package training.subsystems.Elbow;
 
 import edu.wpi.first.math.geometry.Rotation2d;
+import org.littletonrobotics.junction.Logger;
 import utils.GBSubsystem;
 
 public class Elbow extends GBSubsystem {
@@ -17,6 +18,7 @@ protected String getLogPath() {
 
     @Override
     protected void subsystemPeriodic() {
+        Logger.recordOutput("elbow position", elbow.getPosition().getDegrees());
     }
 
 
@@ -32,4 +34,8 @@ public void stopMotor() {
         elbow.stopMotor();
     }
 
+    public void standInPlace() {
+        moveToAngle(getPosition());
+    }
+
 }
diff --git a/src/main/java/training/subsystems/Elbow/ElbowConstants.java b/src/main/java/training/subsystems/Elbow/ElbowConstants.java
@@ -10,15 +10,15 @@ public class ElbowConstants {
     public static final double POSITION_CONVERSION_FACTOR = 1; //check and change
     public static final double VELOCITY_CONVERSION_FACTOR = 1; //check and change
 
-    public static final double KP_VALUE = 45;
+    public static final double KP_VALUE = 0.4;
     public static final double KI_VALUE = 0;
-    public static final double KD_VALUE = 0;
+    public static final double KD_VALUE = 1.5;
     public static final Rotation2d ANGULAR_TOLERANCE = Rotation2d.fromDegrees(1.8);
     public static final ArmFeedforward FEED_FORWARD_PARAMETERS = new ArmFeedforward(1, 1, 0);
-    public static final RobotType ROBOT_TYPE = RobotType.SYNCOPA;
-    public static final double ARM_LENGTH = 1;
-    public static final double ARM_MASS_KG = 1;
-    public static final Rotation2d BACKWARD_ANGLE_LIMIT = Rotation2d.fromDegrees(360);
-    public static final Rotation2d FORWARD_ANGLE_LIMIT = Rotation2d.fromDegrees(360);
+    public static final RobotType ROBOT_TYPE = RobotType.SIMULATION;
+    public static final double ARM_LENGTH = 0.44;
+    public static final double ARM_MASS_KG = 0.44;
+    public static final Rotation2d BACKWARD_ANGLE_LIMIT = Rotation2d.fromDegrees(0);
+    public static final Rotation2d FORWARD_ANGLE_LIMIT = Rotation2d.fromRotations(999);
 
 }
diff --git a/src/main/java/training/subsystems/Elbow/SimulationElbow/SimulationElbow.java b/src/main/java/training/subsystems/Elbow/SimulationElbow/SimulationElbow.java
@@ -1,6 +1,7 @@
 package training.subsystems.Elbow.SimulationElbow;
 
 import com.ctre.phoenix6.StatusCode;
+import com.ctre.phoenix6.configs.ClosedLoopGeneralConfigs;
 import com.ctre.phoenix6.configs.FeedbackConfigs;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.controls.ControlRequest;
@@ -16,9 +17,8 @@
 
 import java.util.Map;
 
-public class SimulationElbow extends GBSubsystem implements IElbow {
+public class SimulationElbow implements IElbow {
     private final SingleJointedArmSimulation motor;
-
     public SimulationElbow() {
         SingleJointedArmSim elbowSimulation = new SingleJointedArmSim(
                 DCMotor.getFalcon500(SimulationElbowConstants.NUMBER_OF_MOTORS),
@@ -32,20 +32,14 @@ public SimulationElbow() {
                 ElbowConstants.FORWARD_ANGLE_LIMIT.getRadians(),
                 false,
                 ElbowConstants.STARTING_POSITION.getRadians());
+        ClosedLoopGeneralConfigs a = new ClosedLoopGeneralConfigs();
+        a.ContinuousWrap = true;
         motor = new SingleJointedArmSimulation(elbowSimulation);
         TalonFXConfiguration talonFXConfiguration = new TalonFXConfiguration()
-                .withFeedback(new FeedbackConfigs().withSensorToMechanismRatio(150/7))
-                .withSlot0(SimulationElbowConstants.SLOT_0_CONFIGS);
+                .withSlot0(SimulationElbowConstants.SLOT_0_CONFIGS)
+                .withClosedLoopGeneral(a);
         motor.applyConfiguration(talonFXConfiguration);
     }
-    @Override
-    protected String getLogPath() {
-        return "";
-    }
-
-    @Override
-    protected void subsystemPeriodic() {
-    }
 
 
     public void moveToAngle(Rotation2d position) {

diff --git a/src/main/java/training/subsystems/Elbow/SimulationElbow/SimulationElbowConstants.java b/src/main/java/training/subsystems/Elbow/SimulationElbow/SimulationElbowConstants.java
@@ -1,12 +1,13 @@
 package training.subsystems.Elbow.SimulationElbow;
 
 import com.ctre.phoenix6.configs.Slot0Configs;
+import training.subsystems.Elbow.ElbowConstants;
 
 public class SimulationElbowConstants {
     public static final int NUMBER_OF_MOTORS = 1;
     public static final double GEAR_RATIO = 1;
     public static final Slot0Configs SLOT_0_CONFIGS = new Slot0Configs()
-            .withKP(45)
-            .withKI(0)
-            .withKD(0);
+            .withKP(ElbowConstants.KP_VALUE)
+            .withKI(ElbowConstants.KI_VALUE)
+            .withKD(ElbowConstants.KD_VALUE);
 }