Skip to content
This repository was archived by the owner on Sep 14, 2019. It is now read-only.

Commit cf78034

Browse files
DriverStationComputerDriverStationComputer
committed
Lots of changes, some important
including, but not limited to: - making gear changing actually work! - letting us toggle between velocity pid and direct control in teleop - new velocity pid stuff, like the R value
1 parent eebefb9 commit cf78034

8 files changed

Lines changed: 67 additions & 62 deletions

File tree

‎Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java‎

Lines changed: 12 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -151,7 +151,7 @@ public void robotInit() {
151151
*/
152152
@Override
153153
public void disabledInit() {
154-
154+
dt.disableVelocityPIDs();
155155
}
156156

157157
@Override
@@ -195,6 +195,7 @@ public void autonomousPeriodic() {
195195

196196
@Override
197197
public void teleopInit() {
198+
System.out.println("In teleopInit()");
198199
// This makes sure that the autonomous stops running when
199200
// teleop starts running. If you want the autonomous to
200201
// continue until interrupted by another command, remove
@@ -225,7 +226,7 @@ public void teleopPeriodic() {
225226
// SmartDashboard.putNumber("Avg Enc Dist", dt.getEncAvgDist());
226227
//
227228

228-
System.out.printf("Left: %1$f4.2; Right: %1$f4.2\n", RobotMap.dtLeft.get(), RobotMap.dtRight.get());
229+
System.out.printf("Left: %1$5.2f; Right: %2$5.2f\n", RobotMap.dtLeft.get(), RobotMap.dtRight.get());
229230

230231
SmartDashboard.putNumber("Angle", dt.getAHRSAngle());
231232
SmartDashboard.putNumber("Left Current draw", rmap.pdp.getCurrent(4));
@@ -247,15 +248,15 @@ public void testPeriodic() {
247248
// Robot.dt.enableVelocityPIDs();
248249
// firstTime = false;
249250
//// }
250-
251-
dt.getLeftVPID().setConsts(getConst("VelocityLeftkI", 0), 0,
252-
getConst("VelocityLeftkD", rmap.calcDefkD(dt.getCurrentMaxSpeed())),
253-
/* 1 / dt.getCurrentMaxSpeed() */Robot.getConst("VelocityLeftkF",
254-
1 / Robot.getConst("Max Low Speed", 84)));
255-
dt.getRightVPID().setConsts(getConst("VelocityRightkI", 0), 0,
256-
getConst("VelocityRightkD", rmap.calcDefkD(dt.getCurrentMaxSpeed())),
257-
/* 1 / dt.getCurrentMaxSpeed() */Robot.getConst("VelocityRightkF",
258-
1 / Robot.getConst("Max Low Speed", 84)));
251+
// dt.getLeftVPID().setConsts(getConst("VelocityLeftkI", 0), 0,
252+
// getConst("VelocityLeftkD", rmap.calcDefkD(dt.getCurrentMaxSpeed())),
253+
// /* 1 / dt.getCurrentMaxSpeed() */Robot.getConst("VelocityLeftkF",
254+
// 1 / Robot.getConst("Max Low Speed", 84)));
255+
// dt.getRightVPID().setConsts(getConst("VelocityRightkI", 0), 0,
256+
// getConst("VelocityRightkD", rmap.calcDefkD(dt.getCurrentMaxSpeed())),
257+
// /* 1 / dt.getCurrentMaxSpeed() */Robot.getConst("VelocityRightkF",
258+
// 1 / Robot.getConst("Max Low Speed", 84)));
259+
dt.resetAllVelocityPIDConsts();
259260

260261
dt.setVPIDs(getConst("VPID Test Set", 0.5));
261262

‎Robot2018/src/org/usfirst/frc/team199/Robot2018/RobotMap.java‎

Lines changed: 16 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -216,11 +216,7 @@ public double calcDefkD(double maxSpeed) {
216216
* be converted from rpm to radians per second, so divide by 60 and multiply to
217217
* get radians.
218218
*/
219-
double gearReduction = getGearRatio();
220-
double radius = getRadius();
221-
double timeConstant = getOmegaMax() / gearReduction / 60 * 2 * Math.PI * convertNtokG(getWeight()) / 2 * radius
222-
* radius / (getStallTorque() * gearReduction * 2);
223-
SmartDashboard.putNumber("Time Const (kD)", timeConstant);
219+
double timeConstant = getDrivetrainTimeConstant();
224220
double cycleTime = getCycleTime();
225221
/*
226222
* The denominator of kD is 1-(e ^ -cycleTime / timeConstant). The numerator is
@@ -230,8 +226,22 @@ public double calcDefkD(double maxSpeed) {
230226
return 1 / denominator / maxSpeed;
231227
}
232228

229+
/**
230+
* Gets the time constant of the drivetrain, which is used to calculate PID
231+
* constants.
232+
*
233+
* @return time constant
234+
*/
235+
public double getDrivetrainTimeConstant() {
236+
double gearReduction = getGearRatio();
237+
double radius = getRadius();
238+
double timeConstant = getOmegaMax() / gearReduction / 60 * 2 * Math.PI * convertNtokG(getWeight()) / 2 * radius
239+
* radius / (getStallTorque() * gearReduction * 2);
240+
return timeConstant;
241+
}
242+
233243
public double getGearRatio() {
234-
return Robot.getBool("High Gear", false) ? Robot.getConst("High Gear Gear Reduction", 5.392)
244+
return SmartDashboard.getBoolean("High Gear", false) ? Robot.getConst("High Gear Gear Reduction", 5.392)
235245
: Robot.getConst("Low Gear Gear Reduction", 12.255);
236246
}
237247

‎Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/VelocityPIDController.java‎

Lines changed: 4 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,7 @@
11
package org.usfirst.frc.team199.Robot2018.autonomous;
22

3+
import org.usfirst.frc.team199.Robot2018.Robot;
4+
35
import edu.wpi.first.wpilibj.PIDController;
46
import edu.wpi.first.wpilibj.PIDSource;
57
import edu.wpi.first.wpilibj.SpeedController;
@@ -29,7 +31,8 @@ public class VelocityPIDController extends PIDController implements SpeedControl
2931
*/
3032
public VelocityPIDController(double kp, double ki, double kd, double kf, PIDSource source,
3133
SpeedControllerGroup output) {
32-
super(kp, ki, kd, kf, source, output);
34+
super(kp, ki, kd, kf, /* LinearDigitalFilter.singlePoleIIR(source, 0.1, Robot.rmap.getCycleTime()) */source,
35+
output, Robot.rmap.getCycleTime());
3336
out = output;
3437
}
3538

‎Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/ShiftDriveType.java‎

Lines changed: 1 addition & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,5 @@
11
package org.usfirst.frc.team199.Robot2018.commands;
22

3-
import org.usfirst.frc.team199.Robot2018.Robot;
4-
53
import edu.wpi.first.wpilibj.command.InstantCommand;
64
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
75

@@ -19,7 +17,7 @@ public ShiftDriveType() {
1917
// Called once when the command executes
2018
@Override
2119
protected void initialize() {
22-
SmartDashboard.putBoolean("Bool/Arcade Drive", !Robot.getBool("Arcade Drive", true));
20+
SmartDashboard.putBoolean("Arcade Drive", !SmartDashboard.getBoolean("Arcade Drive", true));
2321
}
2422

2523
}

‎Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/TeleopDrive.java‎

Lines changed: 7 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -23,14 +23,18 @@ public TeleopDrive() {
2323
// Called just before this Command runs the first time
2424
@Override
2525
protected void initialize() {
26-
Robot.dt.enableVelocityPIDs();
26+
if (Robot.getBool("Teleop velocity PID", false)) {
27+
Robot.dt.enableVelocityPIDs();
28+
}
2729
}
2830

2931
// Called repeatedly when this Command is scheduled to run
3032
@Override
3133
protected void execute() {
32-
Robot.dt.resetVPIDInputRanges();
33-
Robot.dt.updatePidConstants();
34+
if (Robot.getBool("Teleop velocity PID", false)) {
35+
Robot.dt.resetVPIDInputRanges();
36+
Robot.dt.resetAllVelocityPIDConsts();
37+
}
3438
Robot.dt.teleopDrive();
3539
}
3640

‎Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/UpdatePIDConstants.java‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -18,6 +18,6 @@ public UpdatePIDConstants() {
1818
// Called just before this Command runs the first time
1919
@Override
2020
protected void initialize() {
21-
Robot.dt.updatePidConstants();
21+
Robot.dt.resetAllVelocityPIDConsts();
2222
}
2323
}

‎Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Drivetrain.java‎

Lines changed: 24 additions & 30 deletions
Original file line numberDiff line numberDiff line change
@@ -68,9 +68,12 @@ public Drivetrain() {
6868
rightVelocityController.setContinuous(false);
6969
rightVelocityController.setAbsoluteTolerance(Robot.getConst("VelocityToleranceRight", 2));
7070

71-
robotDrive = new DifferentialDrive(leftVelocityController, rightVelocityController);
71+
if (Robot.getBool("Teleop velocity PID", false)) {
72+
robotDrive = new DifferentialDrive(leftVelocityController, rightVelocityController);
73+
} else {
74+
robotDrive = new DifferentialDrive(dtLeft, dtRight);
75+
}
7276
robotDrive.setMaxOutput(Robot.getConst("Max High Speed", 204));
73-
// robotDrive = new DifferentialDrive(dtLeft, dtRight);
7477
}
7578

7679
@Override
@@ -184,7 +187,7 @@ public VelocityPIDController getRightVPID() {
184187
@Override
185188
public void teleopDrive() {
186189
boolean squareJoy = Robot.getBool("Square Joystick Values", true);
187-
if (Robot.getBool("Arcade Drive", true)) {
190+
if (SmartDashboard.getBoolean("Arcade Drive", true)) {
188191
double forw;
189192
double turn;
190193
if (Robot.getBool("Arcade Drive Default Setup", true)) {
@@ -262,18 +265,6 @@ public double getDtRightSpeed() {
262265
return dtRight.get();
263266
}
264267

265-
/**
266-
* Updates the PIDControllers' PIDConstants based on SmartDashboard values
267-
*/
268-
@Override
269-
public void updatePidConstants() {
270-
leftVelocityController.setPID(Robot.getConst("VelocityLeftkI", 0), 0,
271-
Robot.rmap.calcDefkD(getCurrentMaxSpeed()));
272-
rightVelocityController.setPID(Robot.getConst("VelocityRightkI", 0), 0,
273-
Robot.rmap.calcDefkD(getCurrentMaxSpeed()));
274-
resetVelocityPIDkFConsts();
275-
}
276-
277268
/**
278269
* Enable the VelocityPIDControllers used for velocity control on each side of
279270
* the DT
@@ -290,8 +281,10 @@ public void enableVelocityPIDs() {
290281
*/
291282
@Override
292283
public void disableVelocityPIDs() {
293-
leftVelocityController.disable();
294-
rightVelocityController.disable();
284+
// reset disables and also clears the error so that it isn't used when they
285+
// are reenabled.
286+
leftVelocityController.reset();
287+
rightVelocityController.reset();
295288
}
296289

297290
/**
@@ -405,7 +398,7 @@ public PIDSource getGyro() {
405398
@Override
406399
public void resetAllVelocityPIDConsts() {
407400
resetVelocityPIDkFConsts();
408-
resetVelocityPIDkDConsts();
401+
resetVelocityPIDkPConsts();
409402
resetVelocityPIDkIConsts();
410403
}
411404

@@ -419,14 +412,13 @@ public void resetAllVelocityPIDConsts() {
419412
* @return the new kF value as 1 / correct max speed
420413
*/
421414
@Override
422-
public double resetVelocityPIDkFConsts() {
415+
public void resetVelocityPIDkFConsts() {
423416
// double newKF = 1 / getCurrentMaxSpeed();
424417
// for this way of doing velocity PID, kF should always be 0
425418
double newKF = 0;
426419
leftVelocityController.setF(newKF);
427420
rightVelocityController.setF(newKF);
428421
SmartDashboard.putNumber("VPID kF", newKF);
429-
return newKF;
430422
}
431423

432424
/**
@@ -437,8 +429,10 @@ public double resetVelocityPIDkFConsts() {
437429
public void resetVelocityPIDkIConsts() {
438430
// 0.011 was calculated manually. 84 is the low gear max speed, to which we
439431
// scale the constants.
440-
double newLeftkI = Robot.getConst("VelocityLeftkI", 0.011 * 84) / getCurrentMaxSpeed();
441-
double newRightkI = Robot.getConst("VelocityRightkI", 0.011 * 84) / getCurrentMaxSpeed();
432+
double defaultkI = Robot.getConst("VelocityPidR", 3) / Robot.rmap.getDrivetrainTimeConstant()
433+
* Robot.rmap.getCycleTime();
434+
double newLeftkI = Robot.getConst("VelocityLeftkI", defaultkI) / getCurrentMaxSpeed();
435+
double newRightkI = Robot.getConst("VelocityRightkI", defaultkI) / getCurrentMaxSpeed();
442436
// I is P because wpilib is dumb
443437
leftVelocityController.setP(newLeftkI);
444438
rightVelocityController.setP(newRightkI);
@@ -451,15 +445,15 @@ public void resetVelocityPIDkIConsts() {
451445
* gearing (high or low gear).
452446
*/
453447
@Override
454-
public void resetVelocityPIDkDConsts() {
448+
public void resetVelocityPIDkPConsts() {
455449
// 0.012 was calculated manually. 84 is the low gear max speed, to which we
456450
// scale the constants.
457-
double newLeftkD = Robot.getConst("VelocityLeftkD", 0.012 * 84) / getCurrentMaxSpeed();
458-
double newRightkD = Robot.getConst("VelocityRightkD", 0.012 * 84) / getCurrentMaxSpeed();
459-
leftVelocityController.setD(newLeftkD);
460-
rightVelocityController.setD(newRightkD);
461-
SmartDashboard.putNumber("VPID Left kD", newLeftkD);
462-
SmartDashboard.putNumber("VPID Right kD", newRightkD);
451+
double newkP = Robot.getConst("VelocityPidR", 3) / getCurrentMaxSpeed();
452+
// P is D because wpilib is dumb
453+
leftVelocityController.setD(newkP);
454+
rightVelocityController.setD(newkP);
455+
SmartDashboard.putNumber("VPID Left kP", newkP);
456+
SmartDashboard.putNumber("VPID Right kP", newkP);
463457
}
464458

465459
public double resetVPIDInputRanges() {
@@ -476,7 +470,7 @@ public double resetVPIDInputRanges() {
476470
*/
477471
@Override
478472
public double getCurrentMaxSpeed() {
479-
if (Robot.getBool("High Gear", true)) {
473+
if (SmartDashboard.getBoolean("High Gear", true)) {
480474
return Robot.getConst("Max High Speed", 204);
481475
} else {
482476
return Robot.getConst("Max Low Speed", 84);

‎Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/DrivetrainInterface.java‎

Lines changed: 2 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -61,11 +61,6 @@ public interface DrivetrainInterface {
6161
*/
6262
public double getDtRightSpeed();
6363

64-
/**
65-
* Updates the PIDControllers' PIDConstants based on SmartDashboard values
66-
*/
67-
public void updatePidConstants();
68-
6964
/**
7065
* Enable the VelocityPIDControllers used for velocity control on each side of
7166
* the DT
@@ -159,11 +154,11 @@ public interface DrivetrainInterface {
159154
* be 1 / max speed
160155
* @return the new kF value as 1 / correct max speed
161156
*/
162-
public double resetVelocityPIDkFConsts();
157+
public void resetVelocityPIDkFConsts();
163158

164159
public void resetVelocityPIDkIConsts();
165160

166-
public void resetVelocityPIDkDConsts();
161+
public void resetVelocityPIDkPConsts();
167162

168163
public void resetAllVelocityPIDConsts();
169164

0 commit comments

Comments
 (0)