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

Commit be7362f

Browse files
DriverStationComputerDriverStationComputer
committed
Put PID controllers and commands to SmartDashboard and some other stuff
by gabe
1 parent b899642 commit be7362f

6 files changed

Lines changed: 90 additions & 53 deletions

File tree

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

Lines changed: 35 additions & 29 deletions
Original file line numberDiff line numberDiff line change
@@ -41,13 +41,13 @@ public class Robot extends TimedRobot {
4141
public static Listener listen;
4242

4343
public static OI oi;
44-
44+
4545
public static Map<String, ArrayList<String[]>> autoScripts;
4646

4747
Command autonomousCommand;
4848
SendableChooser<Position> posChooser = new SendableChooser<Position>();
4949
Map<String, SendableChooser<Strategy>> stratChoosers = new HashMap<String, SendableChooser<Strategy>>();
50-
String[] fmsPossibilities = {"LL", "LR", "RL", "RR"};
50+
String[] fmsPossibilities = { "LL", "LR", "RL", "RR" };
5151

5252
public static double getConst(String key, double def) {
5353
if (!SmartDashboard.containsKey("Const/" + key)) {
@@ -76,13 +76,13 @@ public void robotInit() {
7676
lift = new Lift();
7777
dt = new Drivetrain();
7878
oi = new OI();
79-
79+
8080
// auto position chooser
8181
for (Position p : Position.values()) {
8282
posChooser.addObject(p.getSDName(), p);
8383
}
8484
SmartDashboard.putData("Starting Position", posChooser);
85-
85+
8686
// auto strategy choosers
8787
for (String input : fmsPossibilities) {
8888
SendableChooser<Strategy> chooser = new SendableChooser<Strategy>();
@@ -92,10 +92,10 @@ public void robotInit() {
9292
SmartDashboard.putData(input, chooser);
9393
stratChoosers.put(input, chooser);
9494
}
95-
95+
9696
// auto delay chooser
9797
SmartDashboard.putNumber("Auto Delay", 0);
98-
98+
9999
// parse scripts from Preferences, which maintains values throughout reboots
100100
autoScripts = AutoUtils.parseScriptFile(Preferences.getInstance().getString("autoscripts", ""));
101101

@@ -118,23 +118,23 @@ public void disabledPeriodic() {
118118
}
119119

120120
/**
121-
* This function is called once during the start of autonomous in order to
122-
* grab values from SmartDashboard and the FMS and call the Autonomous
123-
* command with those values.
121+
* This function is called once during the start of autonomous in order to grab
122+
* values from SmartDashboard and the FMS and call the Autonomous command with
123+
* those values.
124124
*/
125125
@Override
126126
public void autonomousInit() {
127127
String fmsInput = DriverStation.getInstance().getGameSpecificMessage();
128128
Position startPos = posChooser.getSelected();
129129
double autoDelay = SmartDashboard.getNumber("Auto Delay", 0);
130-
130+
131131
Map<String, Strategy> strategies = new HashMap<String, Strategy>();
132132
for (Map.Entry<String, SendableChooser<Strategy>> entry : stratChoosers.entrySet()) {
133-
String key = entry.getKey();
134-
SendableChooser<Strategy> chooser = entry.getValue();
135-
strategies.put(key, chooser.getSelected());
133+
String key = entry.getKey();
134+
SendableChooser<Strategy> chooser = entry.getValue();
135+
strategies.put(key, chooser.getSelected());
136136
}
137-
137+
138138
Autonomous auto = new Autonomous(startPos, strategies, autoDelay, fmsInput, false);
139139
auto.start();
140140
}
@@ -166,25 +166,31 @@ public void teleopPeriodic() {
166166
}
167167

168168
boolean firstTime = true;
169+
169170
/**
170171
* This function is called periodically during test mode
171172
*/
172173
@Override
173174
public void testPeriodic() {
174-
// if(firstTime) {
175-
// Robot.dt.enableVelocityPIDs();
176-
// firstTime = false;
177-
//// }
178-
// Robot.dt.setVPIDs(Robot.getConst("VPID Test Set", 0.5));
179-
// SmartDashboard.putNumber("Drivetrain/Left VPID Targ", Robot.dt.getLeftVPIDSetpoint());
180-
// SmartDashboard.putNumber("Drivetrain/Right VPID Targ", Robot.dt.getRightVPIDSetpoint());
181-
// SmartDashboard.putNumber("Left VPID Error", Robot.dt.getLeftVPIDerror());
182-
// SmartDashboard.putNumber("Right VPID Error", Robot.dt.getRightVPIDerror());
183-
// SmartDashboard.putNumber("Left Enc Rate", Robot.dt.getLeftEncRate());
184-
// SmartDashboard.putNumber("Right Enc Rate", Robot.dt.getRightEncRate());
185-
186-
dt.dtLeft.set(-oi.leftJoy.getY());
187-
dt.dtRight.set(-oi.rightJoy.getY());
188-
175+
// if(firstTime) {
176+
// Robot.dt.enableVelocityPIDs();
177+
// firstTime = false;
178+
//// }
179+
// Robot.dt.setVPIDs(Robot.getConst("VPID Test Set", 0.5));
180+
// SmartDashboard.putNumber("Drivetrain/Left VPID Targ",
181+
// Robot.dt.getLeftVPIDSetpoint());
182+
// SmartDashboard.putNumber("Drivetrain/Right VPID Targ",
183+
// Robot.dt.getRightVPIDSetpoint());
184+
// SmartDashboard.putNumber("Left VPID Error", Robot.dt.getLeftVPIDerror());
185+
// SmartDashboard.putNumber("Right VPID Error", Robot.dt.getRightVPIDerror());
186+
// SmartDashboard.putNumber("Left Enc Rate", Robot.dt.getLeftEncRate());
187+
// SmartDashboard.putNumber("Right Enc Rate", Robot.dt.getRightEncRate());
188+
189+
dt.dtLeft.set(0.1);
190+
// dt.dtRight.set(-oi.rightJoy.getY());
191+
// dt.dtLeft.set(-oi.leftJoy.getY());
192+
// dt.dtRight.set(-oi.rightJoy.getY());
193+
194+
dt.putVelocityControllersToDashboard();
189195
}
190196
}

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

Lines changed: 27 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -60,6 +60,8 @@ public class RobotMap {
6060
public static AHRS fancyGyro;
6161
public static DoubleSolenoid dtGear;
6262

63+
private final double DIST_PER_PULSE_RATIO = (5.0 * Math.PI) * (17 / 25) / (3 * 256);
64+
6365
/**
6466
* This function takes in a TalonSRX motorController and sets nominal and peak
6567
* outputs to the default
@@ -100,6 +102,18 @@ private void configSPX(WPI_VictorSPX mc) {
100102
mc.configPeakOutputReverse(-1, kTimeout);
101103
}
102104

105+
/**
106+
* Uses SmartDashboard and math to calculate a *great* default kD
107+
*/
108+
public double calcDefkD() {
109+
double timeConstant = Robot.getConst("Omega Max", 5330) * Robot.getConst("Mass of Robot", 54.4311)
110+
* Robot.getConst("Radius of Drivetrain Wheel", 0.0635)
111+
* Robot.getConst("Radius of Drivetrain Wheel", 0.0635) / Robot.getConst("Stall Torque", 2.41);
112+
double cycleTime = Robot.getConst("Code cycle time", 0.1);
113+
double denominator = 1 - Math.pow(Math.E, -1 * cycleTime / timeConstant);
114+
return 1 / denominator;
115+
}
116+
103117
public RobotMap() {
104118

105119
// intakeMotor = new WPI_TalonSRX(getPort("IntakeTalonSRX", 4));
@@ -109,21 +123,21 @@ public RobotMap() {
109123
// climberMotor = new WPI_TalonSRX(getPort("ClimberTalonSRX", 6));
110124
// configSRX(climberMotor);
111125

112-
leftEncPort1 = new DigitalInput(getPort("1LeftEnc", 1));
113-
leftEncPort2 = new DigitalInput(getPort("2LeftEnc", 0));
126+
leftEncPort1 = new DigitalInput(getPort("1LeftEnc", 2));
127+
leftEncPort2 = new DigitalInput(getPort("2LeftEnc", 3));
114128
leftEncDist = new Encoder(leftEncPort1, leftEncPort2);
115129
leftEncDist.setPIDSourceType(PIDSourceType.kDisplacement);
116130
leftEncRate = new Encoder(leftEncPort1, leftEncPort2);
117131
leftEncRate.setPIDSourceType(PIDSourceType.kRate);
118-
leftEncDist.setDistancePerPulse(Robot.getConst("Distance Per Pulse Left", 0.184));
119-
leftEncRate.setDistancePerPulse(Robot.getConst("Distance Per Pulse Left", 0.184));
132+
leftEncDist.setDistancePerPulse(Robot.getConst("DPP", DIST_PER_PULSE_RATIO));
133+
leftEncRate.setDistancePerPulse(Robot.getConst("DPP", DIST_PER_PULSE_RATIO));
120134

121135
dtLeftMaster = new WPI_TalonSRX(getPort("LeftTalonSRXMaster", 1));
122136
configSRX(dtLeftMaster);
123137
dtLeftSlave = new WPI_VictorSPX(getPort("LeftVictorSPXSlave", 2));
124138
configSPX(dtLeftSlave);
125139
dtLeft = new SpeedControllerGroup(dtLeftMaster, dtLeftSlave);
126-
//inverted bc gear boxes invert from input to output
140+
// inverted bc gear boxes invert from input to output
127141
dtLeft.setInverted(true);
128142

129143
leftVelocityController = new VelocityPIDController(Robot.getConst("VelocityLeftkP", 0),
@@ -136,21 +150,21 @@ public RobotMap() {
136150
leftVelocityController.setAbsoluteTolerance(Robot.getConst("VelocityToleranceLeft", 2));
137151
SmartDashboard.putData(leftVelocityController);
138152

139-
rightEncPort1 = new DigitalInput(getPort("1RightEnc", 3));
140-
rightEncPort2 = new DigitalInput(getPort("2RightEnc", 2));
153+
rightEncPort1 = new DigitalInput(getPort("1RightEnc", 1));
154+
rightEncPort2 = new DigitalInput(getPort("2RightEnc", 0));
141155
rightEncDist = new Encoder(rightEncPort1, rightEncPort2);
142156
rightEncDist.setPIDSourceType(PIDSourceType.kDisplacement);
143157
rightEncRate = new Encoder(rightEncPort1, rightEncPort2);
144158
rightEncRate.setPIDSourceType(PIDSourceType.kRate);
145-
rightEncDist.setDistancePerPulse(Robot.getConst("Distance Per Pulse Right", 0.184));
146-
rightEncRate.setDistancePerPulse(Robot.getConst("Distance Per Pulse Right", 0.184));
159+
rightEncDist.setDistancePerPulse(Robot.getConst("DPP", DIST_PER_PULSE_RATIO));
160+
rightEncRate.setDistancePerPulse(Robot.getConst("DPP", DIST_PER_PULSE_RATIO));
147161

148162
dtRightMaster = new WPI_TalonSRX(getPort("RightTalonSRXMaster", 4));
149163
configSRX(dtRightMaster);
150164
dtRightSlave = new WPI_VictorSPX(getPort("RightVictorSPXSlave", 3));
151165
configSPX(dtRightSlave);
152166
dtRight = new SpeedControllerGroup(dtRightMaster, dtRightSlave);
153-
//inverted bc gear boxes invert from input to output
167+
// inverted bc gear boxes invert from input to output
154168
dtRight.setInverted(true);
155169

156170
rightVelocityController = new VelocityPIDController(Robot.getConst("VelocityRightkP", 0),
@@ -162,10 +176,9 @@ public RobotMap() {
162176
rightVelocityController.setContinuous(false);
163177
rightVelocityController.setAbsoluteTolerance(Robot.getConst("VelocityToleranceRight", 2));
164178

165-
// robotDrive = new DifferentialDrive(leftVelocityController,
166-
// rightVelocityController);
167-
// robotDrive.setMaxOutput(Robot.getConst("Max High Speed", 204));
168-
robotDrive = new DifferentialDrive(dtLeft, dtRight);
179+
robotDrive = new DifferentialDrive(leftVelocityController, rightVelocityController);
180+
robotDrive.setMaxOutput(Robot.getConst("Max High Speed", 204));
181+
// robotDrive = new DifferentialDrive(dtLeft, dtRight);
169182

170183
distEncAvg = new PIDSourceAverage(leftEncDist, rightEncDist);
171184
fancyGyro = new AHRS(SerialPort.Port.kMXP);

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

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -7,6 +7,7 @@
77
import edu.wpi.first.wpilibj.PIDController;
88
import edu.wpi.first.wpilibj.PIDOutput;
99
import edu.wpi.first.wpilibj.command.Command;
10+
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
1011

1112
/**
1213
*
@@ -40,6 +41,7 @@ public void initialize() {
4041
moveController.setAbsoluteTolerance(Robot.getConst("MoveTolerance", 2));
4142
moveController.setSetpoint(target);
4243
moveController.enable();
44+
SmartDashboard.putData(moveController);
4345
}
4446

4547
// Called repeatedly when this Command is scheduled to run

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

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -23,7 +23,7 @@ public TeleopDrive() {
2323
// Called just before this Command runs the first time
2424
@Override
2525
protected void initialize() {
26-
// Robot.dt.enableVelocityPIDs();
26+
Robot.dt.enableVelocityPIDs();
2727
}
2828

2929
// Called repeatedly when this Command is scheduled to run

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

Lines changed: 20 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -44,43 +44,43 @@ public class Drivetrain extends Subsystem implements DrivetrainInterface {
4444
public void initDefaultCommand() {
4545
setDefaultCommand(new TeleopDrive());
4646
}
47-
47+
4848
public void setLeft(double spd) {
4949
dtLeft.set(spd);
5050
}
51-
51+
5252
public void setRight(double spd) {
5353
dtRight.set(spd);
5454
}
55-
55+
5656
/**
5757
* Use for testing only (i.e. when not going through robotDrive)
58-
* */
58+
*/
5959
public void setVPIDs(double realSpd) {
6060
leftVelocityController.set(realSpd);
6161
rightVelocityController.set(-realSpd);
6262
}
63-
63+
6464
public double getLeftVPIDerror() {
6565
return leftVelocityController.getError();
6666
}
6767

6868
public double getRightVPIDerror() {
6969
return rightVelocityController.getError();
7070
}
71-
71+
7272
public double getLeftVPIDSetpoint() {
7373
return leftVelocityController.get();
7474
}
7575

7676
public double getRightVPIDSetpoint() {
7777
return rightVelocityController.get();
7878
}
79-
79+
8080
public double getLeftEncRate() {
8181
return leftEncRate.getRate();
8282
}
83-
83+
8484
public double getRightEncRate() {
8585
return rightEncRate.getRate();
8686
}
@@ -226,6 +226,7 @@ public void resetDistEncs() {
226226
@Override
227227
public void setDistancePerPulseLeft(double ratio) {
228228
leftEncDist.setDistancePerPulse(ratio);
229+
leftEncRate.setDistancePerPulse(ratio);
229230
}
230231

231232
/**
@@ -238,6 +239,7 @@ public void setDistancePerPulseLeft(double ratio) {
238239
@Override
239240
public void setDistancePerPulseRight(double ratio) {
240241
rightEncDist.setDistancePerPulse(ratio);
242+
rightEncRate.setDistancePerPulse(ratio);
241243
}
242244

243245
/**
@@ -301,7 +303,7 @@ public double resetVelocityPIDkFConsts() {
301303
SmartDashboard.putNumber("VPID kF", newKF);
302304
return newKF;
303305
}
304-
306+
305307
public double resetVPIDInputRanges() {
306308
double currentMaxSpd = getCurrentMaxSpeed();
307309
leftVelocityController.setInputRange(-currentMaxSpd, currentMaxSpd);
@@ -322,4 +324,13 @@ public double getCurrentMaxSpeed() {
322324
return Robot.getConst("Max Low Speed", 84);
323325
}
324326
}
327+
328+
/**
329+
* Put left and right velocity controllers (PID) on SmartDashboard.
330+
*/
331+
@Override
332+
public void putVelocityControllersToDashboard() {
333+
SmartDashboard.putData(leftVelocityController);
334+
SmartDashboard.putData(rightVelocityController);
335+
}
325336
}

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

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -142,4 +142,9 @@ public interface DrivetrainInterface {
142142
* @return the current max speed of the DT in inches/second
143143
*/
144144
public double getCurrentMaxSpeed();
145+
146+
/**
147+
* Put left and right velocity controllers (PID) on SmartDashboard.
148+
*/
149+
public void putVelocityControllersToDashboard();
145150
}

0 commit comments

Comments
 (0)