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

Commit 61bfcc5

Browse files
kevinzwangkevinzwang
committed
2 parents 288358d + 255ca24 commit 61bfcc5

6 files changed

Lines changed: 197 additions & 43 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
@@ -118,4 +118,51 @@ public OI() {
118118
toggleRightIntake = new JoystickButton(manipulator, getButton("Toggle Right Intake Button", 4));
119119
toggleRightIntake.whenPressed(new ToggleRightIntake());
120120
}
121+
122+
// /**
123+
// * Returns the getY from leftJoy squared (preserving sign)
124+
// *
125+
// * @return The y value squared
126+
// */
127+
// public double squareLeftY() {
128+
// return leftJoy.getY() * leftJoy.getY() * Math.signum(leftJoy.getY());
129+
// }
130+
//
131+
// /**
132+
// * Returns the getY from rightJoy squared (preserving sign)
133+
// *
134+
// * @return The y value squared
135+
// */
136+
// public double squareRightY() {
137+
// return rightJoy.getY() * rightJoy.getY() * Math.signum(rightJoy.getY());
138+
// }
139+
//
140+
// /**
141+
// * Returns the getX from leftJoy squared (preserving sign)
142+
// *
143+
// * @return The x value squared
144+
// */
145+
// public double squareLeftX() {
146+
// return leftJoy.getX() * leftJoy.getX() * Math.signum(leftJoy.getX());
147+
// }
148+
//
149+
// /**
150+
// * Returns the getX from rightJoy squared (preserving sign)
151+
// *
152+
// * @return The x value squared
153+
// */
154+
// public double squareRightX() {
155+
// return rightJoy.getX() * rightJoy.getX() * Math.signum(rightJoy.getX());
156+
// }
157+
158+
/**
159+
* Used to square joystick values while keeping sign
160+
*
161+
* @param joyVal
162+
* The joystick value to stick
163+
* @return The squared joystick value with same sign
164+
*/
165+
public double squareValueKeepSign(double joyVal) {
166+
return joyVal * joyVal * Math.signum(joyVal);
167+
}
121168
}

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

Lines changed: 30 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -238,13 +238,11 @@ public double calcDefkD(double maxSpeed) {
238238
* be converted from rpm to radians per second, so divide by 60 and multiply to
239239
* get radians.
240240
*/
241-
double gearReduction = Robot.getBool("High Gear", false) ? Robot.getConst("High Gear Gear Reduction", 5.392)
242-
: Robot.getConst("Low Gear Gear Reduction", 12.255);
243-
double radius = Robot.getConst("Radius of Drivetrain Wheel", 0.0635);
244-
double timeConstant = Robot.getConst("Omega Max", 5330) / gearReduction / 60 * 2 * Math.PI
245-
* convertNtokG(Robot.getConst("Weight of Robot", 342)) / 2 * radius * radius
246-
/ (Robot.getConst("Stall Torque", 2.41) * gearReduction * 2);
247-
double cycleTime = Robot.getConst("Code cycle time", 0.05);
241+
double gearReduction = getGearRatio();
242+
double radius = getRadius();
243+
double timeConstant = getOmegaMax() / gearReduction / 60 * 2 * Math.PI * convertNtokG(getWeight()) / 2 * radius
244+
* radius / (getStallTorque() * gearReduction * 2);
245+
double cycleTime = getCycleTime();
248246
/*
249247
* The denominator of kD is 1-(e ^ -cycleTime / timeConstant). The numerator is
250248
* one.
@@ -253,6 +251,31 @@ public double calcDefkD(double maxSpeed) {
253251
return 1 / denominator / maxSpeed;
254252
}
255253

254+
public double getGearRatio() {
255+
return Robot.getBool("High Gear", false) ? Robot.getConst("High Gear Gear Reduction", 5.392)
256+
: Robot.getConst("Low Gear Gear Reduction", 12.255);
257+
}
258+
259+
public double getRadius() {
260+
return Robot.getConst("Radius of Drivetrain Wheel", 0.0635);
261+
}
262+
263+
public double getOmegaMax() {
264+
return Robot.getConst("Omega Max", 5330);
265+
}
266+
267+
public double getWeight() {
268+
return Robot.getConst("Weight of Robot", 342);
269+
}
270+
271+
public double getCycleTime() {
272+
return Robot.getConst("Code cycle time", 0.05);
273+
}
274+
275+
public double getStallTorque() {
276+
return Robot.getConst("Stall Torque", 2.41);
277+
}
278+
256279
private double convertLbsTokG(double lbs) {
257280
// number from google ;)
258281
return lbs * 0.45359237;

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

Lines changed: 31 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -51,7 +51,19 @@ public PIDMove(double targ, DrivetrainInterface dt, SmartDashboardInterface sd,
5151
}
5252
double kf = 1 / (dt.getCurrentMaxSpeed() * sd.getConst("Default PID Update Time", 0.05));
5353
moveController = new PIDController(sd.getConst("MovekP", 0.1), sd.getConst("MovekI", 0),
54-
sd.getConst("MovekD", 0), kf, avg, this);
54+
sd.getConst("MovekD", 0), kf, avg, this) {
55+
/**
56+
* Move Velocity: V = sqrt(8TGd) / (R*m) where T = max torque of wheels G = gear
57+
* ratio d = distance remaining R = radius of wheels m = mass
58+
*/
59+
@Override
60+
protected double calculateFeedForward() {
61+
double originalFF = super.calculateFeedForward();
62+
double feedForwardConst = dt.getPIDMoveConstant();
63+
double error = getError();
64+
return Math.signum(error) * feedForwardConst * Math.sqrt(Math.abs(error)) + originalFF;
65+
}
66+
};
5567
sd.putData("Move PID", moveController);
5668
}
5769

@@ -86,6 +98,21 @@ public PIDMove(double[] point, DrivetrainInterface dt, SmartDashboardInterface s
8698
moveController = new PIDController(sd.getConst("MovekP", 1), sd.getConst("MovekI", 0), sd.getConst("MovekD", 0),
8799
kf, avg, this);
88100
sd.putData("Move PID", moveController);
101+
moveController = new PIDController(sd.getConst("MovekP", 0.1), sd.getConst("MovekI", 0),
102+
sd.getConst("MovekD", 0), kf, avg, this) {
103+
/**
104+
* Move Velocity: V = sqrt(8TGd) / (R*m) where T = max torque of wheels G = gear
105+
* ratio d = distance remaining R = radius of wheels m = mass
106+
*/
107+
@Override
108+
protected double calculateFeedForward() {
109+
double originalFF = super.calculateFeedForward();
110+
double feedForwardConst = dt.getPIDMoveConstant();
111+
double error = getError();
112+
return Math.signum(error) * feedForwardConst * Math.sqrt(Math.abs(error)) + originalFF;
113+
}
114+
};
115+
sd.putData("Move PID", moveController);
89116
}
90117

91118
/**
@@ -112,7 +139,7 @@ public void initialize() {
112139
moveController.setSetpoint(target);
113140

114141
moveController.enable();
115-
// dt.enableVelocityPIDs();
142+
dt.enableVelocityPIDs();
116143
}
117144

118145
/**
@@ -160,6 +187,8 @@ protected void end() {
160187
AutoUtils.state.changeY(y);
161188

162189
AutoUtils.state.setRot(dt.getAHRSAngle());
190+
191+
dt.disableVelocityPIDs();
163192
}
164193

165194
/**

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

Lines changed: 44 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -68,7 +68,21 @@ public PIDTurn(double targ, DrivetrainInterface dt, SmartDashboardInterface sd,
6868
double maxTurnSpeed = dt.getCurrentMaxSpeed() * 360 / (Math.PI * sd.getConst("Distance Between Wheels", 26.25));
6969
double kf = 1 / (maxTurnSpeed * sd.getConst("Default PID Update Time", 0.05));
7070
turnController = new PIDController(sd.getConst("TurnkP", 1), sd.getConst("TurnkI", 0), sd.getConst("TurnkD", 0),
71-
kf, ahrs, this);
71+
kf, ahrs, this) {
72+
/**
73+
* Turn Velocity: V = 4r sqrt((T*G*theta) / (R*m)) where r = half of distance
74+
* between wheels T = max torque of wheels G = gear ratio theta = rotational
75+
* distance to end of turn R = radius of wheels m = mass
76+
*/
77+
@Override
78+
protected double calculateFeedForward() {
79+
double originalFF = super.calculateFeedForward();
80+
double feedForwardConst = dt.getPIDTurnConstant();
81+
double error = getError();
82+
return feedForwardConst * (getDistanceBetweenWheels() / 2) * Math.signum(error)
83+
* Math.sqrt(Math.abs(error)) + originalFF;
84+
}
85+
};
7286
// tim = new Timer();
7387
}
7488

@@ -114,6 +128,7 @@ public PIDTurn(double targ, DrivetrainInterface dt, SmartDashboardInterface sd,
114128
*/
115129
public PIDTurn(double[] point, DrivetrainInterface dt, SmartDashboardInterface sd, PIDSource ahrs) {
116130
this.dt = dt;
131+
this.sd = sd;
117132
this.ahrs = ahrs;
118133
this.sd = sd;
119134

@@ -127,10 +142,24 @@ public PIDTurn(double[] point, DrivetrainInterface dt, SmartDashboardInterface s
127142
}
128143
// calculates the maximum turning speed in degrees/sec based on the max linear
129144
// speed in inches/s and the distance (inches) between sides of the DT
130-
double maxTurnSpeed = dt.getCurrentMaxSpeed() * 360 / (Math.PI * sd.getConst("Distance Between Wheels", 26.25));
145+
double maxTurnSpeed = dt.getCurrentMaxSpeed() * 360 / (Math.PI * getDistanceBetweenWheels());
131146
double kf = 1 / (maxTurnSpeed * sd.getConst("Default PID Update Time", 0.05));
132147
turnController = new PIDController(sd.getConst("TurnkP", 1), sd.getConst("TurnkI", 0), sd.getConst("TurnkD", 0),
133-
kf, ahrs, this);
148+
kf, ahrs, this) {
149+
/**
150+
* Turn Velocity: V = 4r sqrt((T*G*theta) / (R*m)) where r = half of distance
151+
* between wheels T = max torque of wheels G = gear ratio theta = rotational
152+
* distance to end of turn R = radius of wheels m = mass
153+
*/
154+
@Override
155+
protected double calculateFeedForward() {
156+
double originalFF = super.calculateFeedForward();
157+
double feedForwardConst = dt.getPIDTurnConstant();
158+
double error = getError();
159+
return feedForwardConst * (getDistanceBetweenWheels() / 2) * Math.signum(error)
160+
* Math.sqrt(Math.abs(error)) + originalFF;
161+
}
162+
};
134163
// tim = new Timer();
135164
sd.putData("Turn PID", turnController);
136165
}
@@ -162,7 +191,7 @@ protected void initialize() {
162191
System.out.println("Turn to point: " + turnToPoint);
163192

164193
turnController.disable();
165-
// dt.enableVelocityPIDs();
194+
dt.enableVelocityPIDs();
166195
System.out.println("initialize2s");
167196
// dt.resetAHRS();
168197
System.out.println("after reset");
@@ -233,6 +262,8 @@ protected void end() {
233262
// turnController.free();
234263

235264
AutoUtils.state.setRot(dt.getAHRSAngle());
265+
266+
dt.disableVelocityPIDs();
236267
}
237268

238269
/**
@@ -259,4 +290,13 @@ public void pidWrite(double output) {
259290
dt.arcadeDrive(0, output);
260291
SmartDashboard.putNumber("Turn PID Output", output);
261292
}
293+
294+
/**
295+
* Gets the distance between the two middle wheels.
296+
*
297+
* @return that distance
298+
*/
299+
private double getDistanceBetweenWheels() {
300+
return sd.getConst("Distance Between Wheels", 26.25);
301+
}
262302
}

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

Lines changed: 33 additions & 30 deletions
Original file line numberDiff line numberDiff line change
@@ -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, calcDefkD(getCurrentMaxSpeed()));
226-
rightVelocityController.setPID(Robot.getConst("VelocityRightkI", 0), 0, 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

@@ -403,31 +417,20 @@ public void putVelocityControllersToDashboard() {
403417
SmartDashboard.putData("Right PID Controller", rightVelocityController);
404418
}
405419

406-
/**
407-
* Uses SmartDashboard and math to calculate a *great* default kD
408-
*/
409-
public double calcDefkD(double maxSpeed) {
410-
/*
411-
* timeConstant is proportional to max speed of the shaft (which is the max
412-
* speed of the cim divided by the gear reduction), half the mass (because the
413-
* half of the drivetrain only has to support half of the robot), and radius of
414-
* the drivetrain wheels squared. It's inversely proportional to the stall
415-
* torque of the shaft, which is found by multiplying the stall torque of the
416-
* motor with the gear reduction by the amount of motors.
417-
*/
418-
double gearReduction = Robot.getBool("High Gear", false) ? Robot.getConst("High Gear Gear Reduction", 5.392)
419-
: Robot.getConst("Low Gear Gear Reduction", 12.255);
420-
double radius = Robot.getConst("Radius of Drivetrain Wheel", 0.0635);
421-
double timeConstant = Robot.getConst("Omega Max", 5330) / gearReduction / 60 * 2 * Math.PI
422-
* convertNtokG(Robot.getConst("Weight of Robot", 342)) / 2 * radius * radius
423-
/ (Robot.getConst("Stall Torque", 2.41) * gearReduction * 2);
424-
double cycleTime = Robot.getConst("Code cycle time", 0.05);
425-
/*
426-
* The denominator of kD is 1-(e ^ -cycleTime / timeConstant). The numerator is
427-
* one.
428-
*/
429-
double denominator = Math.pow(Math.E, 1 * cycleTime / timeConstant) - 1;
430-
return 1 / denominator / maxSpeed;
420+
public double getPIDMoveConstant() {
421+
double G = Robot.rmap.getGearRatio();
422+
double T = Robot.rmap.getStallTorque();
423+
double R = Robot.rmap.getRadius();
424+
double M = Robot.rmap.getWeight();
425+
return Math.sqrt((8 * T * G) / (R * M));
426+
}
427+
428+
public double getPIDTurnConstant() {
429+
double G = Robot.rmap.getGearRatio();
430+
double T = Robot.rmap.getStallTorque();
431+
double R = Robot.rmap.getRadius();
432+
double M = Robot.rmap.getWeight();
433+
return 4 * Math.sqrt((T * G) / (R * M));
431434
}
432435

433436
private double convertNtokG(double newtons) {

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

Lines changed: 12 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -172,4 +172,16 @@ public interface DrivetrainInterface {
172172
* Put left and right velocity controllers (PID) on SmartDashboard.
173173
*/
174174
public void putVelocityControllersToDashboard();
175+
176+
/**
177+
* Calculates a constant for calculating feed forward in PIDMove
178+
* @return the constant
179+
*/
180+
public double getPIDMoveConstant();
181+
182+
/**
183+
* Calculates a constant for calculating feed forward in PIDTurn
184+
* @return the constant
185+
*/
186+
public double getPIDTurnConstant();
175187
}

0 commit comments

Comments
 (0)