SHARE
TWEET

Untitled

a guest Feb 14th, 2020 81 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. package frc.robot;
  2.  
  3. import com.ctre.phoenix.motorcontrol.ControlMode;
  4. import com.ctre.phoenix.motorcontrol.FeedbackDevice;
  5. import com.ctre.phoenix.motorcontrol.can.TalonSRX;
  6. import com.revrobotics.CANSparkMax;
  7. import com.revrobotics.CANSparkMaxLowLevel.MotorType;
  8.  
  9. import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
  10.  
  11. public class SwerveModule {
  12.  
  13.     private TalonSRX steer;
  14.     private CANSparkMax drive;
  15.  
  16.     private String name;
  17.  
  18.     private double offset;
  19.    
  20.     public SwerveModule(int steerPort, int drivePort, String name, double offset) {
  21.         this.steer = new TalonSRX(steerPort);
  22.         this.drive = new CANSparkMax(drivePort, MotorType.kBrushless);
  23.         this.name = name;
  24.         this.offset = offset / 360.0 * 4096;
  25.         configure();
  26.     }
  27.  
  28.     public void configure() {
  29.  
  30.         int kTimeoutMs = 10;
  31.         int kPIDLoopIdx = 0;
  32.         int rampvelocity = 15000;
  33.         int rampaccel = 16000;
  34.         int absolutePosition = steer.getSelectedSensorPosition(kTimeoutMs) & 0xFFF;
  35.         int P = 3;
  36.         int I = 0;
  37.         int D = 0;
  38.         int F = 0;
  39.  
  40.         steer.setSelectedSensorPosition(absolutePosition, kPIDLoopIdx, kTimeoutMs);
  41.         steer.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, kPIDLoopIdx, kTimeoutMs);
  42.         steer.setSensorPhase(true);
  43.         steer.setInverted(false);
  44.  
  45.         steer.selectProfileSlot(0, kPIDLoopIdx);
  46.         steer.configNominalOutputForward(0, kTimeoutMs);
  47.         steer.configNominalOutputReverse(0, kTimeoutMs);
  48.         steer.configPeakOutputForward(1, kTimeoutMs);
  49.         steer.configPeakOutputReverse(-1, kTimeoutMs);
  50.         steer.configAllowableClosedloopError(0, kPIDLoopIdx, kTimeoutMs);
  51.         steer.configMotionCruiseVelocity(rampvelocity, kTimeoutMs);
  52.         steer.configMotionAcceleration(rampaccel, kTimeoutMs);
  53.  
  54.         steer.config_kF(kPIDLoopIdx, F, kTimeoutMs);
  55.         steer.config_kP(kPIDLoopIdx, P, kTimeoutMs);
  56.         steer.config_kI(kPIDLoopIdx, I, kTimeoutMs);
  57.         steer.config_kD(kPIDLoopIdx, D, kTimeoutMs);
  58.     }
  59.  
  60.  
  61.     public void setAngle(double angle){
  62.         steer.set(ControlMode.MotionMagic,  ( angle / 360 * 4096 ) );
  63.     }
  64.  
  65.     public void stop() {
  66.         steer.set(ControlMode.PercentOutput, 0);
  67.         drive.set(0);
  68.     }
  69.    
  70.     public double getAngle() {
  71.         double angle = steer.getSelectedSensorPosition(0) / (4096.0 / 360.0);
  72.         SmartDashboard.putNumber(name + " Angle", angle);
  73.         return angle;
  74.     }
  75.  
  76.     /**
  77.      * @angle in degrees
  78.      * @speed in percent output
  79.      */
  80.     public void drive(double speed, double angle) {
  81.         drive.set(speed);
  82.         setAngle(angle);
  83.     }
  84.  
  85.     public void reset() {
  86.         steer.setSelectedSensorPosition(0);
  87.     }
  88. }
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
Top