Skip to content

Commit 29f9c9d

Browse files
IhedgehogsIhedgehogs
committed
added match number smartdashboard
1 parent 1825564 commit 29f9c9d

File tree

3 files changed

+67
-67
lines changed

3 files changed

+67
-67
lines changed

‎src/main/java/org/mayheminc/robot2020/autonomousroutines/TrenchAuto.java

Lines changed: 1 addition & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -41,10 +41,8 @@ public TrenchAuto() {
4141
addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 120, 0));
4242
addCommands(new DriveStraightOnHeading(-0.1, DistanceUnits.INCHES, 6, 0));
4343

44-
addCommands(new ShooterReadyAimFire(6.0));
45-
4644
// turn the wheel off now that the shooting is all done
47-
addCommands( new ShooterSetWheel(0.0));
45+
addCommands(new ShooterSetWheel(0.0));
4846

4947
addCommands(new ShooterSetHoodAbs(Shooter.HOOD_TARGET_ZONE_POSITION));
5048
}

‎src/main/java/org/mayheminc/robot2020/commands/ShooterReadyAimFire.java

Lines changed: 17 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -20,35 +20,32 @@ public class ShooterReadyAimFire extends SequentialCommandGroup {
2020
/**
2121
* Creates a new ShooterReadyAimFire.
2222
*/
23-
public ShooterReadyAimFire(double waitDuration) {
23+
public ShooterReadyAimFire() {
2424
// Add your commands in the super() call, e.g.
2525
// super(new FooCommand(), new BarCommand());
2626
super();
2727

2828
addCommands(new ShooterSetHoodAbs(Shooter.HOOD_INITIATION_LINE_POSITION));
2929
// addCommands(new ShooterSetWheel(3000.0, true));
30-
30+
3131
// addCommands(new Wait(3.0));
3232

33-
// TODO: Add compressor control so that compressor is turned off while actively shooting.
34-
33+
// TODO: Add compressor control so that compressor is turned off while actively
34+
// shooting.
35+
addCommands();
36+
3537
// aim to the target until we are on target.
3638
addCommands(
37-
new ParallelRaceGroup(
38-
new ParallelCommandGroup(new TargetingIsOnTarget(), new ShooterSetWheel(3000.0, true)),
39-
new TurretAimToTarget()));
40-
41-
// turn on the feeder, wiat 0.1, turn on the Chimney, wait 0.1, turn on the magazine, shoot for about 4 seconds
42-
addCommands(new ParallelRaceGroup(
43-
new ShooterSetFeeder(1.0),
44-
new SequentialCommandGroup(new Wait(0.1), new ChimneySetChimney(0.5)),
45-
new SequentialCommandGroup(new Wait(0.2), new MagazineSetTurntable(0.3)),
46-
new Wait(waitDuration)));
47-
48-
addCommands(new ParallelRaceGroup(
49-
new MagazineSetTurntable(0.0),
50-
new ChimneySetChimney(0.0),
51-
new ShooterSetFeeder(0.0),
52-
new Wait(0.1)));
39+
new ParallelRaceGroup(new ParallelCommandGroup(new TargetingIsOnTarget(), new ShooterSetWheel(3000.0, true)),
40+
new TurretAimToTarget()));
41+
42+
// turn on the feeder, wiat 0.1, turn on the Chimney, wait 0.1, turn on the
43+
// magazine, shoot for about 4 seconds
44+
addCommands(new ParallelRaceGroup(new ShooterSetFeeder(1.0),
45+
new SequentialCommandGroup(new Wait(0.1), new ChimneySetChimney(0.5)),
46+
new SequentialCommandGroup(new Wait(0.2), new MagazineSetTurntable(0.3)), new Wait(6.0)));
47+
48+
addCommands(new ParallelRaceGroup(new MagazineSetTurntable(0.0), new ChimneySetChimney(0.0),
49+
new ShooterSetFeeder(0.0), new Wait(0.1)));
5350
}
5451
}

‎src/main/java/org/mayheminc/robot2020/subsystems/Drive.java

Lines changed: 49 additions & 44 deletions
Original file line numberDiff line numberDiff line change
@@ -8,6 +8,7 @@
88
import edu.wpi.first.wpilibj.*;
99
import com.ctre.phoenix.motorcontrol.*;
1010
import com.ctre.phoenix.motorcontrol.ControlMode;
11+
import com.fasterxml.jackson.annotation.JacksonInject.Value;
1112

1213
//import edu.wpi.first.wpilibj.command.Subsystem;
1314
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -29,9 +30,9 @@ public class Drive extends SubsystemBase {
2930
public static final boolean COAST_MODE = false;
3031

3132
// PID loop variables
32-
private PIDController m_HeadingPid;
33-
private PIDHeadingError m_HeadingError;
34-
private PIDHeadingCorrection m_HeadingCorrection;
33+
private final PIDController m_HeadingPid;
34+
private final PIDHeadingError m_HeadingError;
35+
private final PIDHeadingCorrection m_HeadingCorrection;
3536
private boolean m_HeadingPidPreventWindup = false;
3637
private static final int LOOPS_GYRO_DELAY = 10;
3738

@@ -45,21 +46,22 @@ public class Drive extends SubsystemBase {
4546
private AHRS Navx;
4647

4748
// Driving mode
48-
private boolean m_speedRacerDriveMode = true; // set by default
49+
private final boolean m_speedRacerDriveMode = true; // set by default
4950

5051
// NavX parameters
5152
private double m_desiredHeading = 0.0;
5253
private boolean m_useHeadingCorrection = true;
53-
private static final double HEADING_PID_P = 0.007; // was 0.030 in 2019 for HIGH_GEAR
54+
private static final double HEADING_PID_P = 0.007; // was 0.030 in 2019 for HIGH_GEAR
5455
private static final double kToleranceDegreesPIDControl = 0.2;
5556

5657
// Drive parameters
5758
// pi * diameter * (pulley ratios) / (counts per rev * gearbox reduction)
58-
public static final double DISTANCE_PER_PULSE_IN_INCHES = 3.14 * 5.75 * 36.0 / 42.0 / (2048.0 * 7.56); // corrected for 2020
59+
public static final double DISTANCE_PER_PULSE_IN_INCHES = 3.14 * 5.75 * 36.0 / 42.0 / (2048.0 * 7.56); // corrected
60+
// for 2020
5961

6062
private boolean m_closedLoopMode = false;
61-
private double m_maxWheelSpeed = 1.0; // should be maximum wheel speed in native units
62-
private static final double CLOSED_LOOP_RAMP_RATE = 0.1; // time from neutral to full in seconds
63+
private final double m_maxWheelSpeed = 1.0; // should be maximum wheel speed in native units
64+
private static final double CLOSED_LOOP_RAMP_RATE = 0.1; // time from neutral to full in seconds
6365

6466
private double m_initialWheelDistance = 0.0;
6567
private int m_iterationsSinceRotationCommanded = 0;
@@ -84,7 +86,7 @@ public Drive() {
8486
*/
8587
Navx = new AHRS(SPI.Port.kMXP);
8688
Navx.reset();
87-
} catch (RuntimeException ex) {
89+
} catch (final RuntimeException ex) {
8890
DriverStation.reportError("Error instantiating navX MXP: " + ex.getMessage(), true);
8991
System.out.println("Error loading navx.");
9092
}
@@ -128,7 +130,7 @@ public void init() {
128130
configureDriveTalon(rightFrontTalon);
129131
}
130132

131-
public void zeroHeadingGyro(double headingOffset) {
133+
public void zeroHeadingGyro(final double headingOffset) {
132134
Navx.zeroYaw();
133135
setHeadingOffset(headingOffset);
134136

@@ -146,22 +148,22 @@ public void initDefaultCommand() {
146148
// setDefaultCommand(new SpeedRacerDrive());
147149
}
148150

149-
private void configureDriveTalon(MayhemTalonSRX talon) {
150-
double wheelP = 1.5;
151-
double wheelI = 0.0;
152-
double wheelD = 0.0;
153-
double wheelF = 1.0;
151+
private void configureDriveTalon(final MayhemTalonSRX talon) {
152+
final double wheelP = 1.5;
153+
final double wheelI = 0.0;
154+
final double wheelD = 0.0;
155+
final double wheelF = 1.0;
154156

155157
talon.setFeedbackDevice(FeedbackDevice.IntegratedSensor);
156-
158+
157159
talon.configNominalOutputVoltage(+0.0f, -0.0f);
158160
talon.configPeakOutputVoltage(+12.0, -12.0);
159161

160162
talon.config_kP(0, wheelP);
161163
talon.config_kI(0, wheelI);
162164
talon.config_kD(0, wheelD);
163165
talon.config_kF(0, wheelF);
164-
talon.configClosedloopRamp(CLOSED_LOOP_RAMP_RATE); // specify minimum time for neutral to full in seconds
166+
talon.configClosedloopRamp(CLOSED_LOOP_RAMP_RATE); // specify minimum time for neutral to full in seconds
165167

166168
DriverStation.reportError("setWheelPIDF: " + wheelP + " " + wheelI + " " + wheelD + " " + wheelF + "\n", false);
167169
}
@@ -172,9 +174,9 @@ private void configureDriveTalon(MayhemTalonSRX talon) {
172174
* @param brakeMode - true for "brake in neutral" and false for "coast in
173175
* neutral"
174176
*/
175-
public void setBrakeMode(boolean brakeMode) {
177+
public void setBrakeMode(final boolean brakeMode) {
176178

177-
NeutralMode mode = (brakeMode) ? NeutralMode.Brake : NeutralMode.Coast;
179+
final NeutralMode mode = (brakeMode) ? NeutralMode.Brake : NeutralMode.Coast;
178180

179181
leftFrontTalon.setNeutralMode(mode);
180182
leftRearTalon.setNeutralMode(mode);
@@ -221,8 +223,8 @@ public double getLeftSpeed() {
221223

222224
// *************************** GYRO *******************************************
223225

224-
public double calculateHeadingError(double Target) {
225-
double currentHeading = getHeading();
226+
public double calculateHeadingError(final double Target) {
227+
final double currentHeading = getHeading();
226228
double error = Target - currentHeading;
227229
error = error % 360.0;
228230
if (error > 180.0) {
@@ -235,7 +237,7 @@ public boolean getHeadingCorrectionMode() {
235237
return m_useHeadingCorrection;
236238
}
237239

238-
public void setHeadingCorrectionMode(boolean useHeadingCorrection) {
240+
public void setHeadingCorrectionMode(final boolean useHeadingCorrection) {
239241
m_useHeadingCorrection = useHeadingCorrection;
240242
}
241243

@@ -244,8 +246,8 @@ private void resetAndEnableHeadingPID() {
244246
m_HeadingPid.setP(HEADING_PID_P);
245247
// } else
246248
// {
247-
// low gear
248-
// m_HeadingPid.setP(HEADING_PID_P_FOR_LOW_GEAR);
249+
// low gear
250+
// m_HeadingPid.setP(HEADING_PID_P_FOR_LOW_GEAR);
249251
// }
250252
m_HeadingPid.reset();
251253
m_HeadingPid.enable();
@@ -256,11 +258,11 @@ private void resetAndEnableHeadingPID() {
256258
static private double m_prevRightDistance = 0.0;
257259

258260
public boolean isStationary() {
259-
double leftDistance = getLeftEncoder();
260-
double rightDistance = getRightEncoder();
261+
final double leftDistance = getLeftEncoder();
262+
final double rightDistance = getRightEncoder();
261263

262-
double leftDelta = Math.abs(leftDistance - m_prevLeftDistance);
263-
double rightDelta = Math.abs(rightDistance - m_prevRightDistance);
264+
final double leftDelta = Math.abs(leftDistance - m_prevLeftDistance);
265+
final double rightDelta = Math.abs(rightDistance - m_prevRightDistance);
264266

265267
m_prevLeftDistance = leftDistance;
266268
m_prevRightDistance = rightDistance;
@@ -279,7 +281,7 @@ public void displayGyroInfo() {
279281

280282
private double m_headingOffset = 0.0;
281283

282-
public void setHeadingOffset(double arg_offset) {
284+
public void setHeadingOffset(final double arg_offset) {
283285
m_headingOffset = arg_offset;
284286
}
285287

@@ -346,7 +348,7 @@ public void updateSdbPdp() {
346348
double rf;
347349
double lb;
348350
double rb;
349-
double fudgeFactor = 0.0;
351+
final double fudgeFactor = 0.0;
350352

351353
lf = pdp.getCurrent(Constants.PDP.DRIVE_LEFT_A) - fudgeFactor;
352354
rf = pdp.getCurrent(Constants.PDP.DRIVE_LEFT_B) - fudgeFactor;
@@ -357,6 +359,7 @@ public void updateSdbPdp() {
357359
SmartDashboard.putNumber("Right Front I", rf);
358360
SmartDashboard.putNumber("Left Back I", lb);
359361
SmartDashboard.putNumber("Right Back I", rb);
362+
360363
}
361364

362365
/*
@@ -394,12 +397,11 @@ public void setAutoAlignFalse() {
394397
// Robot.lights.set(LedPatternFactory.autoAlignGotIt);
395398
}
396399

397-
public void speedRacerDrive(double throttle, double rawSteeringX, boolean quickTurn) {
400+
public void speedRacerDrive(final double throttle, final double rawSteeringX, final boolean quickTurn) {
398401
speedRacerDriveNew(throttle, rawSteeringX, quickTurn);
399402
}
400403

401-
402-
public void speedRacerDriveNew(double throttle, double rawSteeringX, boolean quickTurn) {
404+
public void speedRacerDriveNew(final double throttle, final double rawSteeringX, final boolean quickTurn) {
403405
double leftPower, rightPower;
404406
double rotation = 0;
405407
final double QUICK_TURN_GAIN = 0.55; // 2019: .75. 2020: .75 was too fast.
@@ -411,7 +413,8 @@ public void speedRacerDriveNew(double throttle, double rawSteeringX, boolean qui
411413
throttleSign = -1;
412414
}
413415

414-
// check for if steering input is essentially zero for "DriveStraight" functionality
416+
// check for if steering input is essentially zero for "DriveStraight"
417+
// functionality
415418
if ((-0.01 < rawSteeringX) && (rawSteeringX < 0.01)) {
416419
// no turn being commanded, drive straight or stay still
417420
m_iterationsSinceRotationCommanded++;
@@ -476,11 +479,10 @@ public void speedRacerDriveNew(double throttle, double rawSteeringX, boolean qui
476479
setMotorPower(leftPower, rightPower);
477480
}
478481

479-
480-
public void speedRacerDriveOld(double throttle, double rawSteeringX, boolean quickTurn) {
482+
public void speedRacerDriveOld(final double throttle, final double rawSteeringX, final boolean quickTurn) {
481483
double leftPower, rightPower;
482484
double rotation = 0;
483-
double adjustedSteeringX = rawSteeringX * throttle;
485+
final double adjustedSteeringX = rawSteeringX * throttle;
484486
final double QUICK_TURN_GAIN = 0.55; // 2019: .75. 2020: .75 was too fast.
485487
final double STD_TURN_GAIN = 0.9; // 2019: 1.5. 2020: 1.5 was too fast// the driver wants the non-quick turn
486488
// turning a little more responsive.
@@ -611,7 +613,7 @@ private double maintainHeading() {
611613
return headingCorrection;
612614
}
613615

614-
public void rotate(double RotateX) {
616+
public void rotate(final double RotateX) {
615617
m_desiredHeading += RotateX;
616618
if (m_desiredHeading > 180) {
617619
m_desiredHeading -= 360;
@@ -623,7 +625,7 @@ public void rotate(double RotateX) {
623625
m_iterationsSinceMovementCommanded = 0;
624626
}
625627

626-
public void rotateToHeading(double desiredHeading) {
628+
public void rotateToHeading(final double desiredHeading) {
627629
m_desiredHeading = desiredHeading;
628630
}
629631

@@ -643,6 +645,9 @@ public void updateSmartDashboard() {
643645
// ***** KBS: Uncommenting below, as it takes a LONG time to get PDP values
644646
// updateSdbPdp();
645647

648+
int matchnumber = DriverStation.getInstance().getMatchNumber();
649+
DriverStation.MatchType MatchType = DriverStation.getInstance().getMatchType();
650+
SmartDashboard.putString("matchInfo", "" + MatchType + '_' + matchnumber);
646651
SmartDashboard.putNumber("Left Front Encoder Counts", leftFrontTalon.getSelectedSensorPosition(0));
647652
SmartDashboard.putNumber("Right Front Encoder Counts", rightFrontTalon.getSelectedSensorPosition(0));
648653
SmartDashboard.putNumber("Left Rear Encoder Counts", leftRearTalon.getSelectedSensorPosition(0));
@@ -687,13 +692,13 @@ public void updateSmartDashboard() {
687692
private static final double CAMERA_LAG = 0.150; // was .200; changing to .150 at CMP
688693

689694
public void updateHistory() {
690-
double now = Timer.getFPGATimestamp();
695+
final double now = Timer.getFPGATimestamp();
691696
headingHistory.add(now, getHeading());
692697
}
693698

694699
public double getHeadingForCapturedImage() {
695-
double now = Timer.getFPGATimestamp();
696-
double indexTime = now - CAMERA_LAG;
700+
final double now = Timer.getFPGATimestamp();
701+
final double indexTime = now - CAMERA_LAG;
697702
return headingHistory.getAzForTime(indexTime);
698703
}
699704

@@ -712,12 +717,12 @@ public void saveInitialWheelDistance() {
712717
* @return
713718
*/
714719
public double getWheelDistance() {
715-
double dist = (getLeftEncoder() + getRightEncoder()) / 2;
720+
final double dist = (getLeftEncoder() + getRightEncoder()) / 2;
716721
return dist - m_initialWheelDistance;
717722
}
718723

719724
// NOTE the difference between rotateToHeading(...) and goToHeading(...)
720-
public void setDesiredHeading(double desiredHeading) {
725+
public void setDesiredHeading(final double desiredHeading) {
721726
m_desiredHeading = desiredHeading;
722727
m_iterationsSinceRotationCommanded = LOOPS_GYRO_DELAY + 1;
723728
m_iterationsSinceMovementCommanded = 0;

0 commit comments

Comments
 (0)