Skip to content

Commit ebd1bdc

Browse files
Tortuga-AMTortuga-AM
committed
slightly less cooked drive maxxing
1 parent 055a285 commit ebd1bdc

File tree

7 files changed

+118
-130
lines changed

7 files changed

+118
-130
lines changed

‎src/main/java/frc/robot/Manager/Manager.java‎

Lines changed: 4 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -9,7 +9,7 @@
99
import frc.robot.Subsystems.HoodedShooterSupersystem.HoodedShooterSupersystem;
1010
import frc.robot.Subsystems.Indexer.Indexer;
1111
import frc.robot.Subsystems.Intake.Intake;
12-
//import frc.robot.Subsystems.Vision.Vision;
12+
import frc.robot.Subsystems.Vision.Vision;
1313
import frc.robot.TeamLib.subsystem.*;
1414
public class Manager extends Subsystem<ManagerStates> {
1515

@@ -18,7 +18,7 @@ public class Manager extends Subsystem<ManagerStates> {
1818
public Drive drive;
1919
private Indexer indexer;
2020
private HoodedShooterSupersystem hoodedShooterSupersystem;
21-
//private Vision vision;
21+
private Vision vision;
2222

2323
public static Manager getInstance() {
2424
if (instance == null) {
@@ -33,7 +33,7 @@ public Manager() {
3333
hoodedShooterSupersystem = HoodedShooterSupersystem.getInstance();
3434
drive = Drive.getInstance();
3535
indexer = Indexer.getInstance();
36-
//vision = Vision.getInstance();
36+
vision = Vision.getInstance();
3737

3838
//add triggers
3939
addTrigger(IDLE, OUTTAKING, DRIVER_CONTROLLER::getXButtonPressed);
@@ -88,11 +88,7 @@ public void runState() {
8888
indexer.periodic();
8989
intake.periodic();
9090
drive.periodic();
91-
//vision.periodic();
92-
}
93-
94-
public boolean hasGamepiece() {
95-
return intake.hasGamepiece();
91+
vision.periodic();
9692
}
9793

9894
public void logData() {

‎src/main/java/frc/robot/Subsystems/Drive/Drive.java‎

Lines changed: 60 additions & 51 deletions
Original file line numberDiff line numberDiff line change
@@ -1,10 +1,3 @@
1-
// Copyright (c) 2021-2025 Littleton Robotics
2-
// http://github.com/Mechanical-Advantage
3-
//
4-
// Use of this source code is governed by a BSD
5-
// license that can be found in the LICENSE file
6-
// at the root directory of this project.
7-
81
package frc.robot.Subsystems.Drive;
92

103
import static frc.robot.Subsystems.Drive.DriveConstants.*;
@@ -34,50 +27,53 @@ public class Drive extends Subsystem<DriveStates> {
3427
private final GyroIO gyroIO;
3528
private final GyroIOInputs gyroInputs = new GyroIOInputs();
3629
private final DifferentialDriveKinematics kinematics =
37-
new DifferentialDriveKinematics(trackWidth);
38-
private final double kS = GlobalConstants.ROBOT_MODE == RobotMode.SIM ? simKs : realKs;
39-
private final double kV = GlobalConstants.ROBOT_MODE == RobotMode.SIM ? simKv : realKv;
30+
new DifferentialDriveKinematics(TRACK_WIDTH);
31+
private final double kS = GlobalConstants.ROBOT_MODE == RobotMode.SIM ? SIM_KS : REAL_KS;
32+
private final double kV = GlobalConstants.ROBOT_MODE == RobotMode.SIM ? SIM_KV : REAL_KV;
4033
private final DifferentialDrivePoseEstimator poseEstimator =
4134
new DifferentialDrivePoseEstimator(kinematics, Rotation2d.kZero, 0.0, 0.0, Pose2d.kZero);
4235
private Rotation2d rawGyroRotation = Rotation2d.kZero;
4336
private double lastLeftPositionMeters = 0.0;
4437
private double lastRightPositionMeters = 0.0;
4538
private XboxController controller = new XboxController(0);
4639

40+
private boolean slowMode = false; // Slow mode variable
41+
4742
private Drive(DriveIO io, GyroIO gyroIO) {
48-
super("Drive", DriveStates.TANK_DRIVE);
43+
super("Drive", DriveStates.TANK_DRIVE);
4944
this.io = io;
5045
this.gyroIO = gyroIO;
5146
// Configure SysId
5247
}
48+
5349
public XboxController getController() {
54-
return controller;
50+
return controller;
5551
}
5652

5753
public static Drive getInstance() {
58-
if (instance == null) {
59-
switch (GlobalConstants.ROBOT_MODE) {
60-
case REAL:
61-
instance = new Drive(new DriveIOReal(), new GyroIOReal());
62-
break;
63-
case SIM:
64-
instance = new Drive(new DriveIOSim(), new GyroIO() {});
65-
break;
66-
case TESTING:
67-
instance = new Drive(new DriveIOReal(), new GyroIOReal());
68-
break;
69-
default:
70-
throw new IllegalStateException("Unexpected value: " + GlobalConstants.ROBOT_MODE);
71-
}
72-
}
73-
return instance;
54+
if (instance == null) {
55+
switch (GlobalConstants.ROBOT_MODE) {
56+
case REAL:
57+
instance = new Drive(new DriveIOReal(), new GyroIOReal());
58+
break;
59+
case SIM:
60+
instance = new Drive(new DriveIOSim(), new GyroIO() {});
61+
break;
62+
case TESTING:
63+
instance = new Drive(new DriveIOReal(), new GyroIOReal());
64+
break;
65+
default:
66+
throw new IllegalStateException("Unexpected value: " + GlobalConstants.ROBOT_MODE);
67+
}
68+
}
69+
return instance;
7470
}
7571

7672
@Override
7773
public void runState() {
7874
io.updateInputs(inputs);
7975
gyroIO.updateInputs(gyroInputs);
80-
getState().driveRobot();
76+
getState().driveRobot();
8177

8278
// Update gyro angle
8379
if (gyroInputs.connected) {
@@ -106,8 +102,8 @@ public void runClosedLoop(ChassisSpeeds speeds) {
106102

107103
/** Runs the drive at the desired left and right velocities. */
108104
public void runClosedLoop(double leftMetersPerSec, double rightMetersPerSec) {
109-
double leftRadPerSec = leftMetersPerSec / wheelRadiusMeters;
110-
double rightRadPerSec = rightMetersPerSec / wheelRadiusMeters;
105+
double leftRadPerSec = leftMetersPerSec / WHEEL_RADIUS_METERS;
106+
double rightRadPerSec = rightMetersPerSec / WHEEL_RADIUS_METERS;
111107
SmartDashboard.putNumber("Drive/LeftSetpointRadPerSec", leftRadPerSec);
112108
SmartDashboard.putNumber("Drive/RightSetpointRadPerSec", rightRadPerSec);
113109

@@ -125,28 +121,27 @@ public void runOpenLoop(double leftVolts, double rightVolts) {
125121
public void stop() {
126122
runOpenLoop(0.0, 0.0);
127123
}
128-
129-
public void tankDrive(double leftSpeed, double rightSpeed) {
130-
double left = MathUtil.applyDeadband(leftSpeed, 0.02);
131-
double right = MathUtil.applyDeadband(rightSpeed, 0.02);
132124

125+
public void tankDrive(double leftSpeed, double rightSpeed) {
126+
double left = MathUtil.applyDeadband(leftSpeed, 0.02);
127+
double right = MathUtil.applyDeadband(rightSpeed, 0.02);
133128

134-
runClosedLoop(left * maxSpeedMetersPerSec, right * maxSpeedMetersPerSec);
129+
runClosedLoop(left * MAX_SPEED_METERS_PER_SEC, right * MAX_SPEED_METERS_PER_SEC);
135130
}
136131

137132
public void arcadeDrive(double forward, double rotation) {
138-
double x = MathUtil.applyDeadband(forward, 0.02);
139-
double z = MathUtil.applyDeadband(rotation, 0.02);
133+
double x = MathUtil.applyDeadband(forward, 0.02);
134+
double z = MathUtil.applyDeadband(rotation, 0.02);
140135

141-
// Calculate speeds
142-
var speeds = DifferentialDrive.arcadeDriveIK(x, z, true);
136+
// Calculate speeds
137+
var speeds = DifferentialDrive.arcadeDriveIK(x, z, true);
143138

144-
// Apply output
145-
runClosedLoop(speeds.left * maxSpeedMetersPerSec, speeds.right * maxSpeedMetersPerSec);
139+
// Apply output
140+
runClosedLoop(speeds.left * MAX_SPEED_METERS_PER_SEC, speeds.right * MAX_SPEED_METERS_PER_SEC);
146141
}
147142

148143
/** Returns the current odometry pose. */
149-
144+
150145
public Pose2d getPose() {
151146
return poseEstimator.getEstimatedPosition();
152147
}
@@ -173,31 +168,45 @@ public void addVisionMeasurement(Pose2d visionPose, double timestamp, Matrix<N3,
173168
}
174169

175170
/** Returns the position of the left wheels in meters. */
176-
171+
177172
public double getLeftPositionMeters() {
178-
return inputs.leftPositionRad * wheelRadiusMeters;
173+
return inputs.leftPositionRad * WHEEL_RADIUS_METERS;
179174
}
180175

181176
/** Returns the position of the right wheels in meters. */
182-
177+
183178
public double getRightPositionMeters() {
184-
return inputs.rightPositionRad * wheelRadiusMeters;
179+
return inputs.rightPositionRad * WHEEL_RADIUS_METERS;
185180
}
186181

187182
/** Returns the velocity of the left wheels in meters/second. */
188-
183+
189184
public double getLeftVelocityMetersPerSec() {
190-
return inputs.leftVelocityRadPerSec * wheelRadiusMeters;
185+
return inputs.leftVelocityRadPerSec * WHEEL_RADIUS_METERS;
186+
}
187+
188+
public double getAngularVelocityRadPerSec() {
189+
return gyroInputs.yawVelocityRadPerSec;
191190
}
192191

193192
/** Returns the velocity of the right wheels in meters/second. */
194-
193+
195194
public double getRightVelocityMetersPerSec() {
196-
return inputs.rightVelocityRadPerSec * wheelRadiusMeters;
195+
return inputs.rightVelocityRadPerSec * WHEEL_RADIUS_METERS;
197196
}
198197

199198
/** Returns the average velocity in radians/second. */
200199
public double getCharacterizationVelocity() {
201200
return (inputs.leftVelocityRadPerSec + inputs.rightVelocityRadPerSec) / 2.0;
202201
}
202+
203+
/** Returns true if slow mode is enabled. */
204+
public boolean isSlowMode() {
205+
return slowMode;
206+
}
207+
208+
// Optionally, you may want to add a setter for slow mode:
209+
public void setSlowMode(boolean enabled) {
210+
slowMode = enabled;
211+
}
203212
}

‎src/main/java/frc/robot/Subsystems/Drive/DriveConstants.java‎

Lines changed: 21 additions & 46 deletions
Original file line numberDiff line numberDiff line change
@@ -1,61 +1,36 @@
1-
// Copyright (c) 2021-2025 Littleton Robotics
2-
// http://github.com/Mechanical-Advantage
3-
//
4-
// Use of this source code is governed by a BSD
5-
// license that can be found in the LICENSE file
6-
// at the root directory of this project.
7-
81
package frc.robot.Subsystems.Drive;
92

10-
import com.pathplanner.lib.config.ModuleConfig;
11-
import com.pathplanner.lib.config.RobotConfig;
123
import edu.wpi.first.math.system.plant.DCMotor;
134
import edu.wpi.first.math.util.Units;
145

156
public class DriveConstants {
16-
public static final double maxSpeedMetersPerSec = 4.0;
17-
public static final double trackWidth = Units.inchesToMeters(26.0);
7+
public static final double MAX_SPEED_METERS_PER_SEC = 4.0;
8+
public static final double TRACK_WIDTH = Units.inchesToMeters(26.0);
189

1910
// Device CAN IDs
20-
public static final int pigeonCanId = 9;
21-
public static final int leftLeaderCanId = 1;
22-
public static final int leftFollowerCanId = 2;
23-
public static final int rightLeaderCanId = 3;
24-
public static final int rightFollowerCanId = 4;
11+
public static final int PIGEON_CAN_ID = 9;
12+
public static final int LEFT_LEADER_CAN_ID = 1;
13+
public static final int LEFT_FOLLOWER_CAN_ID = 2;
14+
public static final int RIGHT_LEADER_CAN_ID = 3;
15+
public static final int RIGHT_FOLLOWER_CAN_ID = 4;
2516

2617
// Motor configuration
27-
public static final int currentLimit = 60;
28-
public static final double wheelRadiusMeters = Units.inchesToMeters(3.0);
29-
public static final double motorReduction = 10.71;
30-
public static final boolean leftInverted = false;
31-
public static final boolean rightInverted = true;
32-
public static final DCMotor gearbox = DCMotor.getCIM(2);
18+
public static final int CURRENT_LIMIT = 60;
19+
public static final double WHEEL_RADIUS_METERS = Units.inchesToMeters(3.0);
20+
public static final double MOTOR_REDUCTION = 10.71;
21+
public static final boolean LEFT_INVERTED = false;
22+
public static final boolean RIGHT_INVERTED = true;
23+
public static final DCMotor GEARBOX = DCMotor.getCIM(2);
3324

3425
// Velocity PID configuration
35-
public static final double realKp = 0.0;
36-
public static final double realKd = 0.0;
37-
public static final double realKs = 0.0;
38-
public static final double realKv = 0.1;
26+
public static final double REAL_KP = 0.0;
27+
public static final double REAL_KD = 0.0;
28+
public static final double REAL_KS = 0.0;
29+
public static final double REAL_KV = 0.1;
3930

40-
public static final double simKp = 0.05;
41-
public static final double simKd = 0.0;
42-
public static final double simKs = 0.0;
43-
public static final double simKv = 0.227;
31+
public static final double SIM_KP = 0.05;
32+
public static final double SIM_KD = 0.0;
33+
public static final double SIM_KS = 0.0;
34+
public static final double SIM_KV = 0.227;
4435

45-
// PathPlanner configuration
46-
public static final double robotMassKg = 74.088;
47-
public static final double robotMOI = 6.883;
48-
public static final double wheelCOF = 1.2;
49-
public static final RobotConfig ppConfig =
50-
new RobotConfig(
51-
robotMassKg,
52-
robotMOI,
53-
new ModuleConfig(
54-
wheelRadiusMeters,
55-
maxSpeedMetersPerSec,
56-
wheelCOF,
57-
gearbox.withReduction(motorReduction),
58-
currentLimit,
59-
2),
60-
trackWidth);
6136
}

‎src/main/java/frc/robot/Subsystems/Drive/DriveIOReal.java‎

Lines changed: 8 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -14,15 +14,15 @@
1414
public class DriveIOReal implements DriveIO {
1515
private static final double ticksPerRevolution = 1440;
1616

17-
private final TalonSRX leftLeader = new TalonSRX(leftLeaderCanId);
18-
private final TalonSRX leftFollower = new TalonSRX(leftFollowerCanId);
19-
private final TalonSRX rightLeader = new TalonSRX(rightLeaderCanId);
20-
private final TalonSRX rightFollower = new TalonSRX(rightFollowerCanId);
17+
private final TalonSRX leftLeader = new TalonSRX(LEFT_LEADER_CAN_ID);
18+
private final TalonSRX leftFollower = new TalonSRX(LEFT_FOLLOWER_CAN_ID);
19+
private final TalonSRX rightLeader = new TalonSRX(RIGHT_LEADER_CAN_ID);
20+
private final TalonSRX rightFollower = new TalonSRX(RIGHT_FOLLOWER_CAN_ID);
2121

2222
public DriveIOReal() {
2323
var config = new TalonSRXConfiguration();
24-
config.peakCurrentLimit = currentLimit;
25-
config.continuousCurrentLimit = currentLimit - 15;
24+
config.peakCurrentLimit = CURRENT_LIMIT;
25+
config.continuousCurrentLimit = CURRENT_LIMIT - 15;
2626
config.peakCurrentDuration = 250;
2727
config.voltageCompSaturation = 12.0;
2828
config.primaryPID.selectedFeedbackSensor = FeedbackDevice.QuadEncoder;
@@ -32,8 +32,8 @@ public DriveIOReal() {
3232
rightLeader.configAllSettings(config);
3333
rightFollower.configAllSettings(config);
3434

35-
leftLeader.setInverted(leftInverted);
36-
rightLeader.setInverted(rightInverted);
35+
leftLeader.setInverted(LEFT_INVERTED);
36+
rightLeader.setInverted(RIGHT_INVERTED);
3737

3838
leftFollower.follow(leftLeader);
3939
rightFollower.follow(rightLeader);

‎src/main/java/frc/robot/Subsystems/Drive/DriveIOSim.java‎

Lines changed: 8 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -17,19 +17,19 @@ public class DriveIOSim implements DriveIO {
1717
private double leftAppliedVolts = 0.0;
1818
private double rightAppliedVolts = 0.0;
1919
private boolean closedLoop = false;
20-
private PIDController leftPID = new PIDController(simKp, 0.0, simKd);
21-
private PIDController rightPID = new PIDController(simKp, 0.0, simKd);
20+
private PIDController leftPID = new PIDController(SIM_KP, 0.0, SIM_KD);
21+
private PIDController rightPID = new PIDController(SIM_KP, 0.0, SIM_KD);
2222
private double leftFFVolts = 0.0;
2323
private double rightFFVolts = 0.0;
2424

2525
@Override
2626
public void updateInputs(DriveIOInputs inputs) {
2727
if (closedLoop) {
2828
leftAppliedVolts =
29-
leftFFVolts + leftPID.calculate(sim.getLeftVelocityMetersPerSecond() / wheelRadiusMeters);
29+
leftFFVolts + leftPID.calculate(sim.getLeftVelocityMetersPerSecond() / WHEEL_RADIUS_METERS);
3030
rightAppliedVolts =
3131
rightFFVolts
32-
+ rightPID.calculate(sim.getRightVelocityMetersPerSecond() / wheelRadiusMeters);
32+
+ rightPID.calculate(sim.getRightVelocityMetersPerSecond() / WHEEL_RADIUS_METERS);
3333
}
3434

3535
// Update simulation state
@@ -38,13 +38,13 @@ public void updateInputs(DriveIOInputs inputs) {
3838
MathUtil.clamp(rightAppliedVolts, -12.0, 12.0));
3939
sim.update(0.02);
4040

41-
inputs.leftPositionRad = sim.getLeftPositionMeters() / wheelRadiusMeters;
42-
inputs.leftVelocityRadPerSec = sim.getLeftVelocityMetersPerSecond() / wheelRadiusMeters;
41+
inputs.leftPositionRad = sim.getLeftPositionMeters() / WHEEL_RADIUS_METERS;
42+
inputs.leftVelocityRadPerSec = sim.getLeftVelocityMetersPerSecond() / WHEEL_RADIUS_METERS;
4343
inputs.leftAppliedVolts = leftAppliedVolts;
4444
inputs.leftCurrentAmps = new double[] {sim.getLeftCurrentDrawAmps()};
4545

46-
inputs.rightPositionRad = sim.getRightPositionMeters() / wheelRadiusMeters;
47-
inputs.rightVelocityRadPerSec = sim.getRightVelocityMetersPerSecond() / wheelRadiusMeters;
46+
inputs.rightPositionRad = sim.getRightPositionMeters() / WHEEL_RADIUS_METERS;
47+
inputs.rightVelocityRadPerSec = sim.getRightVelocityMetersPerSecond() / WHEEL_RADIUS_METERS;
4848
inputs.rightAppliedVolts = rightAppliedVolts;
4949
inputs.rightCurrentAmps = new double[] {sim.getRightCurrentDrawAmps()};
5050
}

0 commit comments

Comments
 (0)