stuff

diff --git a/gradlew b/gradlew
old mode 100644 (file)
new mode 100755 (executable)

diff --git a/simgui-ds.json b/simgui-ds.json
new file mode 100644 (file)
index 0000000..73cc713
--- /dev/null
+++ b/simgui-ds.json
@@ -0,0 +1,92 @@
+{
+  "keyboardJoysticks": [
+    {
+      "axisConfig": [
+        {
+          "decKey": 65,
+          "incKey": 68
+        },
+        {
+          "decKey": 87,
+          "incKey": 83
+        },
+        {
+          "decKey": 69,
+          "decayRate": 0.0,
+          "incKey": 82,
+          "keyRate": 0.009999999776482582
+        }
+      ],
+      "axisCount": 3,
+      "buttonCount": 4,
+      "buttonKeys": [
+        90,
+        88,
+        67,
+        86
+      ],
+      "povConfig": [
+        {
+          "key0": 328,
+          "key135": 323,
+          "key180": 322,
+          "key225": 321,
+          "key270": 324,
+          "key315": 327,
+          "key45": 329,
+          "key90": 326
+        }
+      ],
+      "povCount": 1
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 74,
+          "incKey": 76
+        },
+        {
+          "decKey": 73,
+          "incKey": 75
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 4,
+      "buttonKeys": [
+        77,
+        44,
+        46,
+        47
+      ],
+      "povCount": 0
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 263,
+          "incKey": 262
+        },
+        {
+          "decKey": 265,
+          "incKey": 264
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 6,
+      "buttonKeys": [
+        260,
+        268,
+        266,
+        261,
+        269,
+        267
+      ],
+      "povCount": 0
+    },
+    {
+      "axisCount": 0,
+      "buttonCount": 0,
+      "povCount": 0
+    }
+  ]
+}

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 49efa97e1f9b72a9d42d08e1c7b07ca85f37fde8..52c3f123c681d9be58a74929494d95135fbcde21 100644 (file)
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -24,6 +24,7 @@ import frc.robot.controls.Operator;
 import frc.robot.controls.PS5ControllerDriverConfig;
 import frc.robot.subsystems.drivetrain.Drivetrain;
 import frc.robot.subsystems.drivetrain.GyroIOPigeon2;
+import frc.robot.subsystems.turret.Turret;
 import frc.robot.util.PathGroupLoader;
 import frc.robot.util.Vision.DetectedObject;
 import frc.robot.util.Vision.Vision;
@@ -41,6 +42,7 @@ public class RobotContainer {
   // The robot's subsystems are defined here...
   private Drivetrain drive = null;
   private Vision vision = null;
+  private Turret turret = null;
   private Command auto = new DoNothing();
 
   // Controllers are defined here
@@ -53,6 +55,7 @@ public class RobotContainer {
    * Different robots may have different subsystems.
    */
   public RobotContainer(RobotId robotId) {
+    turret = new Turret();
     // dispatch on the robot
     switch (robotId) {
       case TestBed1:

diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java
index 25c68e75e335bc72c726e3450a61e2c4a1b5574f..01f49d6c07cb6f2d1b9be4f910adb72cf5648b03 100644 (file)
--- a/src/main/java/frc/robot/subsystems/turret/Turret.java
+++ b/src/main/java/frc/robot/subsystems/turret/Turret.java
@@ -42,7 +42,7 @@ public class Turret extends SubsystemBase {
     private SingleJointedArmSim turretSim;
     private static final DCMotor turretMotorSim = DCMotor.getKrakenX60(1);
     private TalonFXSimState encoderSim;
-    private MotionMagicVoltage voltageRequest = new MotionMagicVoltage(Units.degreesToRotations(0) * 1); // 1 is placeholder for gear ratio
+    private MotionMagicVoltage voltageRequest = new MotionMagicVoltage(Units.degreesToRotations(0) * gearRatio); // gear ratio
     private double setpoint = 0;
     Mechanism2d mechanism2d = new Mechanism2d(100, 100);
     MechanismRoot2d mechanismRoot = mechanism2d.getRoot("pivot", 50, 50);
@@ -70,7 +70,7 @@ public class Turret extends SubsystemBase {
 
         TalonFXConfiguration config = new TalonFXConfiguration();
         CurrentLimitsConfigs limitConfig = new CurrentLimitsConfigs();
-        limitConfig.StatorCurrentLimit = 40;
+        limitConfig.StatorCurrentLimit = 200;
         limitConfig.StatorCurrentLimitEnable = true;
 
         motor.getConfigurator().apply(limitConfig);
@@ -86,21 +86,30 @@ 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(0.1) * gearRatio; // max velocity * gear ratio
-        motionMagicConfigs.MotionMagicAcceleration = Units.degreesToRotations(0.1) * gearRatio; // max Acceleration * gear ratio
+        motionMagicConfigs.MotionMagicCruiseVelocity = Units.degreesToRotations(90) * gearRatio; // max velocity * gear ratio
+        motionMagicConfigs.MotionMagicAcceleration = Units.degreesToRotations(90) * gearRatio; // max Acceleration * gear ratio
 
         config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
         motor.getConfigurator().apply(config);
 
-        config.SoftwareLimitSwitch.ForwardSoftLimitEnable = true;
+        // config.ClosedLoopGeneral.ContinuousWrap = true;
+
+        motor.getConfigurator().apply(config);
+
+        config.SoftwareLimitSwitch.ForwardSoftLimitEnable = false;
         config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = Units.degreesToRotations(360) * gearRatio; // max angle * gear ratio
         
-        config.SoftwareLimitSwitch.ReverseSoftLimitEnable = true;
+        config.SoftwareLimitSwitch.ReverseSoftLimitEnable = false;
         config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = Units.degreesToRotations(0) * gearRatio; // min angle * gear ratio
         
         SmartDashboard.putData("turret", mechanism2d);
         SmartDashboard.putData("PID", pid);
 
+        SmartDashboard.putData("Set to 0 degrees", new InstantCommand(() -> setSetpoint(0)));
+        SmartDashboard.putData("Set to 90 degrees", new InstantCommand(( )-> setSetpoint(90)));
+        SmartDashboard.putData("Set to 180 degrees", new InstantCommand(() -> setSetpoint(180)));
+        SmartDashboard.putData("Set to 270 degrees", new InstantCommand(() -> setSetpoint(270)));
+
         SmartDashboard.putData("Set to 1,1", new InstantCommand(() -> setTarget(1,1)));
         SmartDashboard.putData("Set to -1,1", new InstantCommand(( )-> setTarget(-1,1)));
         SmartDashboard.putData("Set to -1,-1", new InstantCommand(() -> setTarget(-1,-1)));
@@ -124,13 +133,17 @@ public class Turret extends SubsystemBase {
          return motor.getPosition().getValueAsDouble() / gearRatio; // Gear ratio
     }
 
-    public void setSetpoint(double setpointDegrees) {
-        // If you want to handle the wrap-around ( -180 to 180)
-        double clampedSetpoint = MathUtil.inputModulus(setpointDegrees, -180, 180);
-        this.setpoint = clampedSetpoint;
+    // public void setSetpoint(double setpointDegrees) {
+    //     double clampedSetpoint = MathUtil.inputModulus(setpointDegrees, 0, 360);
+    //     this.setpoint = clampedSetpoint;
     
-        // Convert mechanism degrees to motor rotations: (Degrees / 360) * GearRatio
-        double motorTargetRotations = (clampedSetpoint / 360.0) * gearRatio;
+    //     // Convert mechanism degrees to motor rotations: (Degrees / 360) * GearRatio
+    //     double motorTargetRotations = (clampedSetpoint / 360.0) * gearRatio;
+    //     motor.setControl(voltageRequest.withPosition(motorTargetRotations));
+    // }
+
+    public void setSetpoint(double setpointDegrees) {
+        double motorTargetRotations = (setpointDegrees / 360.0) * gearRatio;
         motor.setControl(voltageRequest.withPosition(motorTargetRotations));
     }
 
@@ -147,7 +160,6 @@ public class Turret extends SubsystemBase {
         // SmartDashboard.putNumber("Turret Position", position);
         // align();
         ligament2d.setAngle(position);
-
     }
 
     public double getAppliedVoltage() {