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

Commit 4957c07

Browse files
lhmcgannlhmcgann
committed
talon pid stuff
# Conflicts: # Robot2018/src/org/usfirst/frc/team199/Robot2018/RobotMap.java
1 parent 507e4e7 commit 4957c07

File tree

7 files changed

+394
-113
lines changed

7 files changed

+394
-113
lines changed

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

Lines changed: 62 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -8,6 +8,7 @@
88
package org.usfirst.frc.team199.Robot2018;
99

1010
import org.usfirst.frc.team199.Robot2018.autonomous.PIDSourceAverage;
11+
import org.usfirst.frc.team199.Robot2018.autonomous.TalonVelocityController;
1112
import org.usfirst.frc.team199.Robot2018.autonomous.VelocityPIDController;
1213

1314
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
@@ -44,6 +45,7 @@ public class RobotMap {
4445
public static WPI_VictorSPX dtLeftSlave;
4546
public static SpeedControllerGroup dtLeft;
4647
public static VelocityPIDController leftVelocityController;
48+
public static TalonVelocityController leftTalVelController;
4749

4850
public static DigitalSource rightEncPort1;
4951
public static DigitalSource rightEncPort2;
@@ -53,6 +55,7 @@ public class RobotMap {
5355
public static WPI_VictorSPX dtRightSlave;
5456
public static SpeedControllerGroup dtRight;
5557
public static VelocityPIDController rightVelocityController;
58+
public static TalonVelocityController rightTalVelController;
5659

5760
public static DifferentialDrive robotDrive;
5861
public static PIDSourceAverage distEncAvg;
@@ -121,15 +124,17 @@ public RobotMap() {
121124
configSPX(dtLeftSlave);
122125
dtLeft = new SpeedControllerGroup(dtLeftMaster, dtLeftSlave);
123126

124-
leftVelocityController = new VelocityPIDController(Robot.getConst("VelocityLeftkP", 1),
125-
Robot.getConst("VelocityLeftkI", 0), Robot.getConst("VelocityLeftkD", 0),
126-
1 / Robot.getConst("Max Low Speed", 84), leftEncRate, dtLeft);
127-
leftVelocityController.enable();
128-
leftVelocityController.setInputRange(-Robot.getConst("Max High Speed", 204),
129-
Robot.getConst("Max High Speed", 204));
130-
leftVelocityController.setOutputRange(-1.0, 1.0);
131-
leftVelocityController.setContinuous(false);
132-
leftVelocityController.setAbsoluteTolerance(Robot.getConst("VelocityToleranceLeft", 2));
127+
// leftVelocityController = new
128+
// VelocityPIDController(Robot.getConst("VelocityLeftkP", 1),
129+
// Robot.getConst("VelocityLeftkI", 0), Robot.getConst("VelocityLeftkD", 0),
130+
// 1 / Robot.getConst("Max Low Speed", 84), leftEncRate, dtLeft);
131+
// leftVelocityController.enable();
132+
// leftVelocityController.setInputRange(-Robot.getConst("Max High Speed", 204),
133+
// Robot.getConst("Max High Speed", 204));
134+
// leftVelocityController.setOutputRange(-1.0, 1.0);
135+
// leftVelocityController.setContinuous(false);
136+
// leftVelocityController.setAbsoluteTolerance(Robot.getConst("VelocityToleranceLeft",
137+
// 2));
133138

134139
rightEncPort1 = new DigitalInput(getPort("1RightEnc", 2));
135140
rightEncPort2 = new DigitalInput(getPort("2RightEnc", 3));
@@ -143,17 +148,54 @@ public RobotMap() {
143148
configSPX(dtRightSlave);
144149
dtRight = new SpeedControllerGroup(dtRightMaster, dtRightSlave);
145150

146-
rightVelocityController = new VelocityPIDController(Robot.getConst("VelocityRightkP", 1),
147-
Robot.getConst("VelocityRightkI", 0), Robot.getConst("VelocityRightkD", 0),
148-
1 / Robot.getConst("Max Low Speed", 84), rightEncRate, dtRight);
149-
rightVelocityController.enable();
150-
rightVelocityController.setInputRange(-Robot.getConst("Max High Speed", 204),
151-
Robot.getConst("Max High Speed", 204));
152-
rightVelocityController.setOutputRange(-1.0, 1.0);
153-
rightVelocityController.setContinuous(false);
154-
rightVelocityController.setAbsoluteTolerance(Robot.getConst("VelocityToleranceRight", 2));
155-
156-
robotDrive = new DifferentialDrive(leftVelocityController, rightVelocityController);
151+
// rightVelocityController = new
152+
// VelocityPIDController(Robot.getConst("VelocityRightkP", 1),
153+
// Robot.getConst("VelocityRightkI", 0), Robot.getConst("VelocityRightkD", 0),
154+
// 1 / Robot.getConst("Max Low Speed", 84), rightEncRate, dtRight);
155+
// rightVelocityController.enable();
156+
// rightVelocityController.setInputRange(-Robot.getConst("Max High Speed", 204),
157+
// Robot.getConst("Max High Speed", 204));
158+
// rightVelocityController.setOutputRange(-1.0, 1.0);
159+
// rightVelocityController.setContinuous(false);
160+
// rightVelocityController.setAbsoluteTolerance(Robot.getConst("VelocityToleranceRight",
161+
// 2));
162+
//
163+
// robotDrive = new DifferentialDrive(leftVelocityController,
164+
// rightVelocityController);
165+
// robotDrive.setMaxOutput(Robot.getConst("Max High Speed", 204));
166+
167+
if (!Robot.getBool("Talon PID", false)) {
168+
leftVelocityController = new VelocityPIDController(Robot.getConst("VelocityLeftkI", 0),
169+
/* Robot.getConst("VelocityLeftkD", 0) */ 0, Robot.getConst("VelocityLeftkP", 0),
170+
1 / Robot.getConst("Max Low Speed", 84), leftEncRate, dtLeft);
171+
leftVelocityController.setInputRange(-Robot.getConst("Max High Speed", 204),
172+
Robot.getConst("Max High Speed", 204));
173+
leftVelocityController.setOutputRange(-1.0, 1.0);
174+
leftVelocityController.setContinuous(false);
175+
leftVelocityController.setAbsoluteTolerance(Robot.getConst("VelocityToleranceLeft", 2));
176+
177+
rightVelocityController = new VelocityPIDController(Robot.getConst("VelocityRightkI", 0),
178+
/* Robot.getConst("VelocityRightkD", 0) */ 0, Robot.getConst("VelocityRightkP", 0),
179+
1 / Robot.getConst("Max Low Speed", 84), rightEncRate, dtRight);
180+
rightVelocityController.setInputRange(-Robot.getConst("Max High Speed", 204),
181+
Robot.getConst("Max High Speed", 204));
182+
rightVelocityController.setOutputRange(-1.0, 1.0);
183+
rightVelocityController.setContinuous(false);
184+
rightVelocityController.setAbsoluteTolerance(Robot.getConst("VelocityToleranceRight", 2));
185+
186+
robotDrive = new DifferentialDrive(leftVelocityController, rightVelocityController);
187+
} else {
188+
leftTalVelController = new TalonVelocityController(Robot.getConst("VelocityLeftkP", 0),
189+
Robot.getConst("VelocityLeftkI", 0), Robot.getConst("VelocityLeftkD", 0),
190+
1 / Robot.getConst("Max Low Speed", 84), leftEncRate, dtLeftMaster, 0, 0);
191+
192+
rightTalVelController = new TalonVelocityController(Robot.getConst("VelocityRightkP", 0),
193+
Robot.getConst("VelocityRightkI", 0), Robot.getConst("VelocityRightkD", 0),
194+
1 / Robot.getConst("Max Low Speed", 84), rightEncRate, dtRightMaster, 0, 0);
195+
196+
robotDrive = new DifferentialDrive(leftTalVelController, rightTalVelController);
197+
}
198+
157199
robotDrive.setMaxOutput(Robot.getConst("Max High Speed", 204));
158200

159201
distEncAvg = new PIDSourceAverage(leftEncDist, rightEncDist);

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

Lines changed: 47 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,47 @@
1+
package org.usfirst.frc.team199.Robot2018.autonomous;
2+
3+
import com.ctre.phoenix.motorcontrol.ControlMode;
4+
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
5+
6+
import edu.wpi.first.wpilibj.PIDSource;
7+
import edu.wpi.first.wpilibj.PIDSourceType;
8+
9+
public class PIDSourceFromTalon implements PIDSource {
10+
11+
private PIDSourceType type;
12+
private WPI_TalonSRX talon;
13+
14+
public PIDSourceFromTalon(WPI_TalonSRX talon) {
15+
this.talon = talon;
16+
ControlMode mode = talon.getControlMode();
17+
switch (mode.toString()) {
18+
case "Position":
19+
type = PIDSourceType.kDisplacement;
20+
break;
21+
default:
22+
type = PIDSourceType.kRate;
23+
break;
24+
}
25+
}
26+
27+
@Override
28+
public void setPIDSourceType(PIDSourceType pidSource) {
29+
type = pidSource;
30+
}
31+
32+
@Override
33+
public PIDSourceType getPIDSourceType() {
34+
return type;
35+
}
36+
37+
@Override
38+
public double pidGet() {
39+
switch (type.toString()) {
40+
case "kDisplacement":
41+
return talon.getSelectedSensorPosition(0);
42+
default:
43+
return talon.getSelectedSensorVelocity(0);
44+
}
45+
}
46+
47+
}

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

Lines changed: 141 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -1,57 +1,184 @@
11
package org.usfirst.frc.team199.Robot2018.autonomous;
22

3+
import com.ctre.phoenix.motorcontrol.ControlMode;
4+
import com.ctre.phoenix.motorcontrol.FeedbackDevice;
35
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
46

7+
import edu.wpi.first.wpilibj.Encoder;
8+
import edu.wpi.first.wpilibj.PIDSource;
9+
import edu.wpi.first.wpilibj.PIDSourceType;
510
import edu.wpi.first.wpilibj.SpeedController;
611

712
public class TalonVelocityController implements SpeedController {
813

914
private WPI_TalonSRX talon;
1015

11-
public TalonVelocityController(WPI_TalonSRX output) {
16+
/**
17+
* Constructs a TalonVelocityContoller. Sets the four PID constants and the
18+
* FeedbackDevice of the output Talon.
19+
*
20+
* @param kp
21+
* the proportional PID constant, multiplied by the error
22+
* @param ki
23+
* the integral PID constant, multiplied by the total error
24+
* @param kd
25+
* the derivative PID constant, multiplied by the change in error
26+
* @param kf
27+
* the feed forward value: should be 1/MaxSpeed
28+
* @param source
29+
* the sensor (e.g. velocity encoder) you are reading from
30+
* @param output
31+
* the Talon SRX you are telling what to do
32+
* @param slotIdx
33+
* Parameter slot for the constant.
34+
* @param timeoutMs
35+
* Timeout value in ms. If nonzero, function will wait for config
36+
* success and report an error if it times out. If zero, no blocking
37+
* or checking is performed.
38+
*/
39+
public TalonVelocityController(double kp, double ki, double kd, double kf, PIDSource source, WPI_TalonSRX output,
40+
int slotIdx, int timeoutMs) {
1241
talon = output;
42+
talon.config_kP(slotIdx, kp, timeoutMs);
43+
talon.config_kI(slotIdx, ki, timeoutMs);
44+
talon.config_kD(slotIdx, kd, timeoutMs);
45+
talon.config_kF(slotIdx, kf, timeoutMs);
46+
if (source instanceof Encoder && source.getPIDSourceType().equals(PIDSourceType.kRate)) {
47+
talon.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder, 0, timeoutMs);
48+
} else {
49+
throw new IllegalArgumentException();
50+
}
1351
}
1452

53+
/**
54+
* Sets the output of the output Talon based on the specified Velocity
55+
* ControlMode.
56+
*
57+
* @param output
58+
* setpoint value in inches/second
59+
*/
1560
@Override
1661
public void pidWrite(double output) {
17-
// TODO Auto-generated method stub
1862
set(output);
1963
}
2064

65+
/**
66+
* Sets the output of the output Talon based on the specified Velocity
67+
* ControlMode.
68+
*
69+
* @param output
70+
* setpoint value in inches/second
71+
*/
2172
@Override
22-
public void set(double speed) {
23-
// TODO Auto-generated method stub
24-
73+
public void set(double output) {
74+
// the div. by 10 is to translate from our desired param units (in/s) to the
75+
// Talon's set() method param units (in/100ms)
76+
talon.set(ControlMode.Velocity, output / 10);
2577
}
2678

79+
/**
80+
* Get the current set speed of this side of the DT.
81+
*
82+
* @return the current set speed of this side of the DT from [-1, 1]
83+
*/
2784
@Override
2885
public double get() {
29-
// TODO Auto-generated method stub
30-
return 0;
86+
return talon.get();
3187
}
3288

89+
/**
90+
* Sets the kP constant.
91+
*
92+
* @param value
93+
* the value to set kP to
94+
*/
95+
public void setP(double value) {
96+
talon.config_kP(0, value, 0);
97+
}
98+
99+
/**
100+
* Sets the kI constant.
101+
*
102+
* @param value
103+
* the value to set kI to
104+
*/
105+
public void setI(double value) {
106+
talon.config_kI(0, value, 0);
107+
}
108+
109+
/**
110+
* Sets the kD constant.
111+
*
112+
* @param value
113+
* the value to set kD to
114+
*/
115+
public void setD(double value) {
116+
talon.config_kD(0, value, 0);
117+
}
118+
119+
/**
120+
* Sets the feed forward gain.
121+
*
122+
* @param value
123+
* the value to set kF to
124+
*/
125+
public void setF(double value) {
126+
talon.config_kF(0, value, 0);
127+
}
128+
129+
/**
130+
* Invert this side of the DT (flip forwards and backwards).
131+
*
132+
* @param isInverted
133+
* invert this side of the DT or not
134+
*/
33135
@Override
34136
public void setInverted(boolean isInverted) {
35-
// TODO Auto-generated method stub
36-
137+
talon.setInverted(isInverted);
37138
}
38139

140+
/**
141+
* Get whether or not this side of the DT is inverted.
142+
*
143+
* @return is this side of the DT inverted or not
144+
*/
39145
@Override
40146
public boolean getInverted() {
41-
// TODO Auto-generated method stub
42-
return false;
147+
return talon.getInverted();
43148
}
44149

150+
/**
151+
* Disable this controller.
152+
*/
45153
@Override
46154
public void disable() {
47-
// TODO Auto-generated method stub
48-
155+
talon.disable();
49156
}
50157

158+
/**
159+
* Stop the motor until Set is called again.
160+
*/
51161
@Override
52162
public void stopMotor() {
53-
// TODO Auto-generated method stub
163+
talon.stopMotor();
164+
}
165+
166+
/**
167+
* Gets the selected sensor position (in raw sensor units).
168+
*
169+
* @return the selected sensor position (in raw sensor units).
170+
*/
171+
public double getCurrentPosition() {
172+
return talon.getSelectedSensorPosition(0);
173+
}
54174

175+
/**
176+
* Gets the selected sensor velocity (in raw sensor units per 100ms).
177+
*
178+
* @return the selected sensor velocity (in raw sensor units per 100ms).
179+
*/
180+
public double getCurrentVelocity() {
181+
return talon.getSelectedSensorVelocity(0);
55182
}
56183

57184
}

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

Lines changed: 6 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -11,15 +11,16 @@ public class VelocityPIDController extends PIDController implements SpeedControl
1111

1212
/**
1313
* Constructs a VelocityPIDContoller and invokes the super constructor
14-
* (PIDController), setting the three PID constants and the source and output
15-
* for this VelocityPIDController.
14+
* (PIDController), setting the four PID constants and the source and output for
15+
* this VelocityPIDController.
1616
*
1717
* @param kp
18-
* the proportional PID constant
18+
* the proportional PID constant, multiplied by the total error (aka
19+
* I)
1920
* @param ki
20-
* the integral PID constant
21+
* the integral PID constant, not used in velocity control
2122
* @param kd
22-
* the derivative PID constant
23+
* the derivative PID constant, multiplied by the error (aka P)
2324
* @param kf
2425
* the feed forward value: should be 1/MaxSpeed
2526
* @param source

0 commit comments

Comments
 (0)