Skip to content

Commit 421b3a3

Browse files
Niosocket11Niosocket11
committed
Fixy drivetrain and made elevator slightly
1 parent a1b114b commit 421b3a3

File tree

7 files changed

+674
-40
lines changed

7 files changed

+674
-40
lines changed

‎src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Arm/ArmConfig.java‎

Lines changed: 8 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -15,6 +15,7 @@
1515

1616
import edu.wpi.first.math.controller.ArmFeedforward;
1717
import edu.wpi.first.math.controller.PIDController;
18+
import edu.wpi.first.wpilibj.DriverStation;
1819

1920

2021
public final class ArmConfig {
@@ -261,7 +262,7 @@ private void checkMotorConfigs() {
261262
}
262263
}
263264
if (armMasterMotorType != MotorConfig.NEO && armMasterMotorType != MotorConfig.NEO_VORTEX && armMasterMotorType != MotorConfig.NEO_550) {
264-
throw new IllegalArgumentException("What is this config???");
265+
throw new IllegalArgumentException("What is this config??? If null pls change, if not null you're cooked");
265266
}
266267
}
267268

@@ -325,7 +326,7 @@ private void checkEncoders(int armMotorOfBackupRelativeEncoderId) {
325326
}
326327
else {
327328
armMainRelativeEncoderExists = false;
328-
System.out.println("Arm does not have backup encoder, highly recommended");
329+
DriverStation.reportWarning("Arm does not have backup encoder, highly recommended", true);
329330
}
330331

331332

@@ -342,7 +343,7 @@ private void checkPID() {
342343
if (armPIDContinuousInput) {
343344
armPID.enableContinuousInput(-180, 180);
344345
}
345-
armPIDExists = false;
346+
armPIDExists = true;
346347
return;
347348
}
348349
throw new IllegalArgumentException("Need to have 3 values for PID");
@@ -351,7 +352,7 @@ private void checkPID() {
351352
armKI = 0;
352353
armKD = 0;
353354
armPIDExists = false;
354-
System.out.println("ArmPID is off");
355+
DriverStation.reportWarning("ArmPID is off", true);
355356
}
356357

357358
private void checkFeedForward() {
@@ -370,8 +371,9 @@ private void checkFeedForward() {
370371
armKS = 0;
371372
armKG = 0;
372373
armKV = 0;
374+
armKA = 0;
373375
armFeedForwardExists = false;
374-
System.out.println("ArmFeedForward is off");
376+
DriverStation.reportWarning("ArmFeedForward is off", true);
375377
}
376378

377379
private void checkGearReduction() {
@@ -422,7 +424,7 @@ private void checkGearReduction() {
422424
// this.armStates = (Enum<?>) armStates;
423425
// }
424426
// else {
425-
// System.out.println("Daniel is sad that you don't like states");
427+
// DriverStation.reportWarning("Daniel is sad that you don't like states", true);
426428
// }
427429
// }
428430

‎src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Arm/SimpleArm.java‎

Lines changed: 38 additions & 22 deletions
Original file line numberDiff line numberDiff line change
@@ -20,29 +20,30 @@
2020
import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
2121

2222
import edu.wpi.first.math.MathUtil;
23+
import edu.wpi.first.wpilibj.DriverStation;
2324
import edu.wpi.first.wpilibj.Timer;
2425
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
2526
import edu.wpi.first.wpilibj2.command.SubsystemBase;
2627

2728
public class SimpleArm extends SubsystemBase {
2829
/** Creates a new SimpleArm. */
29-
ArmConfig armConfig;
30-
SparkBase armMasterMotor;
31-
RelativeEncoder armFeedbackEncoder;
32-
AbsoluteEncoder armResetEncoder;
33-
RelativeEncoder armBackupEncoder;
34-
SparkBase[] armFollowMotors;
35-
double currentDif = 0;
36-
double pastDif = 0;
37-
final double difDanger = 1;
30+
private ArmConfig armConfig;
31+
private SparkBase armMasterMotor;
32+
private RelativeEncoder armFeedbackEncoder;
33+
private AbsoluteEncoder armResetEncoder;
34+
private RelativeEncoder armBackupEncoder;
35+
private SparkBase[] armFollowMotors;
36+
private double currentDif = 0;
37+
private double pastDif = 0;
38+
private final double difDanger = 1;
3839

39-
double armGoal; // in degrees
40-
double latestManualInput; // in percentage of power (-1, 1)
40+
private double armGoal; // in degrees
41+
private double latestManualInput; // in percentage of power (-1, 1)
4142

42-
ArmControlState currentState = ArmControlState.AUTO;
43-
IdleMode armIdleMode = IdleMode.kBrake;
43+
private ArmControlState currentState = ArmControlState.AUTO;
44+
private IdleMode armIdleMode = IdleMode.kBrake;
4445

45-
Timer armTimer = new Timer();
46+
private Timer armTimer = new Timer();
4647

4748
private final double defaultRelativeEncoderResetValue = -90;
4849

@@ -51,7 +52,6 @@ public enum ArmControlState {
5152
MANUAL,
5253
BRAKE,
5354
COAST
54-
5555
}
5656

5757
public SimpleArm(ArmConfig armConfig) {
@@ -64,10 +64,18 @@ public void setGoal(double goal){
6464
if (goal <= armConfig.topLimit && goal >= armConfig.bottomLimit) {armGoal = goal;}
6565
}
6666

67+
public double getGoal() {
68+
return armGoal;
69+
}
70+
6771
public void setManualInput(double input) {
6872
latestManualInput = input;
6973
}
7074

75+
public double getLatestManualInput() {
76+
return latestManualInput;
77+
}
78+
7179
public void toggleControlMode() {
7280
if (currentState == ArmControlState.AUTO) {
7381
currentState = ArmControlState.MANUAL;
@@ -79,20 +87,20 @@ else if (currentState == ArmControlState.MANUAL) {
7987

8088
public void setArmControlState(ArmControlState controlState) {
8189
switch (controlState) {
82-
case AUTO -> setAdjustingOn();
90+
case AUTO -> setAutoOn();
8391
case MANUAL -> setManualOn();
8492
case BRAKE -> setBrakeOn();
8593
case COAST -> setCoastOn();
86-
default -> System.out.println("Such control mode has not been implemented yet");
94+
default -> DriverStation.reportWarning("Such control mode has not been implemented yet", true);
8795
}
8896
}
8997

90-
public void setAdjustingOn() {
98+
public void setAutoOn() {
9199
if (armConfig.armPIDExists || armConfig.armFeedForwardExists) {
92100
currentState = ArmControlState.AUTO;
93101
}
94102
else {
95-
System.out.println("Any sort of autonomous control mode is disabled due to no PID or FeedForward");
103+
DriverStation.reportWarning("Any sort of autonomous control mode is disabled due to no PID or FeedForward", true);
96104
}
97105
}
98106

@@ -130,7 +138,7 @@ private void checkFeedBackEncoder() {
130138
pastDif = currentDif;
131139
currentDif = armFeedbackEncoder.getPosition() - armBackupEncoder.getPosition();
132140
if (currentDif - pastDif > difDanger) {
133-
System.out.println("Arm encoder seems to be off!");
141+
DriverStation.reportWarning("Arm encoder seems to be off!", true);
134142
}
135143
}
136144

@@ -284,6 +292,10 @@ private void coastMotors() {
284292
}
285293
}
286294

295+
public void stopArm() {
296+
armMasterMotor.set(0);
297+
}
298+
287299
/**
288300
* Use this to do any fun stuff you want to do, for armSubsystemPeriodic
289301
*/
@@ -311,8 +323,12 @@ private void postSmartDashboardValues() {
311323
*/
312324
@Override
313325
public void periodic() {
314-
if (currentState == ArmControlState.AUTO) {autoArm();}
315-
else if (currentState == ArmControlState.MANUAL) {manualArm();}
326+
switch (currentState) {
327+
case AUTO -> autoArm();
328+
case MANUAL -> manualArm();
329+
case BRAKE -> stopArm();
330+
case COAST -> stopArm();
331+
}
316332
checkFeedBackEncoder();
317333
userPeriodic();
318334
postSmartDashboardValues();

‎src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Drivetrain/SimpleDrivetrain.java‎

Lines changed: 75 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -155,16 +155,82 @@ public enum Mode {
155155
double mu = 1;
156156
double autoCentripetalAccel = mu * g * 2;
157157

158+
159+
//Extra
160+
double wheelBase;
161+
double trackWidth;
162+
163+
int driveFrontLeftPort;
164+
int turnFrontLeftPort;
165+
int canCoderPortFL;
166+
167+
int driveFrontRightPort;
168+
int turnFrontRightPort;
169+
int canCoderPortFR;
170+
171+
int driveBackLeftPort;
172+
int turnBackLeftPort;
173+
int canCoderPortBL;
174+
175+
int driveBackRightPort;
176+
int turnBackRightPort;
177+
int canCoderPortBR;
178+
179+
double secsPer12Volts;
180+
double wheelDiameterMeters;
181+
double driveGearing;
182+
double turnGearing;
183+
184+
boolean isGyroReversed;
185+
double maxSpeed;
186+
187+
RobotConfig robotConfig;
188+
189+
double COLLISION_ACCELERATION_THRESHOLD;
190+
191+
192+
158193
/**
159194
*
160195
* @param wheelBase in meters
161196
* @param trackWidth in meters
162197
*/
163198
public SimpleDrivetrain(
164-
SwerveConfig swerveconfig
199+
SwerveConfig swerveConfig,
200+
double wheelBase, double trackWidth,
201+
int driveFrontLeftPort, int turnFrontLeftPort, int canCoderPortFL,
202+
int driveFrontRightPort, int turnFrontRightPort, int canCoderPortFR,
203+
int driveBackLeftPort, int turnBackLeftPort, int canCoderPortBL,
204+
int driveBackRightPort, int turnBackRightPort, int canCoderPortBR,
205+
double secsPer12Volts, double wheelDiameterMeters, double driveGearing, double turnGearing,
206+
boolean isGyroReversed, double maxSpeed,
207+
RobotConfig robotConfig,
208+
double COLLISION_ACCELERATION_THRESHOLD
165209
)
166210
{
167211
this.swerveConfig = swerveConfig;
212+
this.wheelBase = wheelBase;
213+
this.trackWidth = trackWidth;
214+
this.driveFrontLeftPort = driveFrontLeftPort;
215+
this.turnFrontLeftPort = turnFrontLeftPort;
216+
this.canCoderPortFL = canCoderPortFL;
217+
this.driveFrontRightPort = driveFrontRightPort;
218+
this.turnFrontRightPort = turnFrontRightPort;
219+
this.canCoderPortFR = canCoderPortFR;
220+
this.driveBackLeftPort = driveBackLeftPort;
221+
this.turnBackLeftPort = turnBackLeftPort;
222+
this.canCoderPortBL = canCoderPortBL;
223+
this.driveBackRightPort = driveBackRightPort;
224+
this.turnBackRightPort = turnBackRightPort;
225+
this.canCoderPortBR = canCoderPortBR;
226+
this.secsPer12Volts = secsPer12Volts;
227+
this.wheelDiameterMeters = wheelDiameterMeters;
228+
this.driveGearing = driveGearing;
229+
this.turnGearing = turnGearing;
230+
this.isGyroReversed = isGyroReversed;
231+
this.maxSpeed = maxSpeed;
232+
this.robotConfig = robotConfig;
233+
this.COLLISION_ACCELERATION_THRESHOLD = COLLISION_ACCELERATION_THRESHOLD;
168234

169235
AutoBuilder();
170236

@@ -214,22 +280,22 @@ public SimpleDrivetrain(
214280
moduleFL = new SwerveModule(swerveConfig, SwerveModule.ModuleType.FL,
215281
driveMotors[0] = MotorControllerFactory.createSparkFlex(driveFrontLeftPort),
216282
turnMotors[0] = MotorControllerFactory.createSparkMax(turnFrontLeftPort, MotorConfig.NEO),
217-
turnEncoders[0] = SensorFactory.createCANCoder(Constants.Drivetrainc.canCoderPortFL), 0, pitchSupplier, rollSupplier);
283+
turnEncoders[0] = SensorFactory.createCANCoder(canCoderPortFL), 0, pitchSupplier, rollSupplier);
218284
//SmartDashboard.putNumber("FL Motor Val", turnMotors[0].getEncoder().getPosition());
219-
moduleFR = new SwerveModule(Constants.Drivetrainc.swerveConfig, SwerveModule.ModuleType.FR,
285+
moduleFR = new SwerveModule(swerveConfig, SwerveModule.ModuleType.FR,
220286
driveMotors[1] = MotorControllerFactory.createSparkFlex(driveFrontRightPort),
221287
turnMotors[1] = MotorControllerFactory.createSparkMax(turnFrontRightPort, MotorConfig.NEO),
222-
turnEncoders[1] = SensorFactory.createCANCoder(Constants.Drivetrainc.canCoderPortFR), 1, pitchSupplier, rollSupplier);
288+
turnEncoders[1] = SensorFactory.createCANCoder(canCoderPortFR), 1, pitchSupplier, rollSupplier);
223289

224-
moduleBL = new SwerveModule(Constants.Drivetrainc.swerveConfig, SwerveModule.ModuleType.BL,
290+
moduleBL = new SwerveModule(swerveConfig, SwerveModule.ModuleType.BL,
225291
driveMotors[2] = MotorControllerFactory.createSparkFlex(driveBackLeftPort),
226292
turnMotors[2] = MotorControllerFactory.createSparkMax(turnBackLeftPort, MotorConfig.NEO),
227-
turnEncoders[2] = SensorFactory.createCANCoder(Constants.Drivetrainc.canCoderPortBL), 2, pitchSupplier, rollSupplier);
293+
turnEncoders[2] = SensorFactory.createCANCoder(canCoderPortBL), 2, pitchSupplier, rollSupplier);
228294

229-
moduleBR = new SwerveModule(Constants.Drivetrainc.swerveConfig, SwerveModule.ModuleType.BR,
295+
moduleBR = new SwerveModule(swerveConfig, SwerveModule.ModuleType.BR,
230296
driveMotors[3] = MotorControllerFactory.createSparkFlex(driveBackRightPort),
231297
turnMotors[3] = MotorControllerFactory.createSparkMax(turnBackRightPort, MotorConfig.NEO),
232-
turnEncoders[3] = SensorFactory.createCANCoder(Constants.Drivetrainc.canCoderPortBR), 3, pitchSupplier, rollSupplier);
298+
turnEncoders[3] = SensorFactory.createCANCoder(canCoderPortBR), 3, pitchSupplier, rollSupplier);
233299
modules = new SwerveModule[] { moduleFL, moduleFR, moduleBL, moduleBR };
234300
turnPidControllers[0] = turnMotors[0].getClosedLoopController();
235301
turnPidControllers[1] = turnMotors[1].getClosedLoopController();
@@ -588,7 +654,7 @@ public void drive(SwerveModuleState[] moduleStates) {
588654
* Configures PathPlanner AutoBuilder
589655
*/
590656
public void AutoBuilder() {
591-
RobotConfig config = Constants.Drivetrainc.Autoc.robotConfig;
657+
RobotConfig config = robotConfig;
592658
AutoBuilder.configure(
593659
//Supplier<Pose2d> poseSupplier,
594660
this::getPose, // Robot pose supplier

‎src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Drivetrain/SimpleRotateToFieldRelativeAngle.java‎

Whitespace-only changes.

0 commit comments

Comments
 (0)