Skip to content

Commit

Permalink
Add SparkFlex
Browse files Browse the repository at this point in the history
  • Loading branch information
BenG49 committed Jan 26, 2024
1 parent 926d4d6 commit 6b04bd9
Show file tree
Hide file tree
Showing 4 changed files with 44 additions and 11 deletions.
44 changes: 38 additions & 6 deletions src/main/java/com/stuypulse/robot/constants/Motors.java
Original file line number Diff line number Diff line change
Expand Up @@ -5,11 +5,9 @@

package com.stuypulse.robot.constants;

import com.ctre.phoenix.motorcontrol.NeutralMode;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX;
import com.revrobotics.CANSparkBase.IdleMode;
import com.revrobotics.CANSparkLowLevel.PeriodicFrame;
import com.revrobotics.CANSparkFlex;
import com.revrobotics.CANSparkMax;
import static com.revrobotics.CANSparkMax.IdleMode;

Expand Down Expand Up @@ -38,7 +36,8 @@ public interface Intake {
}

public interface Swerve {
public CANSparkMaxConfig DRIVE_CONFIG = new CANSparkMaxConfig(false, IdleMode.kBrake, 60);
public CANSparkFlexConfig DRIVE_CONFIG = new CANSparkFlexConfig(false, IdleMode.kBrake, 60);
public CANSparkMaxConfig JIM_DRIVE_CONFIG = new CANSparkMaxConfig(false, IdleMode.kBrake, 60);
public CANSparkMaxConfig TURN_CONFIG = new CANSparkMaxConfig(false, IdleMode.kBrake, 60);
}

Expand Down Expand Up @@ -75,7 +74,40 @@ public void configure(CANSparkMax motor) {
motor.setSmartCurrentLimit(CURRENT_LIMIT_AMPS);
motor.setOpenLoopRampRate(OPEN_LOOP_RAMP_RATE);
motor.burnFlash();
}

}
}

public static class CANSparkFlexConfig {
public final boolean INVERTED;
public final IdleMode IDLE_MODE;
public final int CURRENT_LIMIT_AMPS;
public final double OPEN_LOOP_RAMP_RATE;

public CANSparkFlexConfig(
boolean inverted,
IdleMode idleMode,
int currentLimitAmps,
double openLoopRampRate) {
this.INVERTED = inverted;
this.IDLE_MODE = idleMode;
this.CURRENT_LIMIT_AMPS = currentLimitAmps;
this.OPEN_LOOP_RAMP_RATE = openLoopRampRate;
}

public CANSparkFlexConfig(boolean inverted, IdleMode idleMode, int currentLimitAmps) {
this(inverted, idleMode, currentLimitAmps, 0.0);
}

public CANSparkFlexConfig(boolean inverted, IdleMode idleMode) {
this(inverted, idleMode, 80);
}

public void configure(CANSparkFlex motor) {
motor.setInverted(INVERTED);
motor.setIdleMode(IDLE_MODE);
motor.setSmartCurrentLimit(CURRENT_LIMIT_AMPS);
motor.setOpenLoopRampRate(OPEN_LOOP_RAMP_RATE);
motor.burnFlash();
}
}
}
Original file line number Diff line number Diff line change
Expand Up @@ -91,15 +91,15 @@ public void configureAutoBuilder() {
Swerve.Motion.XY,
Swerve.Motion.THETA,
Swerve.MAX_MODULE_SPEED.get(),
Swerve.WIDTH,
Math.hypot(Swerve.LENGTH, Swerve.WIDTH),
new ReplanningConfig(true, true)),
() -> {
var alliance = DriverStation.getAlliance();
if (alliance.isPresent()) {
return alliance.get() == DriverStation.Alliance.Red;
}
return false;
},
},
instance
);
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -175,7 +175,7 @@ public JimModule(String id, Translation2d location, int turnCANId, Rotation2d an

targetState = new SwerveModuleState();

Motors.Swerve.DRIVE_CONFIG.configure(driveMotor);
Motors.Swerve.JIM_DRIVE_CONFIG.configure(driveMotor);
Motors.Swerve.TURN_CONFIG.configure(turnMotor);
}

Expand Down
Original file line number Diff line number Diff line change
@@ -1,6 +1,7 @@
package com.stuypulse.robot.subsystems.swerve.module;

import com.ctre.phoenix6.hardware.CANcoder;
import com.revrobotics.CANSparkFlex;
import com.revrobotics.CANSparkMax;
import com.revrobotics.RelativeEncoder;
import com.revrobotics.CANSparkBase.IdleMode;
Expand Down Expand Up @@ -40,7 +41,7 @@ public class RetepModule extends AbstractModule {
private final CANcoder turnEncoder;

// drive
private final CANSparkMax driveMotor;
private final CANSparkFlex driveMotor;
private final RelativeEncoder driveEncoder;

// controllers
Expand All @@ -53,7 +54,7 @@ public RetepModule(String id, Translation2d translationOffset, Rotation2d angleO
this.angleOffset = angleOffset;

turnMotor = new CANSparkMax(turnID, MotorType.kBrushless);
driveMotor = new CANSparkMax(driveID, MotorType.kBrushless);
driveMotor = new CANSparkFlex(driveID, MotorType.kBrushless);

turnMotor.setIdleMode(IdleMode.kBrake);
driveMotor.setIdleMode(IdleMode.kBrake);
Expand Down

0 comments on commit 6b04bd9

Please sign in to comment.