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

Commit 255ca24

Browse files
DriverStationComputerDriverStationComputer
committed
add velocity pid and other small changes
1 parent 385e1fb commit 255ca24

6 files changed

Lines changed: 103 additions & 36 deletions

File tree

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

Lines changed: 47 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -108,4 +108,51 @@ public OI() {
108108
// outake = new JoystickButton(manipulator, getButton("Outake Button", 6));
109109
// outake.whenPressed(new OutakeCube());
110110
}
111+
112+
// /**
113+
// * Returns the getY from leftJoy squared (preserving sign)
114+
// *
115+
// * @return The y value squared
116+
// */
117+
// public double squareLeftY() {
118+
// return leftJoy.getY() * leftJoy.getY() * Math.signum(leftJoy.getY());
119+
// }
120+
//
121+
// /**
122+
// * Returns the getY from rightJoy squared (preserving sign)
123+
// *
124+
// * @return The y value squared
125+
// */
126+
// public double squareRightY() {
127+
// return rightJoy.getY() * rightJoy.getY() * Math.signum(rightJoy.getY());
128+
// }
129+
//
130+
// /**
131+
// * Returns the getX from leftJoy squared (preserving sign)
132+
// *
133+
// * @return The x value squared
134+
// */
135+
// public double squareLeftX() {
136+
// return leftJoy.getX() * leftJoy.getX() * Math.signum(leftJoy.getX());
137+
// }
138+
//
139+
// /**
140+
// * Returns the getX from rightJoy squared (preserving sign)
141+
// *
142+
// * @return The x value squared
143+
// */
144+
// public double squareRightX() {
145+
// return rightJoy.getX() * rightJoy.getX() * Math.signum(rightJoy.getX());
146+
// }
147+
148+
/**
149+
* Used to square joystick values while keeping sign
150+
*
151+
* @param joyVal
152+
* The joystick value to stick
153+
* @return The squared joystick value with same sign
154+
*/
155+
public double squareValueKeepSign(double joyVal) {
156+
return joyVal * joyVal * Math.signum(joyVal);
157+
}
111158
}

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

Lines changed: 11 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -195,10 +195,9 @@ public RobotMap() {
195195
rightVelocityController.setContinuous(false);
196196
rightVelocityController.setAbsoluteTolerance(Robot.getConst("VelocityToleranceRight", 2));
197197

198-
// robotDrive = new DifferentialDrive(leftVelocityController,
199-
// rightVelocityController);
200-
// robotDrive.setMaxOutput(Robot.getConst("Max High Speed", 204));
201-
robotDrive = new DifferentialDrive(dtLeft, dtRight);
198+
robotDrive = new DifferentialDrive(leftVelocityController, rightVelocityController);
199+
robotDrive.setMaxOutput(Robot.getConst("Max High Speed", 204));
200+
// robotDrive = new DifferentialDrive(dtLeft, dtRight);
202201

203202
distEncAvg = new PIDSourceAverage(leftEncDist, rightEncDist);
204203
fancyGyro = new AHRS(SPI.Port.kMXP);
@@ -241,9 +240,8 @@ public double calcDefkD(double maxSpeed) {
241240
*/
242241
double gearReduction = getGearRatio();
243242
double radius = getRadius();
244-
double timeConstant = getOmegaMax() / gearReduction / 60 * 2 * Math.PI
245-
* convertNtokG(getWeight()) / 2 * radius * radius
246-
/ (getStallTorque() * gearReduction * 2);
243+
double timeConstant = getOmegaMax() / gearReduction / 60 * 2 * Math.PI * convertNtokG(getWeight()) / 2 * radius
244+
* radius / (getStallTorque() * gearReduction * 2);
247245
double cycleTime = getCycleTime();
248246
/*
249247
* The denominator of kD is 1-(e ^ -cycleTime / timeConstant). The numerator is
@@ -252,28 +250,28 @@ public double calcDefkD(double maxSpeed) {
252250
double denominator = Math.pow(Math.E, 1 * cycleTime / timeConstant) - 1;
253251
return 1 / denominator / maxSpeed;
254252
}
255-
253+
256254
public double getGearRatio() {
257255
return Robot.getBool("High Gear", false) ? Robot.getConst("High Gear Gear Reduction", 5.392)
258256
: Robot.getConst("Low Gear Gear Reduction", 12.255);
259257
}
260-
258+
261259
public double getRadius() {
262260
return Robot.getConst("Radius of Drivetrain Wheel", 0.0635);
263261
}
264-
262+
265263
public double getOmegaMax() {
266264
return Robot.getConst("Omega Max", 5330);
267265
}
268-
266+
269267
public double getWeight() {
270268
return Robot.getConst("Weight of Robot", 342);
271269
}
272-
270+
273271
public double getCycleTime() {
274272
return Robot.getConst("Code cycle time", 0.05);
275273
}
276-
274+
277275
public double getStallTorque() {
278276
return Robot.getConst("Stall Torque", 2.41);
279277
}

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

Lines changed: 9 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -58,9 +58,10 @@ public PIDMove(double targ, DrivetrainInterface dt, SmartDashboardInterface sd,
5858
*/
5959
@Override
6060
protected double calculateFeedForward() {
61+
double originalFF = super.calculateFeedForward();
6162
double feedForwardConst = dt.getPIDMoveConstant();
62-
double setPt = getSetpoint();
63-
return (setPt / Math.abs(setPt)) * feedForwardConst * Math.sqrt(Math.abs(setPt));
63+
double error = getError();
64+
return Math.signum(error) * feedForwardConst * Math.sqrt(Math.abs(error)) + originalFF;
6465
}
6566
};
6667
sd.putData("Move PID", moveController);
@@ -105,9 +106,10 @@ public PIDMove(double[] point, DrivetrainInterface dt, SmartDashboardInterface s
105106
*/
106107
@Override
107108
protected double calculateFeedForward() {
109+
double originalFF = super.calculateFeedForward();
108110
double feedForwardConst = dt.getPIDMoveConstant();
109-
double setPt = getSetpoint();
110-
return (setPt / Math.abs(setPt)) * feedForwardConst * Math.sqrt(Math.abs(setPt));
111+
double error = getError();
112+
return Math.signum(error) * feedForwardConst * Math.sqrt(Math.abs(error)) + originalFF;
111113
}
112114
};
113115
sd.putData("Move PID", moveController);
@@ -137,7 +139,7 @@ public void initialize() {
137139
moveController.setSetpoint(target);
138140

139141
moveController.enable();
140-
// dt.enableVelocityPIDs();
142+
dt.enableVelocityPIDs();
141143
}
142144

143145
/**
@@ -185,6 +187,8 @@ protected void end() {
185187
AutoUtils.position.changeY(y);
186188

187189
AutoUtils.position.setRot(dt.getAHRSAngle());
190+
191+
dt.disableVelocityPIDs();
188192
}
189193

190194
/**

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

Lines changed: 11 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -76,10 +76,11 @@ public PIDTurn(double targ, DrivetrainInterface dt, SmartDashboardInterface sd,
7676
*/
7777
@Override
7878
protected double calculateFeedForward() {
79+
double originalFF = super.calculateFeedForward();
7980
double feedForwardConst = dt.getPIDTurnConstant();
80-
double setpt = getSetpoint();
81-
return feedForwardConst * (getDistanceBetweenWheels() / 2) * (setpt / Math.abs(setpt))
82-
* Math.sqrt(Math.abs(setpt));
81+
double error = getError();
82+
return feedForwardConst * (getDistanceBetweenWheels() / 2) * Math.signum(error)
83+
* Math.sqrt(Math.abs(error)) + originalFF;
8384
}
8485
};
8586
// tim = new Timer();
@@ -152,10 +153,11 @@ public PIDTurn(double[] point, DrivetrainInterface dt, SmartDashboardInterface s
152153
*/
153154
@Override
154155
protected double calculateFeedForward() {
156+
double originalFF = super.calculateFeedForward();
155157
double feedForwardConst = dt.getPIDTurnConstant();
156-
double setpt = getSetpoint();
157-
return feedForwardConst * (getDistanceBetweenWheels() / 2) * (setpt / Math.abs(setpt))
158-
* Math.sqrt(Math.abs(setpt));
158+
double error = getError();
159+
return feedForwardConst * (getDistanceBetweenWheels() / 2) * Math.signum(error)
160+
* Math.sqrt(Math.abs(error)) + originalFF;
159161
}
160162
};
161163
// tim = new Timer();
@@ -189,7 +191,7 @@ protected void initialize() {
189191
System.out.println("Turn to point: " + turnToPoint);
190192

191193
turnController.disable();
192-
// dt.enableVelocityPIDs();
194+
dt.enableVelocityPIDs();
193195
System.out.println("initialize2s");
194196
// dt.resetAHRS();
195197
System.out.println("after reset");
@@ -260,6 +262,8 @@ protected void end() {
260262
// turnController.free();
261263

262264
AutoUtils.position.setRot(dt.getAHRSAngle());
265+
266+
dt.disableVelocityPIDs();
263267
}
264268

265269
/**

‎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: 24 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -44,7 +44,7 @@ public class Drivetrain extends Subsystem implements DrivetrainInterface {
4444
private final DifferentialDrive robotDrive = RobotMap.robotDrive;
4545
private final VelocityPIDController leftVelocityController = RobotMap.leftVelocityController;
4646
private final VelocityPIDController rightVelocityController = RobotMap.rightVelocityController;
47-
47+
4848
private final AHRS fancyGyro = RobotMap.fancyGyro;
4949
private final DoubleSolenoid dtGear = RobotMap.dtGear;
5050

@@ -150,14 +150,26 @@ public void dtRightPIDDrive(double value) {
150150
*/
151151
@Override
152152
public void teleopDrive() {
153+
boolean squareJoy = Robot.getBool("Square Joystick Values", true);
153154
if (Robot.getBool("Arcade Drive", true)) {
155+
double forw;
156+
double turn;
154157
if (Robot.getBool("Arcade Drive Default Setup", true)) {
155-
Robot.dt.arcadeDrive(-Robot.oi.leftJoy.getY(), Robot.oi.rightJoy.getX());
158+
forw = -Robot.oi.leftJoy.getY();
159+
turn = Robot.oi.rightJoy.getX();
160+
Robot.dt.arcadeDrive(squareJoy ? Robot.oi.squareValueKeepSign(forw) : forw,
161+
squareJoy ? Robot.oi.squareValueKeepSign(turn) : turn);
156162
} else {
157-
Robot.dt.arcadeDrive(-Robot.oi.rightJoy.getY(), Robot.oi.leftJoy.getX());
163+
forw = -Robot.oi.rightJoy.getY();
164+
turn = Robot.oi.leftJoy.getX();
165+
Robot.dt.arcadeDrive(squareJoy ? Robot.oi.squareValueKeepSign(forw) : forw,
166+
squareJoy ? Robot.oi.squareValueKeepSign(turn) : turn);
158167
}
159168
} else {
160-
Robot.dt.tankDrive(-Robot.oi.leftJoy.getY(), -Robot.oi.rightJoy.getY());
169+
double left = -Robot.oi.leftJoy.getY();
170+
double right = -Robot.oi.rightJoy.getY();
171+
Robot.dt.tankDrive(squareJoy ? Robot.oi.squareValueKeepSign(left) : left,
172+
squareJoy ? Robot.oi.squareValueKeepSign(right) : right);
161173
}
162174
SmartDashboard.putNumber("Drivetrain/Left VPID Targ", leftVelocityController.getSetpoint());
163175
SmartDashboard.putNumber("Drivetrain/Right VPID Targ", rightVelocityController.getSetpoint());
@@ -222,8 +234,10 @@ public double getDtRightSpeed() {
222234
*/
223235
@Override
224236
public void updatePidConstants() {
225-
leftVelocityController.setPID(Robot.getConst("VelocityLeftkI", 0), 0, Robot.rmap.calcDefkD(getCurrentMaxSpeed()));
226-
rightVelocityController.setPID(Robot.getConst("VelocityRightkI", 0), 0, Robot.rmap.calcDefkD(getCurrentMaxSpeed()));
237+
leftVelocityController.setPID(Robot.getConst("VelocityLeftkI", 0), 0,
238+
Robot.rmap.calcDefkD(getCurrentMaxSpeed()));
239+
rightVelocityController.setPID(Robot.getConst("VelocityRightkI", 0), 0,
240+
Robot.rmap.calcDefkD(getCurrentMaxSpeed()));
227241
resetVelocityPIDkFConsts();
228242
}
229243

@@ -408,17 +422,17 @@ public double getPIDMoveConstant() {
408422
double T = Robot.rmap.getStallTorque();
409423
double R = Robot.rmap.getRadius();
410424
double M = Robot.rmap.getWeight();
411-
return Math.sqrt((8*T*G)/(R*M));
425+
return Math.sqrt((8 * T * G) / (R * M));
412426
}
413-
427+
414428
public double getPIDTurnConstant() {
415429
double G = Robot.rmap.getGearRatio();
416430
double T = Robot.rmap.getStallTorque();
417431
double R = Robot.rmap.getRadius();
418432
double M = Robot.rmap.getWeight();
419-
return 4 * Math.sqrt((T*G) / (R*M));
433+
return 4 * Math.sqrt((T * G) / (R * M));
420434
}
421-
435+
422436
private double convertNtokG(double newtons) {
423437
// weight / accel due to grav = kg
424438
return newtons / 9.81;

0 commit comments

Comments
 (0)