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

Commit c2257f6

Browse files
DriverStationComputerDriverStationComputer
committed
Velocity PID and deadband (committed by Alex B)
1 parent af72c09 commit c2257f6

5 files changed

Lines changed: 234 additions & 23 deletions

File tree

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

Lines changed: 26 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -232,6 +232,7 @@ public void teleopPeriodic() {
232232
boolean firstTime = true;
233233

234234
public void testInit() {
235+
dt.enableVelocityPIDs();
235236
}
236237

237238
/**
@@ -243,26 +244,39 @@ public void testPeriodic() {
243244
// Robot.dt.enableVelocityPIDs();
244245
// firstTime = false;
245246
//// }
246-
// Robot.dt.setVPIDs(Robot.getConst("VPID Test Set", 0.5));
247247

248-
Scheduler.getInstance().run();
248+
dt.getLeftVPID().setConsts(getConst("VelocityLeftkI", 0), 0,
249+
getConst("VelocityLeftkD", rmap.calcDefkD(dt.getCurrentMaxSpeed())),
250+
/* 1 / dt.getCurrentMaxSpeed() */Robot.getConst("VelocityLeftkF",
251+
1 / Robot.getConst("Max Low Speed", 84)));
252+
dt.getRightVPID().setConsts(getConst("VelocityRightkI", 0), 0,
253+
getConst("VelocityRightkD", rmap.calcDefkD(dt.getCurrentMaxSpeed())),
254+
/* 1 / dt.getCurrentMaxSpeed() */Robot.getConst("VelocityRightkF",
255+
1 / Robot.getConst("Max Low Speed", 84)));
256+
257+
dt.setVPIDs(getConst("VPID Test Set", 0.5));
258+
259+
// Scheduler.getInstance().run();
249260

250-
SmartDashboard.putNumber("Drivetrain/Left VPID Targ", Robot.dt.getLeftVPIDSetpoint());
251-
SmartDashboard.putNumber("Drivetrain/Right VPID Targ", Robot.dt.getRightVPIDSetpoint());
252-
SmartDashboard.putNumber("Left VPID Error", Robot.dt.getLeftVPIDerror());
253-
SmartDashboard.putNumber("Right VPID Error", Robot.dt.getRightVPIDerror());
254-
SmartDashboard.putNumber("Left Enc Rate", Robot.dt.getLeftEncRate());
255-
SmartDashboard.putNumber("Right Enc Rate", Robot.dt.getRightEncRate());
261+
System.out.println("Left VPID Targ: " + dt.getLeftVPIDOutput());
262+
System.out.println("Right VPID Targ: " + dt.getRightVPIDOutput());
263+
System.out.println("Left VPID Error: " + dt.getLeftVPIDerror());
264+
System.out.println("Right VPID Error: " + dt.getRightVPIDerror());
265+
System.out.println("Left Enc Rate: " + dt.getLeftEncRate());
266+
System.out.println("Right Enc Rate: " + dt.getRightEncRate());
256267

257-
SmartDashboard.putNumber("Left Enc Dist", dt.getLeftDist());
258-
SmartDashboard.putNumber("Right Enc Dist", dt.getRightDist());
259-
SmartDashboard.putNumber("Avg Enc Dist", dt.getEncAvgDist());
268+
System.out.println("Left Talon Speed: " + rmap.dtLeftMaster.get());
269+
System.out.println("Right Talon Speed: " + rmap.dtRightMaster.get());
270+
271+
// System.out.println("Left Enc Dist " + dt.getLeftDist());
272+
// System.out.println("Right Enc Dist " + dt.getRightDist());
273+
// System.out.println("Avg Enc Dist" + dt.getEncAvgDist());
260274

261275
// dt.dtLeft.set(0.1);
262276
// dt.dtRight.set(-oi.rightJoy.getY());
263277
// dt.dtLeft.set(-oi.leftJoy.getY());
264278
// dt.dtRight.set(-oi.rightJoy.getY());
265279

266-
dt.putVelocityControllersToDashboard();
280+
// dt.putVelocityControllersToDashboard();
267281
}
268282
}

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

Lines changed: 19 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -98,6 +98,8 @@ private void configSRX(WPI_TalonSRX mc) {
9898
// so if we went above 40, the motors would stop completely
9999
mc.configContinuousCurrentLimit(40, 0);
100100
mc.enableCurrentLimit(true);
101+
102+
mc.configNeutralDeadband(Robot.getConst("Motor Deadband", 0.001), kTimeout);
101103
}
102104

103105
/**
@@ -114,6 +116,8 @@ private void configSPX(WPI_VictorSPX mc) {
114116
mc.configNominalOutputReverse(0, kTimeout);
115117
mc.configPeakOutputForward(1, kTimeout);
116118
mc.configPeakOutputReverse(-1, kTimeout);
119+
120+
mc.configNeutralDeadband(Robot.getConst("Motor Deadband", 0.001), kTimeout);
117121
}
118122

119123
public RobotMap() {
@@ -159,8 +163,12 @@ public RobotMap() {
159163
dtLeft.setInverted(true);
160164

161165
leftVelocityController = new VelocityPIDController(Robot.getConst("VelocityLeftkI", 0), 0,
162-
Robot.getConst("VelocityLeftkD", calcDefkD(Robot.getConst("Max Low Speed", 84))),
163-
1 / Robot.getConst("Max Low Speed", 84), leftEncRate, dtLeft);
166+
/*
167+
* Robot.getConst("VelocityLeftkD", calcDefkD(Robot.getConst("Max Low Speed",
168+
* 84)))
169+
*/ 0, /* 1 / Robot.getConst("Max Low Speed", 84) */Robot.getConst("VelocityLeftkF",
170+
1 / Robot.getConst("Max Low Speed", 84)),
171+
leftEncRate, dtLeft);
164172
leftVelocityController.setInputRange(-Robot.getConst("Max High Speed", 204),
165173
Robot.getConst("Max High Speed", 204));
166174
leftVelocityController.setOutputRange(-1.0, 1.0);
@@ -187,8 +195,12 @@ public RobotMap() {
187195
dtRight.setInverted(true);
188196

189197
rightVelocityController = new VelocityPIDController(Robot.getConst("VelocityRightkI", 0), 0,
190-
Robot.getConst("VelocityRightkD", calcDefkD(Robot.getConst("Max Low Speed", 84))),
191-
1 / Robot.getConst("Max Low Speed", 84), rightEncRate, dtRight);
198+
/*
199+
* Robot.getConst("VelocityRightkD", calcDefkD(Robot.getConst("Max Low Speed",
200+
* 84)))
201+
*/0, /* 1 / Robot.getConst("Max Low Speed", 84) */Robot.getConst("VelocityRightkF",
202+
1 / Robot.getConst("Max Low Speed", 84)),
203+
rightEncRate, dtRight);
192204
rightVelocityController.setInputRange(-Robot.getConst("Max High Speed", 204),
193205
Robot.getConst("Max High Speed", 204));
194206
rightVelocityController.setOutputRange(-1.0, 1.0);
@@ -202,6 +214,8 @@ public RobotMap() {
202214
distEncAvg = new PIDSourceAverage(leftEncDist, rightEncDist);
203215
fancyGyro = new AHRS(SPI.Port.kMXP);
204216
dtGear = new DoubleSolenoid(getPort("1dtGearSolenoid", 0), getPort("2dtGearSolenoid", 1));
217+
218+
calcDefkD(Robot.getConst("Max Low Speed", 84));
205219
}
206220

207221
/**
@@ -242,6 +256,7 @@ public double calcDefkD(double maxSpeed) {
242256
double radius = getRadius();
243257
double timeConstant = getOmegaMax() / gearReduction / 60 * 2 * Math.PI * convertNtokG(getWeight()) / 2 * radius
244258
* radius / (getStallTorque() * gearReduction * 2);
259+
SmartDashboard.putNumber("Time Const (kD)", timeConstant);
245260
double cycleTime = getCycleTime();
246261
/*
247262
* The denominator of kD is 1-(e ^ -cycleTime / timeConstant). The numerator is

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

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -55,6 +55,13 @@ public void set(double speed) {
5555
setSetpoint(speed);
5656
}
5757

58+
public void setConsts(double kP, double kI, double kD, double kF) {
59+
super.setP(kP);
60+
super.setI(kI);
61+
super.setD(kD);
62+
super.setF(kF);
63+
}
64+
5865
/**
5966
* Gets the current set voltage (setpoint) sent to the output
6067
* SpeedControllerGroup

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

Lines changed: 10 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -93,11 +93,11 @@ public double getRightVPIDerror() {
9393
return rightVelocityController.getError();
9494
}
9595

96-
public double getLeftVPIDSetpoint() {
96+
public double getLeftVPIDOutput() {
9797
return leftVelocityController.get();
9898
}
9999

100-
public double getRightVPIDSetpoint() {
100+
public double getRightVPIDOutput() {
101101
return rightVelocityController.get();
102102
}
103103

@@ -145,6 +145,14 @@ public void dtRightPIDDrive(double value) {
145145
dtRightSlave.set(ControlMode.Velocity, setValue);
146146
}
147147

148+
public VelocityPIDController getLeftVPID() {
149+
return leftVelocityController;
150+
}
151+
152+
public VelocityPIDController getRightVPID() {
153+
return rightVelocityController;
154+
}
155+
148156
/**
149157
* Drives based on joystick input and SmartDashboard values
150158
*/

‎shuffleboard.json‎

Lines changed: 172 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,5 @@
11
{
2-
"dividerPosition": 0.4530791788856305,
2+
"dividerPosition": 0.28519061583577715,
33
"tabPane": [
44
{
55
"title": "PID Testing",
@@ -87,7 +87,7 @@
8787
"content": {
8888
"_type": "Graph",
8989
"_source0": "network_table:///SmartDashboard/Drivetrain/Left VPID Targ",
90-
"_title": "SmartDashboard/Drivetrain/Left VPID Targ",
90+
"_title": "/SmartDashboard/Drivetrain/Left VPID Targ",
9191
"Visible time": 30.0,
9292
"SmartDashboard/Drivetrain/Left VPID Targ visible": true
9393
}
@@ -100,7 +100,7 @@
100100
"content": {
101101
"_type": "Graph",
102102
"_source0": "network_table:///SmartDashboard/Drivetrain/Right VPID Targ",
103-
"_title": "SmartDashboard/Drivetrain/Right VPID Targ",
103+
"_title": "/SmartDashboard/Drivetrain/Right VPID Targ",
104104
"Visible time": 30.0,
105105
"SmartDashboard/Drivetrain/Right VPID Targ visible": true
106106
}
@@ -240,6 +240,28 @@
240240
"colorWhenTrue": "#7CFC00FF",
241241
"colorWhenFalse": "#8B0000FF"
242242
}
243+
},
244+
"2,1": {
245+
"size": [
246+
2,
247+
1
248+
],
249+
"content": {
250+
"_type": "Toggle Switch",
251+
"_source0": "network_table:///Preferences/Bool/Square Drive Values",
252+
"_title": "Preferences/Bool/Square Drive Values"
253+
}
254+
},
255+
"2,2": {
256+
"size": [
257+
2,
258+
1
259+
],
260+
"content": {
261+
"_type": "Toggle Switch",
262+
"_source0": "network_table:///Preferences/Bool/Square Joystick Values",
263+
"_title": "Preferences/Bool/Square Joystick Values"
264+
}
243265
}
244266
}
245267
}
@@ -363,7 +385,7 @@
363385
"content": {
364386
"_type": "Graph",
365387
"_source0": "network_table:///SmartDashboard/Drivetrain/Left VPID Targ",
366-
"_title": "SmartDashboard/Drivetrain/Left VPID Targ",
388+
"_title": "/SmartDashboard/Drivetrain/Left VPID Targ",
367389
"Visible time": 10.0,
368390
"SmartDashboard/Drivetrain/Left VPID Targ visible": true
369391
}
@@ -376,7 +398,7 @@
376398
"content": {
377399
"_type": "Graph",
378400
"_source0": "network_table:///SmartDashboard/Drivetrain/Right VPID Targ",
379-
"_title": "SmartDashboard/Drivetrain/Right VPID Targ",
401+
"_title": "/SmartDashboard/Drivetrain/Right VPID Targ",
380402
"Visible time": 10.0,
381403
"SmartDashboard/Drivetrain/Right VPID Targ visible": true
382404
}
@@ -760,6 +782,151 @@
760782
"_source0": "network_table:///Preferences/Const/MoveTolerance",
761783
"_title": "Preferences/Const/MoveTolerance"
762784
}
785+
},
786+
"0,3": {
787+
"size": [
788+
1,
789+
1
790+
],
791+
"content": {
792+
"_type": "Text View",
793+
"_source0": "network_table:///Preferences/Const/Stall Torque",
794+
"_title": "Preferences/Const/Stall Torque"
795+
}
796+
},
797+
"1,3": {
798+
"size": [
799+
2,
800+
1
801+
],
802+
"content": {
803+
"_type": "Text View",
804+
"_source0": "network_table:///Preferences/Const/PID Move Fudge Factor",
805+
"_title": "Preferences/Const/PID Move Fudge Factor"
806+
}
807+
}
808+
}
809+
}
810+
},
811+
{
812+
"title": "VPID Testing 2",
813+
"autoPopulate": false,
814+
"autoPopulatePrefix": "",
815+
"widgetPane": {
816+
"gridSize": 128.0,
817+
"showGrid": true,
818+
"hgap": 16.0,
819+
"vgap": 16.0,
820+
"tiles": {
821+
"0,0": {
822+
"size": [
823+
1,
824+
1
825+
],
826+
"content": {
827+
"_type": "Text View",
828+
"_source0": "network_table:///Preferences/Const/VelocityLeftkD",
829+
"_title": "Preferences/Const/VelocityLeftkD"
830+
}
831+
},
832+
"0,1": {
833+
"size": [
834+
1,
835+
1
836+
],
837+
"content": {
838+
"_type": "Text View",
839+
"_source0": "network_table:///Preferences/Const/VelocityLeftkI",
840+
"_title": "Preferences/Const/VelocityLeftkI"
841+
}
842+
},
843+
"1,0": {
844+
"size": [
845+
1,
846+
1
847+
],
848+
"content": {
849+
"_type": "Text View",
850+
"_source0": "network_table:///Preferences/Const/VelocityRightkD",
851+
"_title": "Preferences/Const/VelocityRightkD"
852+
}
853+
},
854+
"1,1": {
855+
"size": [
856+
1,
857+
1
858+
],
859+
"content": {
860+
"_type": "Text View",
861+
"_source0": "network_table:///Preferences/Const/VelocityRightkI",
862+
"_title": "Preferences/Const/VelocityRightkI"
863+
}
864+
},
865+
"2,0": {
866+
"size": [
867+
1,
868+
1
869+
],
870+
"content": {
871+
"_type": "Text View",
872+
"_source0": "network_table:///Preferences/Const/VPID Test Set",
873+
"_title": "Preferences/Const/VPID Test Set"
874+
}
875+
},
876+
"3,0": {
877+
"size": [
878+
1,
879+
1
880+
],
881+
"content": {
882+
"_type": "Text View",
883+
"_source0": "network_table:///Preferences/Const/kTimeoutMs",
884+
"_title": "Preferences/Const/kTimeoutMs"
885+
}
886+
},
887+
"3,1": {
888+
"size": [
889+
1,
890+
1
891+
],
892+
"content": {
893+
"_type": "Text View",
894+
"_source0": "network_table:///Preferences/Const/Motor Deadband",
895+
"_title": "Preferences/Const/Motor Deadband"
896+
}
897+
},
898+
"0,2": {
899+
"size": [
900+
1,
901+
1
902+
],
903+
"content": {
904+
"_type": "Text View",
905+
"_source0": "network_table:///Preferences/Const/VelocityLeftkF",
906+
"_title": "Preferences/Const/VelocityLeftkF"
907+
}
908+
},
909+
"1,2": {
910+
"size": [
911+
1,
912+
1
913+
],
914+
"content": {
915+
"_type": "Text View",
916+
"_source0": "network_table:///Preferences/Const/VelocityRightkF",
917+
"_title": "Preferences/Const/VelocityRightkF"
918+
}
919+
},
920+
"3,2": {
921+
"size": [
922+
2,
923+
1
924+
],
925+
"content": {
926+
"_type": "Text View",
927+
"_source0": "network_table:///SmartDashboard/Time Const (kD)",
928+
"_title": "SmartDashboard/Time Const (kD)"
929+
}
763930
}
764931
}
765932
}

0 commit comments

Comments
 (0)