ijiooij

diff --git a/src/main/java/frc/robot/commands/gpm/AimAtPose.java b/src/main/java/frc/robot/commands/gpm/AimAtPose.java
index 6d583284ae5e36bf0587bcd1403572e6d762a823..4b204d325e57468ade0aa52d7d54a96b816025ab 100644 (file)
--- a/src/main/java/frc/robot/commands/gpm/AimAtPose.java
+++ b/src/main/java/frc/robot/commands/gpm/AimAtPose.java
@@ -38,7 +38,7 @@ public class AimAtPose extends Command {
   @Override
   public void execute() {
     //Logger.recordOutput("TurretPose", new Pose2d(drive.getPose().getTranslation(), turret.getPosition()));
-    turret.setSetpoint(-target.getTranslation().minus(drive.getPose().getTranslation()).getAngle().getDegrees(), 0);
+    //.setSetpoint(-target.getTranslation().minus(drive.getPose().getTranslation()).getAngle().getDegrees(), 0);
   }
 
   /**

diff --git a/src/main/java/frc/robot/commands/gpm/TurretJoyStickAim.java b/src/main/java/frc/robot/commands/gpm/TurretJoyStickAim.java
index 90e8be27a481d7014a7f5fd92ffbdafca93e7b09..06e42cbd8bf4d9188223d0dbc999a222bddb4507 100644 (file)
--- a/src/main/java/frc/robot/commands/gpm/TurretJoyStickAim.java
+++ b/src/main/java/frc/robot/commands/gpm/TurretJoyStickAim.java
@@ -21,7 +21,7 @@ public class TurretJoyStickAim extends Command{
 
     @Override
     public void execute() {
-        turret.setSetpoint(angle, 0);
+        //turret.setSetpoint(angle, 0);
     }
 
 }

diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java
index 1e7b43a3667fe2cacd7d864fa70bc444b4298b37..a92ef606fa99ddecb37a7671b116579ca02874cb 100644 (file)
--- a/src/main/java/frc/robot/subsystems/turret/Turret.java
+++ b/src/main/java/frc/robot/subsystems/turret/Turret.java
@@ -30,8 +30,8 @@ public class Turret extends SubsystemBase {
     private static final double MIN_ANGLE_RAD = Units.degreesToRadians(-180.0);
     private static final double MAX_ANGLE_RAD = Units.degreesToRadians(180.0);
 
-    private static final double MAX_VEL_RAD_PER_SEC = Units.degreesToRadians(360);
-    private static final double MAX_ACCEL_RAD_PER_SEC2 = Units.degreesToRadians(720);
+    private static double MAX_VEL_RAD_PER_SEC = 4*Units.degreesToRadians(360);
+    private static double MAX_ACCEL_RAD_PER_SEC2 = 4*Units.degreesToRadians(720);
 
     private static final double VERSA_RATIO = 5.0;
     private static final double TURRET_RATIO = 140.0 / 10.0;
@@ -126,6 +126,13 @@ public class Turret extends SubsystemBase {
 
     @Override
     public void periodic() {
+        MAX_VEL_RAD_PER_SEC = SmartDashboard.getNumber("Max Velocity", MAX_VEL_RAD_PER_SEC);
+        MAX_ACCEL_RAD_PER_SEC2 = SmartDashboard.getNumber("Max Accleration", MAX_ACCEL_RAD_PER_SEC2);
+        kP = SmartDashboard.getNumber("kP Value", MAX_VEL_RAD_PER_SEC);
+        SmartDashboard.putNumber("Max Velocity", MAX_VEL_RAD_PER_SEC);
+        SmartDashboard.putNumber("Max Acceleration", MAX_ACCEL_RAD_PER_SEC2);
+        SmartDashboard.putNumber("kP Value", kP);
+
         double robotRelativeGoal = goalAngle.getRadians();
 
         // MA-style continuous optimization