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

Commit 4937c90

Browse files
lhmcgannlhmcgann
authored
Merge pull request #52 from lhmcgann/master
PID input/output ranges and un-overriding all of kate's stuff
2 parents 52a5c61 + 87d1e16 commit 4937c90

14 files changed

Lines changed: 311 additions & 31 deletions

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

Lines changed: 13 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -111,14 +111,15 @@ public RobotMap() {
111111
configSPX(dtLeftSlave);
112112
dtLeft = new SpeedControllerGroup(dtLeftMaster, dtLeftSlave);
113113

114-
leftVelocityController = new VelocityPIDController(Robot.getConst("MoveLeftkP", 1),
115-
Robot.getConst("MoveLeftkI", 0), Robot.getConst("MoveLeftkD", 0), 1 / Robot.getConst("MaxSpeed", 17),
116-
leftEncRate, dtLeft);
114+
leftVelocityController = new VelocityPIDController(Robot.getConst("VelocityLeftkP", 1),
115+
Robot.getConst("VelocityLeftkI", 0), Robot.getConst("VelocityLeftkD", 0),
116+
1 / Robot.getConst("Max Low Speed", 84), leftEncRate, dtLeft);
117117
leftVelocityController.enable();
118-
leftVelocityController.setInputRange(0, Double.MAX_VALUE);
118+
leftVelocityController.setInputRange(-Robot.getConst("Max High Speed", 204),
119+
Robot.getConst("Max High Speed", 204));
119120
leftVelocityController.setOutputRange(-1.0, 1.0);
120121
leftVelocityController.setContinuous(false);
121-
leftVelocityController.setAbsoluteTolerance(Robot.getConst("MoveToleranceLeft", 2));
122+
leftVelocityController.setAbsoluteTolerance(Robot.getConst("VelocityToleranceLeft", 2));
122123

123124
rightEncPort1 = new DigitalInput(getPort("1RightEnc", 2));
124125
rightEncPort2 = new DigitalInput(getPort("2RightEnc", 3));
@@ -132,16 +133,18 @@ public RobotMap() {
132133
configSPX(dtRightSlave);
133134
dtRight = new SpeedControllerGroup(dtRightMaster, dtRightSlave);
134135

135-
rightVelocityController = new VelocityPIDController(Robot.getConst("MoveRightkP", 1),
136-
Robot.getConst("MoveRightkI", 0), Robot.getConst("MoveRightkD", 0), 1 / Robot.getConst("MaxSpeed", 17),
137-
rightEncRate, dtRight);
136+
rightVelocityController = new VelocityPIDController(Robot.getConst("VelocityRightkP", 1),
137+
Robot.getConst("VelocityRightkI", 0), Robot.getConst("VelocityRightkD", 0),
138+
1 / Robot.getConst("Max Low Speed", 84), rightEncRate, dtRight);
138139
rightVelocityController.enable();
139-
rightVelocityController.setInputRange(0, Double.MAX_VALUE);
140+
rightVelocityController.setInputRange(-Robot.getConst("Max High Speed", 204),
141+
Robot.getConst("Max High Speed", 204));
140142
rightVelocityController.setOutputRange(-1.0, 1.0);
141143
rightVelocityController.setContinuous(false);
142-
rightVelocityController.setAbsoluteTolerance(Robot.getConst("MoveToleranceRight", 2));
144+
rightVelocityController.setAbsoluteTolerance(Robot.getConst("VelocityToleranceRight", 2));
143145

144146
robotDrive = new DifferentialDrive(leftVelocityController, rightVelocityController);
147+
robotDrive.setMaxOutput(Robot.getConst("Max High Speed", 204));
145148

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

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

Lines changed: 11 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -37,7 +37,7 @@ public VelocityPIDController(double kp, double ki, double kd, double kf, PIDSour
3737
* Sets the target speed
3838
*
3939
* @param speed
40-
* the target speed [-1, 1]
40+
* the target speed in inches/second
4141
*/
4242
@Override
4343
public void pidWrite(double output) {
@@ -48,7 +48,7 @@ public void pidWrite(double output) {
4848
* Sets the target speed
4949
*
5050
* @param speed
51-
* the target speed [-1, 1]
51+
* the target speed in inches/second
5252
*/
5353
@Override
5454
public void set(double speed) {
@@ -68,24 +68,29 @@ public double get() {
6868
}
6969

7070
/**
71+
* Invert this side of the DT (flip forwards and backwards).
7172
*
72-
* */
73+
* @param isInverted
74+
* invert this side of the DT or not
75+
*/
7376
@Override
7477
public void setInverted(boolean isInverted) {
7578
out.setInverted(isInverted);
7679
}
7780

7881
/**
82+
* Get whether or not this side of the DT is inverted.
7983
*
80-
* */
84+
* @return is this side of the DT inverted or not
85+
*/
8186
@Override
8287
public boolean getInverted() {
8388
return out.getInverted();
8489
}
8590

8691
/**
87-
*
88-
* */
92+
* Set the output to zero.
93+
*/
8994
@Override
9095
public void stopMotor() {
9196
out.stopMotor();

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

Lines changed: 5 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -23,15 +23,18 @@ public PIDMove(double targ, DrivetrainInterface dt, PIDSourceAverage avg) {
2323
target = targ;
2424
this.dt = dt;
2525
requires(Robot.dt);
26+
double kf = 1 / (dt.getCurrentMaxSpeed() * Robot.getConst("Default PID Update Time", 0.05));
2627
moveController = new PIDController(Robot.getConst("MovekP", 1), Robot.getConst("MovekI", 0),
27-
Robot.getConst("MovekD", 0), avg, this);
28+
Robot.getConst("MovekD", 0), kf, avg, this);
2829
}
2930

3031
// Called just before this Command runs the first time
3132
@Override
3233
public void initialize() {
3334
dt.resetDistEncs();
34-
moveController.setInputRange(0, Double.MAX_VALUE);
35+
// input is in inches
36+
moveController.setInputRange(-Robot.getConst("Max High Speed", 204), Robot.getConst("Max High Speed", 204));
37+
// output in "motor units" (arcade and tank only accept values [-1, 1]
3538
moveController.setOutputRange(-1.0, 1.0);
3639
moveController.setContinuous(false);
3740
moveController.setAbsoluteTolerance(Robot.getConst("MoveTolerance", 2));

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

Lines changed: 8 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -24,16 +24,23 @@ public PIDTurn(double targ, DrivetrainInterface dt, AHRS ahrs) {
2424
target = targ;
2525
this.dt = dt;
2626
requires(Robot.dt);
27+
// calculates the maximum turning speed in degrees/sec based on the max linear
28+
// speed in inches/s and the distance (inches) between sides of the DT
29+
double maxTurnSpeed = dt.getCurrentMaxSpeed() * 360
30+
/ (Math.PI * Robot.getConst("Distance Between Wheels", 26.25));
31+
double kf = 1 / (maxTurnSpeed * Robot.getConst("Default PID Update Time", 0.05));
2732
turnController = new PIDController(Robot.getConst("TurnkP", 1), Robot.getConst("TurnkI", 0),
28-
Robot.getConst("TurnkD", 0), ahrs, this);
33+
Robot.getConst("TurnkD", 0), kf, ahrs, this);
2934
}
3035

3136
// Called just before this Command runs the first time
3237
@Override
3338
protected void initialize() {
3439
dt.resetAHRS();
3540
turnController.disable();
41+
// input is in degrees
3642
turnController.setInputRange(-180, 180);
43+
// output in "motor units" (arcade and tank only accept values [-1, 1]
3744
turnController.setOutputRange(-1.0, 1.0);
3845
turnController.setContinuous();
3946
turnController.setAbsoluteTolerance(Robot.getConst("TurnTolerance", 1));

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

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -21,5 +21,6 @@ public ShiftHighGear() {
2121
protected void initialize() {
2222
Robot.dt.shiftGears(true);
2323
SmartDashboard.putBoolean("High Gear", true);
24+
Robot.dt.resetVelocityPIDkFConsts();
2425
}
2526
}

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

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -21,5 +21,6 @@ public ShiftLowGear() {
2121
protected void initialize() {
2222
Robot.dt.shiftGears(false);
2323
SmartDashboard.putBoolean("High Gear", false);
24+
Robot.dt.resetVelocityPIDkFConsts();
2425
}
2526
}

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

Lines changed: 41 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -2,13 +2,17 @@
22

33
import edu.wpi.first.wpilibj.command.Subsystem;
44

5+
import org.usfirst.frc.team199.Robot2018.RobotMap;
6+
7+
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
8+
59
/**
610
*
711
*/
812
public class Climber extends Subsystem implements ClimberInterface {
913

10-
// Put methods for controlling this subsystem
11-
// here. Call these from Commands.
14+
private final WPI_TalonSRX climberMotor = RobotMap.climberMotor;
15+
1216

1317
/**
1418
* Set the default command for a subsystem here.
@@ -17,5 +21,40 @@ public void initDefaultCommand() {
1721
// Set the default command for a subsystem here.
1822
//setDefaultCommand(new MySpecialCommand());
1923
}
24+
25+
26+
27+
28+
/**
29+
* runs the motors
30+
*
31+
*/
32+
public void runClimber(double speed) {
33+
34+
}
35+
36+
/**
37+
* attaches the climber hook to the lift.
38+
* Requires that Lift is on the ground
39+
*/
40+
public void attachToLift() {
41+
42+
}
43+
44+
/**
45+
* attaches hook to bar and releases it from the lift
46+
*/
47+
public void attachToBar() {
48+
49+
}
50+
51+
/**
52+
* stops the climber
53+
*/
54+
public void stopClimber() {
55+
climberMotor.stopMotor();
56+
}
57+
58+
2059
}
2160

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

Lines changed: 22 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -7,4 +7,26 @@ public interface ClimberInterface {
77
* */
88
public void initDefaultCommand();
99

10+
/**
11+
* runs the motors
12+
*/
13+
public void runClimber(double speed);
14+
15+
/**
16+
* attaches the climber hook to the lift.
17+
* Requires that Lift is on the ground
18+
*/
19+
public void attachToLift();
20+
21+
/**
22+
* attaches hook to bar and releases it from the lift
23+
*/
24+
public void attachToBar();
25+
26+
/**
27+
* stops the climber
28+
*/
29+
public void stopClimber();
30+
31+
1032
}

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

Lines changed: 37 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -111,10 +111,11 @@ public double getDtRightSpeed() {
111111
*/
112112
@Override
113113
public void updatePidConstants() {
114-
leftVelocityController.setPID(Robot.getConst("MoveLeftkP", 1), Robot.getConst("MoveLeftkI", 0),
115-
Robot.getConst("MoveLeftkD", 0), 1 / Robot.getConst("MaxSpeed", 17));
116-
rightVelocityController.setPID(Robot.getConst("MoveRightkP", 1), Robot.getConst("MoveRightkI", 0),
117-
Robot.getConst("MoveRightkD", 0), 1 / Robot.getConst("MaxSpeed", 17));
114+
leftVelocityController.setPID(Robot.getConst("VelocityLeftkP", 1), Robot.getConst("VelocityLeftkI", 0),
115+
Robot.getConst("VelocityLeftkD", 0));
116+
rightVelocityController.setPID(Robot.getConst("VelocityRightkP", 1), Robot.getConst("VelocityRightkI", 0),
117+
Robot.getConst("VelocityRightkD", 0));
118+
resetVelocityPIDkFConsts();
118119
}
119120

120121
/**
@@ -209,7 +210,7 @@ public double getRightEncDist() {
209210
}
210211

211212
/**
212-
* Activates the solenoid to push the drivetrain into high or low gear
213+
* Activates the solenoid to push the drivetrain into high or low gear.
213214
*
214215
* @param highGear
215216
* If the solenoid is to be pushed into high gear (true, kForward) or
@@ -231,4 +232,35 @@ public void shiftGears(boolean highGear) {
231232
public void shiftGearSolenoidOff() {
232233
dtGear.set(DoubleSolenoid.Value.kOff);
233234
}
235+
236+
/**
237+
* Reset the kf constants for both VelocityPIDControllers based on current DT
238+
* gearing (high or low gear).
239+
*
240+
* @param newKF
241+
* the new kF constant based on high and low gear max speeds; should
242+
* be 1 / max speed
243+
* @return the new kF value as 1 / correct max speed
244+
*/
245+
@Override
246+
public double resetVelocityPIDkFConsts() {
247+
double newKF = 1 / getCurrentMaxSpeed();
248+
leftVelocityController.setF(newKF);
249+
rightVelocityController.setF(newKF);
250+
return newKF;
251+
}
252+
253+
/**
254+
* Gets the current max speed of the DT based on gearing (high or low gear)
255+
*
256+
* @return the current max speed of the DT in inches/second
257+
*/
258+
@Override
259+
public double getCurrentMaxSpeed() {
260+
if (Robot.getBool("High Gear", true)) {
261+
return Robot.getConst("Max High Speed", 204);
262+
} else {
263+
return Robot.getConst("Max Low Speed", 84);
264+
}
265+
}
234266
}

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

Lines changed: 18 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -124,4 +124,22 @@ public interface DrivetrainInterface {
124124
* Stops the solenoid that pushes the drivetrain into low or high gear
125125
*/
126126
public void shiftGearSolenoidOff();
127+
128+
/**
129+
* Reset the kf constants for both VelocityPIDControllers based on current DT
130+
* gearing (high or low gear).
131+
*
132+
* @param newKF
133+
* the new kF constant based on high and low gear max speeds; should
134+
* be 1 / max speed
135+
* @return the new kF value as 1 / correct max speed
136+
*/
137+
public double resetVelocityPIDkFConsts();
138+
139+
/**
140+
* Gets the current max speed of the DT based on gearing (high or low gear)
141+
*
142+
* @return the current max speed of the DT in inches/second
143+
*/
144+
public double getCurrentMaxSpeed();
127145
}

0 commit comments

Comments
 (0)