vision pose estimator (#1)

FRC-Team-1160 · Jan 14, 2025 · 23ffc58 · 23ffc58
2 parents 5770edb + 77465df
commit 23ffc58
Show file tree

Hide file tree

Showing 3 changed files with 20 additions and 4 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -134,6 +134,10 @@ public static class SteerMotorConfigs {
       public static final double kV = 0;
       public static final double kA = 0;
       public static final double kG = 0;
+
+      public static final double mm_cruise_velocity = 0.5;
+      public static final double mm_acceleration = 1.0;
+      public static final double mm_jerk = 0;
     }
   }
 

diff --git a/src/main/java/frc/robot/Subsystems/DriveTrain/DriveTrain.java b/src/main/java/frc/robot/Subsystems/DriveTrain/DriveTrain.java
@@ -117,7 +117,7 @@ public void setSwerveDrive(ChassisSpeeds chassis_speeds){
     SwerveDriveKinematics.desaturateWheelSpeeds(m_module_states, SwerveConstants.MAX_SPEED); 
 
     setModules(m_module_states);
-
+    
   }
 
   /**

diff --git a/src/main/java/frc/robot/Subsystems/DriveTrain/SwerveModuleRealIO.java b/src/main/java/frc/robot/Subsystems/DriveTrain/SwerveModuleRealIO.java
@@ -4,7 +4,10 @@
 
 package frc.robot.Subsystems.DriveTrain;
 
+import com.ctre.phoenix6.configs.MotionMagicConfigs;
 import com.ctre.phoenix6.configs.Slot0Configs;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.MotionMagicVoltage;
 import com.ctre.phoenix6.controls.VelocityVoltage;
 import com.ctre.phoenix6.hardware.CANcoder;
 import com.ctre.phoenix6.hardware.TalonFX;
@@ -34,7 +37,7 @@ public SwerveModuleRealIO(int drive_port, int steer_port, int sensor_port){
     drive_motor = new TalonFX(drive_port, "CANivore");
     steer_motor = new TalonFX(steer_port, "CANivore");
     steer_sensor = new CANcoder(sensor_port, "CANivore");
-
+    
     Slot0Configs driveConfigs = new Slot0Configs()
       .withKP(DriveMotorConfigs.kP)
       .withKI(DriveMotorConfigs.kI)
@@ -43,16 +46,25 @@ public SwerveModuleRealIO(int drive_port, int steer_port, int sensor_port){
       .withKV(DriveMotorConfigs.kV)
       .withKA(DriveMotorConfigs.kA)
       .withKG(DriveMotorConfigs.kG);
+
     drive_motor.getConfigurator().apply(driveConfigs);
 
-    Slot0Configs steerConfigs = new Slot0Configs()
+    TalonFXConfiguration steerConfigs = new TalonFXConfiguration();
+
+    steerConfigs.Slot0 = new Slot0Configs()
       .withKP(SteerMotorConfigs.kP)
       .withKI(SteerMotorConfigs.kI)
       .withKD(SteerMotorConfigs.kD)
       .withKS(SteerMotorConfigs.kS)
       .withKV(SteerMotorConfigs.kV)
       .withKA(SteerMotorConfigs.kA)
       .withKG(SteerMotorConfigs.kG);
+
+    steerConfigs.MotionMagic = new MotionMagicConfigs()
+      .withMotionMagicCruiseVelocity(SteerMotorConfigs.mm_cruise_velocity)
+      .withMotionMagicAcceleration(SteerMotorConfigs.mm_acceleration)
+      .withMotionMagicJerk(SteerMotorConfigs.mm_jerk);
+
     steer_motor.getConfigurator().apply(steerConfigs);
 
   }
@@ -91,7 +103,7 @@ public void setSpeed(double speedMetersPerSecond){
 
   public void setAngle(Rotation2d angle){
     steer_motor.setControl(new VelocityVoltage(0));
-    // steer_motor.setVoltage(volts);
+    // steer_motor.setControl(new MotionMagicVoltage(angle.getRotations()));
   }
 
 }
-Original file line number
+Diff line change
@@ Expand Up / @@ -117,7 +117,7 @@ public void setSwerveDrive(ChassisSpeeds chassis_speeds){ @@
         SwerveDriveKinematics.desaturateWheelSpeeds(m_module_states, SwerveConstants.MAX_SPEED);
         setModules(m_module_states);
       }
       /**
@@ Expand Down @@