Si besoin de Sysid. Code retirer lors de ce commit
This commit is contained in:
		| @@ -22,7 +22,6 @@ import edu.wpi.first.wpilibj.Notifier; | |||||||
| import edu.wpi.first.wpilibj.RobotController; | import edu.wpi.first.wpilibj.RobotController; | ||||||
| import edu.wpi.first.wpilibj2.command.Command; | import edu.wpi.first.wpilibj2.command.Command; | ||||||
| import edu.wpi.first.wpilibj2.command.Subsystem; | import edu.wpi.first.wpilibj2.command.Subsystem; | ||||||
| import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; |  | ||||||
| import frc.robot.TunerConstants.TunerConstants.TunerSwerveDrivetrain; | import frc.robot.TunerConstants.TunerConstants.TunerSwerveDrivetrain; | ||||||
|  |  | ||||||
| /** | /** | ||||||
| @@ -44,71 +43,8 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su | |||||||
|  |  | ||||||
|     private final SwerveRequest.ApplyRobotSpeeds m_pathApplyRobotSpeeds = new SwerveRequest.ApplyRobotSpeeds(); |     private final SwerveRequest.ApplyRobotSpeeds m_pathApplyRobotSpeeds = new SwerveRequest.ApplyRobotSpeeds(); | ||||||
|  |  | ||||||
|     /* Swerve requests to apply during SysId characterization */ |  | ||||||
|     private final SwerveRequest.SysIdSwerveTranslation m_translationCharacterization = new SwerveRequest.SysIdSwerveTranslation(); |  | ||||||
|     private final SwerveRequest.SysIdSwerveSteerGains m_steerCharacterization = new SwerveRequest.SysIdSwerveSteerGains(); |  | ||||||
|     private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = new SwerveRequest.SysIdSwerveRotation(); |  | ||||||
|  |  | ||||||
|     /* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */ |     /* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */ | ||||||
|  |  | ||||||
|     private final SysIdRoutine m_sysIdRoutineTranslation = new SysIdRoutine( |  | ||||||
|         new SysIdRoutine.Config( |  | ||||||
|             null,        // Use default ramp rate (1 V/s) |  | ||||||
|             Volts.of(4), // Reduce dynamic step voltage to 4 V to prevent brownout |  | ||||||
|             null,        // Use default timeout (10 s) |  | ||||||
|             // Log state with SignalLogger class |  | ||||||
|             state -> SignalLogger.writeString("SysIdTranslation_State", state.toString()) |  | ||||||
|         ), |  | ||||||
|         new SysIdRoutine.Mechanism( |  | ||||||
|             output -> setControl(m_translationCharacterization.withVolts(output)), |  | ||||||
|             null, |  | ||||||
|             this |  | ||||||
|         ) |  | ||||||
|     ); |  | ||||||
|  |  | ||||||
|     /* SysId routine for characterizing steer. This is used to find PID gains for the steer motors. */ |  | ||||||
|     private final SysIdRoutine m_sysIdRoutineSteer = new SysIdRoutine( |  | ||||||
|         new SysIdRoutine.Config( |  | ||||||
|             null,        // Use default ramp rate (1 V/s) |  | ||||||
|             Volts.of(7), // Use dynamic voltage of 7 V |  | ||||||
|             null,        // Use default timeout (10 s) |  | ||||||
|             // Log state with SignalLogger class |  | ||||||
|             state -> SignalLogger.writeString("SysIdSteer_State", state.toString()) |  | ||||||
|         ), |  | ||||||
|         new SysIdRoutine.Mechanism( |  | ||||||
|             volts -> setControl(m_steerCharacterization.withVolts(volts)), |  | ||||||
|             null, |  | ||||||
|             this |  | ||||||
|         ) |  | ||||||
|     ); |  | ||||||
|  |  | ||||||
|     /* |  | ||||||
|      * SysId routine for characterizing rotation. |  | ||||||
|      * This is used to find PID gains for the FieldCentricFacingAngle HeadingController. |  | ||||||
|      * See the documentation of SwerveRequest.SysIdSwerveRotation for info on importing the log to SysId. |  | ||||||
|      */ |  | ||||||
|     private final SysIdRoutine m_sysIdRoutineRotation = new SysIdRoutine( |  | ||||||
|         new SysIdRoutine.Config( |  | ||||||
|             /* This is in radians per second², but SysId only supports "volts per second" */ |  | ||||||
|             Volts.of(Math.PI / 6).per(Second), |  | ||||||
|             /* This is in radians per second, but SysId only supports "volts" */ |  | ||||||
|             Volts.of(Math.PI), |  | ||||||
|             null, // Use default timeout (10 s) |  | ||||||
|             // Log state with SignalLogger class |  | ||||||
|             state -> SignalLogger.writeString("SysIdRotation_State", state.toString()) |  | ||||||
|         ), |  | ||||||
|         new SysIdRoutine.Mechanism( |  | ||||||
|             output -> { |  | ||||||
|                 /* output is actually radians per second, but SysId only supports "volts" */ |  | ||||||
|                 setControl(m_rotationCharacterization.withRotationalRate(output.in(Volts))); |  | ||||||
|                 /* also log the requested output for SysId */ |  | ||||||
|                 SignalLogger.writeDouble("Rotational_Rate", output.in(Volts)); |  | ||||||
|             }, |  | ||||||
|             null, |  | ||||||
|             this |  | ||||||
|         ) |  | ||||||
|     ); |  | ||||||
|      |  | ||||||
|     private void configureAutoBuilder() { |     private void configureAutoBuilder() { | ||||||
|         try { |         try { | ||||||
|             var config = RobotConfig.fromGUISettings(); |             var config = RobotConfig.fromGUISettings(); | ||||||
| @@ -140,9 +76,6 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su | |||||||
|             return 0; |             return 0; | ||||||
|          });   |          });   | ||||||
|         } |         } | ||||||
|   |  | ||||||
|     /* The SysId routine to test */ |  | ||||||
|     private SysIdRoutine m_sysIdRoutineToApply = m_sysIdRoutineSteer; |  | ||||||
|  |  | ||||||
|     /** |     /** | ||||||
|      * Constructs a CTRE SwerveDrivetrain using the specified constants. |      * Constructs a CTRE SwerveDrivetrain using the specified constants. | ||||||
| @@ -234,28 +167,6 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su | |||||||
|     public Command applyRequest(Supplier<SwerveRequest> requestSupplier) { |     public Command applyRequest(Supplier<SwerveRequest> requestSupplier) { | ||||||
|         return run(() -> this.setControl(requestSupplier.get())); |         return run(() -> this.setControl(requestSupplier.get())); | ||||||
|     } |     } | ||||||
|  |  | ||||||
|     /** |  | ||||||
|      * Runs the SysId Quasistatic test in the given direction for the routine |  | ||||||
|      * specified by {@link #m_sysIdRoutineToApply}. |  | ||||||
|      * |  | ||||||
|      * @param direction Direction of the SysId Quasistatic test |  | ||||||
|      * @return Command to run |  | ||||||
|      */ |  | ||||||
|     public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { |  | ||||||
|         return m_sysIdRoutineToApply.quasistatic(direction); |  | ||||||
|     } |  | ||||||
|  |  | ||||||
|     /** |  | ||||||
|      * Runs the SysId Dynamic test in the given direction for the routine |  | ||||||
|      * specified by {@link #m_sysIdRoutineToApply}. |  | ||||||
|      * |  | ||||||
|      * @param direction Direction of the SysId Dynamic test |  | ||||||
|      * @return Command to run |  | ||||||
|      */ |  | ||||||
|     public Command sysIdDynamic(SysIdRoutine.Direction direction) { |  | ||||||
|         return m_sysIdRoutineToApply.dynamic(direction); |  | ||||||
|     } |  | ||||||
|      |      | ||||||
|     @Override |     @Override | ||||||
|     public void periodic() { |     public void periodic() { | ||||||
|   | |||||||
		Reference in New Issue
	
	Block a user