dirtbikerxz

Untitled

Jan 19th, 2021
963
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. package com.team364.frc2020;
  2.  
  3. import edu.wpi.first.wpilibj.geometry.Rotation2d;
  4. import edu.wpi.first.wpilibj.kinematics.SwerveModuleState;
  5. import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
  6. import edu.wpi.first.wpilibj.util.Units;
  7.  
  8. import com.team364.frc2020.States.*;
  9. import static com.team364.lib.math.Boundaries.*;
  10. import static com.team364.lib.math.Conversions.*;
  11.  
  12. import com.ctre.phoenix.motorcontrol.ControlMode;
  13. import com.ctre.phoenix.motorcontrol.StatusFrame;
  14. import com.ctre.phoenix.motorcontrol.can.TalonFX;
  15. import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration;
  16. import com.ctre.phoenix.sensors.CANCoder;
  17. import com.ctre.phoenix.sensors.CANCoderConfiguration;
  18.  
  19. public class SwerveMod {
  20.     public int moduleNumber;
  21.     private double offset;
  22.     private TalonFX mAngleMotor;
  23.     private TalonFX mDriveMotor;
  24.     private CANCoder angleEncoder;
  25.     private TalonFXConfiguration angleMotorConfig;
  26.     private TalonFXConfiguration driveMotorConfig;
  27.     private CANCoderConfiguration canCoderConfig;
  28.     private boolean invertDrive;
  29.     private boolean invertAngle;
  30.     private double lastAngle;
  31.  
  32.     public SwerveMod(int moduleNumber, double offset, int angleMotorID, int angleEncoderID, int driveMotorID,
  33.     boolean invertDrive, boolean invertAngle, TalonFXConfiguration angleMotorConfig, TalonFXConfiguration driveMotorConfig, CANCoderConfiguration canCoderConfig){
  34.         this.moduleNumber = moduleNumber;
  35.         this.offset = offset;
  36.         this.angleMotorConfig = angleMotorConfig;
  37.         this.driveMotorConfig = driveMotorConfig;
  38.         this.canCoderConfig = canCoderConfig;
  39.         this.invertDrive = invertDrive;
  40.         this.invertAngle = invertAngle;
  41.        
  42.         /* Angle Encoder Config */
  43.         angleEncoder = new CANCoder(angleEncoderID);
  44.         configAngleEncoder();
  45.  
  46.         /* Angle Motor Config */
  47.         mAngleMotor = new TalonFX(angleMotorID);
  48.         configAngleMotor();
  49.  
  50.         /* Drive Motor Config */
  51.         mDriveMotor = new TalonFX(driveMotorID);
  52.         configDriveMotor();
  53.  
  54.         lastAngle = getAngle();
  55.     }
  56.  
  57.     public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop){
  58.         SwerveModuleState state = desiredState;
  59.         state = SwerveModuleState.optimize(desiredState, newGetAngle());
  60.  
  61.         if(isOpenLoop){
  62.             double percentOutput = state.speedMetersPerSecond / Constants.Swerve.maxSpeed;
  63.             percentOutput = (States.driveState == DriveStates.openLoopDampened) ? (percentOutput *= Constants.Swerve.driveDampenRatio) : percentOutput;
  64.             mDriveMotor.set(ControlMode.PercentOutput, 0);
  65.             SmartDashboard.putNumber("percentOutput " + moduleNumber + ": ", percentOutput);
  66.         }
  67.         else {
  68.             mDriveMotor.set(ControlMode.Velocity, toFalconFromMetersPerSecond(state.speedMetersPerSecond));
  69.         }
  70.  
  71.         double angle = (Math.abs(state.speedMetersPerSecond) <= (Constants.Swerve.maxSpeed * 0.1)) ? lastAngle : state.angle.getDegrees();
  72.         mAngleMotor.set(ControlMode.Position, toFalconFromDegrees(angle, Constants.Swerve.angleGearRatio));
  73.         lastAngle = angle;
  74.        
  75.         SmartDashboard.putNumber("sAngle " + moduleNumber + ": ", angle);
  76.        
  77.         SmartDashboard.putNumber("sCAngle " + moduleNumber + ": ", getAngle());
  78.     }
  79.  
  80.     private void resetToAbsolute(){
  81.         int absolutePosition = toFalconFromDegrees(getCanCoder() + offset, Constants.Swerve.angleGearRatio);
  82.         mAngleMotor.setSelectedSensorPosition(absolutePosition);
  83.     }
  84.  
  85.     private void configAngleEncoder(){        
  86.         angleEncoder.configFactoryDefault();
  87.         angleEncoder.configAllSettings(canCoderConfig);
  88.     }
  89.  
  90.     private void configAngleMotor(){
  91.         mAngleMotor.configFactoryDefault();
  92.         mAngleMotor.configAllSettings(angleMotorConfig, Constants.ctreTimeout);
  93.         mAngleMotor.setInverted(invertAngle);
  94.         mAngleMotor.setNeutralMode(Constants.Swerve.angleNeutralMode);
  95.         mAngleMotor.setStatusFramePeriod(StatusFrame.Status_1_General, Constants.Swerve.angleStatus1, Constants.ctreTimeout);
  96.         mAngleMotor.setStatusFramePeriod(StatusFrame.Status_2_Feedback0, Constants.Swerve.angleStatus2, Constants.ctreTimeout);        
  97.         resetToAbsolute();
  98.     }
  99.  
  100.     private void configDriveMotor(){        
  101.         mDriveMotor.configFactoryDefault();
  102.         mDriveMotor.configAllSettings(driveMotorConfig);
  103.         mDriveMotor.setInverted(invertDrive);
  104.         mDriveMotor.setNeutralMode(Constants.Swerve.driveNeutralMode);
  105.         mDriveMotor.setStatusFramePeriod(StatusFrame.Status_1_General, Constants.Swerve.driveStatus1);
  106.         mDriveMotor.setStatusFramePeriod(StatusFrame.Status_2_Feedback0, Constants.Swerve.driveStatus2);
  107.     }
  108.  
  109.     public double getCanCoder(){
  110.         return angleEncoder.getAbsolutePosition();
  111.     }
  112.  
  113.     public double getAngle(){
  114.         return toDegreesFromFalcon(mAngleMotor.getSelectedSensorPosition(), Constants.Swerve.angleGearRatio);
  115.     }
  116.  
  117.     public Rotation2d newGetAngle() {
  118.         Rotation2d angle = Rotation2d.fromDegrees(
  119.                 toDegreesFromFalcon(mAngleMotor.getSelectedSensorPosition(), Constants.Swerve.angleGearRatio) % 360)
  120.                 .rotateBy(new Rotation2d());
  121.         return angle;
  122.     }
  123.  
  124.     public double getVelocity(){
  125.         double wheelRPM = toRPMFromFalcon(mDriveMotor.getSelectedSensorVelocity(), Constants.Swerve.driveGearRatio);
  126.         double wheelIPS = (wheelRPM * Constants.Swerve.wheelCircumference) / 60; //Inches per Second
  127.         double wheelMPS = Units.inchesToMeters(wheelIPS); //Meters per Second
  128.         return wheelMPS;
  129.     }
  130.  
  131.     public SwerveModuleState getState(){
  132.         return new SwerveModuleState(getVelocity(), Rotation2d.fromDegrees(getAngle()));
  133.     }
  134.    
  135. }
RAW Paste Data