ffsg

diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java
index 8475f05b564b0a125c07d54ea74133e51d1e8e3c..76dd7a3cb3a7223b917b5c540391baea7e19708d 100644 (file)
--- a/src/main/java/frc/robot/subsystems/turret/Turret.java
+++ b/src/main/java/frc/robot/subsystems/turret/Turret.java
@@ -91,8 +91,8 @@ public class Turret extends SubsystemBase {
         config.Slot0.kD = Units.radiansToRotations(0.00 * 12); // Derivative term (used to dampen oscillations)
         
         MotionMagicConfigs motionMagicConfigs = config.MotionMagic;
-        motionMagicConfigs.MotionMagicCruiseVelocity = Units.degreesToRotations(90) * gearRatio; // max velocity * gear ratio
-        motionMagicConfigs.MotionMagicAcceleration = Units.degreesToRotations(90) * gearRatio; // max Acceleration * gear ratio
+        motionMagicConfigs.MotionMagicCruiseVelocity = Units.radiansToRotations(TurretConstants.MAX_VELOCITY / TurretConstants.TURRET_RADIUS) * gearRatio; // max velocity * gear ratio
+        motionMagicConfigs.MotionMagicCruiseVelocity = Units.radiansToRotations(TurretConstants.MAX_ACCELERATION / TurretConstants.TURRET_RADIUS) * gearRatio; // max Acceleration * gear ratio
 
         config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
         motor.getConfigurator().apply(config);
@@ -180,7 +180,7 @@ public class Turret extends SubsystemBase {
             double motorTargetRotations = (setpoint / 360.0) * gearRatio;
 
             //Tune this with rotating robot
-            double dV = 0;
+            double dV = TurretConstants.ROTATIONAL_VELOCITY_CONSTANT;
             motor.setControl(voltageRequest.withPosition(motorTargetRotations).withFeedForward(dV * robotRotVel));
         }
     }

diff --git a/src/main/java/frc/robot/subsystems/turret/TurretConstants.java b/src/main/java/frc/robot/subsystems/turret/TurretConstants.java
index c505ce2e08ab04f50dc11bf53398efaf15d1e04e..569cc389a3eabbfa07ab64a69f163198778f254f 100644 (file)
--- a/src/main/java/frc/robot/subsystems/turret/TurretConstants.java
+++ b/src/main/java/frc/robot/subsystems/turret/TurretConstants.java
@@ -1,6 +1,16 @@
 package frc.robot.subsystems.turret;
 
+import edu.wpi.first.math.util.Units;
+
 public class TurretConstants {
-    public static double MAX_ANGLE = 150;
-    public static double MIN_ANGLE = -150;
+    public static double MAX_ANGLE = 180;
+    public static double MIN_ANGLE = -180;
+
+    public static double MAX_VELOCITY = 0.1; // m/s
+    public static double MAX_ACCELERATION = 5; // m/s^2
+
+    public static double TURRET_WIDTH = Units.feetToMeters(1.0);
+    public static double TURRET_RADIUS = TURRET_WIDTH / 2;
+
+    public static double ROTATIONAL_VELOCITY_CONSTANT = 0;
 }