diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java index edb1f3d..cb7e073 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java @@ -58,13 +58,13 @@ public ModuleIOSparkMax(int index) { absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED break; case 1: - driveSparkMax = new CANSparkMax(3, MotorType.kBrushless); - turnSparkMax = new CANSparkMax(4, MotorType.kBrushless); + driveSparkMax = new CANSparkMax(9, MotorType.kBrushless); + turnSparkMax = new CANSparkMax(10, MotorType.kBrushless); turnAbsoluteEncoder = new AnalogInput(1); absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED break; case 2: - driveSparkMax = new CANSparkMax(5, MotorType.kBrushless); + driveSparkMax = new CANSparkMax(4, MotorType.kBrushless); turnSparkMax = new CANSparkMax(6, MotorType.kBrushless); turnAbsoluteEncoder = new AnalogInput(2); absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index 3ac7650..deb74f1 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -1,7 +1,7 @@ package frc.robot.subsystems.intake; public class IntakeConstants { - public static final int CanID = 2; // .. + public static final int CanID = 5; // .. public static final int currentLimit = 40; // vortex public static final int current = 10; // .. } diff --git a/src/main/java/frc/robot/subsystems/pivot/PivotIOReal.java b/src/main/java/frc/robot/subsystems/pivot/PivotIOReal.java index e48ca28..a8436cc 100644 --- a/src/main/java/frc/robot/subsystems/pivot/PivotIOReal.java +++ b/src/main/java/frc/robot/subsystems/pivot/PivotIOReal.java @@ -8,9 +8,8 @@ import edu.wpi.first.math.controller.PIDController; public class PivotIOReal implements PivotIO { - private CANSparkFlex pivotLeft = new CANSparkFlex(10, MotorType.kBrushless); - private CANSparkFlex pivotRight = new CANSparkFlex(11, MotorType.kBrushless); - private RelativeEncoder pivotEncoderRight = pivotRight.getEncoder(); + private CANSparkFlex pivotMotor = new CANSparkFlex(11, MotorType.kBrushless); + private RelativeEncoder pivotEncoderRight = pivotMotor.getEncoder(); private PIDController pid = new PIDController(1.0, 0.0, 0.0); private boolean closedLoop = false; @@ -18,23 +17,14 @@ public class PivotIOReal implements PivotIO { private double appliedVolts = 0.0; public PivotIOReal() { - pivotLeft.restoreFactoryDefaults(); - pivotRight.restoreFactoryDefaults(); - - pivotLeft.follow(pivotRight); - pivotLeft.setInverted(true); - - pivotLeft.setCANTimeout(250); - pivotRight.setCANTimeout(250); - pivotLeft.enableVoltageCompensation(12); - pivotRight.enableVoltageCompensation(12); - pivotLeft.setSmartCurrentLimit(80); - pivotRight.setSmartCurrentLimit(80); - pivotLeft.setIdleMode(IdleMode.kBrake); - pivotRight.setIdleMode(IdleMode.kBrake); - - pivotLeft.burnFlash(); - pivotRight.burnFlash(); + pivotMotor.restoreFactoryDefaults(); + + pivotMotor.setCANTimeout(250); + pivotMotor.enableVoltageCompensation(12); + pivotMotor.setSmartCurrentLimit(80); + pivotMotor.setIdleMode(IdleMode.kBrake); + + pivotMotor.burnFlash(); } @Override @@ -57,6 +47,6 @@ public void setAngle(double targetAngle) { @Override public void setVoltage(double volts) { appliedVolts = MathUtil.clamp(volts, -12, 12); - pivotRight.setVoltage(appliedVolts); + pivotMotor.setVoltage(appliedVolts); } }