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

Commit d016096

Browse files
DriverStationComputerDriverStationComputer
committed
Multiple changes
slow movement on trigger hold, add putConstant for preferences, FindTurnTimeConstant command to run experiment to find turn PID time constant (not done), maybe more.
1 parent f07bf9a commit d016096

File tree

8 files changed

+181
-57
lines changed

8 files changed

+181
-57
lines changed

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

Lines changed: 9 additions & 2 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.commands.CloseIntake;
11+
import org.usfirst.frc.team199.Robot2018.commands.FindTurnTimeConstant;
1112
import org.usfirst.frc.team199.Robot2018.commands.IntakeCube;
1213
import org.usfirst.frc.team199.Robot2018.commands.OpenIntake;
1314
import org.usfirst.frc.team199.Robot2018.commands.OutakeCube;
@@ -38,6 +39,7 @@ public class OI {
3839
*/
3940

4041
public Joystick leftJoy;
42+
public Joystick rightJoy;
4143
private JoystickButton shiftLowGearButton;
4244
private JoystickButton shiftHighGearButton;
4345
private JoystickButton shiftDriveTypeButton;
@@ -46,9 +48,10 @@ public class OI {
4648
private JoystickButton resetEncButton;
4749
private JoystickButton MoveLiftUpButton;
4850
private JoystickButton MoveLiftDownButton;
49-
public Joystick rightJoy;
51+
private JoystickButton findTurnTimeConstantButton;
5052
private JoystickButton updatePIDConstantsButton;
5153
private JoystickButton updateEncoderDPPButton;
54+
5255
public Joystick manipulator;
5356
private JoystickButton closeIntake;
5457
private JoystickButton openIntake;
@@ -69,7 +72,7 @@ public int getButton(String key, int def) {
6972
return (int) SmartDashboard.getNumber("Button/" + key, def);
7073
}
7174

72-
public OI() {
75+
public OI(Robot robot) {
7376
leftJoy = new Joystick(0);
7477
shiftDriveTypeButton = new JoystickButton(leftJoy, getButton("Shift Drive Type", 2));
7578
shiftDriveTypeButton.whenPressed(new ShiftDriveType());
@@ -83,6 +86,10 @@ public OI() {
8386
.whenReleased(new PIDTurn(Robot.getConst("Turn Targ", 90), Robot.dt, Robot.sd, RobotMap.fancyGyro));
8487
resetEncButton = new JoystickButton(leftJoy, getButton("Reset Dist Enc", 10));
8588
resetEncButton.whenPressed(new ResetEncoders());
89+
findTurnTimeConstantButton = new JoystickButton(leftJoy, getButton("Find Turn Time Constant", 11));
90+
// the command will only run in test mode
91+
findTurnTimeConstantButton
92+
.whenPressed(new FindTurnTimeConstant(robot, Robot.dt, Robot.rmap.fancyGyro, Robot.sd));
8693

8794
rightJoy = new Joystick(1);
8895
shiftHighGearButton = new JoystickButton(rightJoy, getButton("Shift High Gear", 3));

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

Lines changed: 29 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -71,6 +71,14 @@ public double getConst(String key, double def) {
7171
return pref.getDouble("Const/" + key, def);
7272
}
7373

74+
public void putConst(String key, double def) {
75+
Preferences pref = Preferences.getInstance();
76+
pref.putDouble("Const/" + key, def);
77+
if (pref.getDouble("Const/ + key", def) != def) {
78+
System.err.println("pref Key" + "Const/" + key + "already taken by a different type");
79+
}
80+
}
81+
7482
public void putData(String string, PIDController controller) {
7583
SmartDashboard.putData(string, controller);
7684
}
@@ -96,6 +104,10 @@ public static double getConst(String key, double def) {
96104
return sd.getConst(key, def);
97105
}
98106

107+
public static void putConst(String key, double def) {
108+
sd.putConst(key, def);
109+
}
110+
99111
public static boolean getBool(String key, boolean def) {
100112
Preferences pref = Preferences.getInstance();
101113
if (!pref.containsKey("Bool/" + key)) {
@@ -120,7 +132,7 @@ public void robotInit() {
120132
intakeEject = new IntakeEject();
121133
lift = new Lift();
122134
dt = new Drivetrain(sd);
123-
oi = new OI();
135+
oi = new OI(this);
124136

125137
// auto position chooser
126138
for (Autonomous.Position p : Autonomous.Position.values()) {
@@ -242,8 +254,11 @@ public void teleopPeriodic() {
242254

243255
boolean firstTime = true;
244256

257+
@Override
245258
public void testInit() {
246-
dt.enableVelocityPIDs();
259+
System.out.println("In testInit()");
260+
dt.resetAHRS();
261+
AutoUtils.state = new State(0, 0, 0);
247262
}
248263

249264
/**
@@ -263,21 +278,21 @@ public void testPeriodic() {
263278
// getConst("VelocityRightkD", rmap.calcDefkD(dt.getCurrentMaxSpeed())),
264279
// /* 1 / dt.getCurrentMaxSpeed() */Robot.getConst("VelocityRightkF",
265280
// 1 / Robot.getConst("Max Low Speed", 84)));
266-
dt.resetAllVelocityPIDConsts();
281+
// dt.resetAllVelocityPIDConsts();
267282

268-
dt.setVPIDs(getConst("VPID Test Set", 0.5));
283+
// dt.setVPIDs(getConst("VPID Test Set", 0.5));
269284

270-
// Scheduler.getInstance().run();
271-
272-
System.out.println("Left VPID Targ: " + dt.getLeftVPIDOutput());
273-
System.out.println("Right VPID Targ: " + dt.getRightVPIDOutput());
274-
System.out.println("Left VPID Error: " + dt.getLeftVPIDerror());
275-
System.out.println("Right VPID Error: " + dt.getRightVPIDerror());
276-
System.out.println("Left Enc Rate: " + dt.getLeftEncRate());
277-
System.out.println("Right Enc Rate: " + dt.getRightEncRate());
285+
Scheduler.getInstance().run();
278286

279-
System.out.println("Left Talon Speed: " + rmap.dtLeftMaster.get());
280-
System.out.println("Right Talon Speed: " + rmap.dtRightMaster.get());
287+
// System.out.println("Left VPID Targ: " + dt.getLeftVPIDOutput());
288+
// System.out.println("Right VPID Targ: " + dt.getRightVPIDOutput());
289+
// System.out.println("Left VPID Error: " + dt.getLeftVPIDerror());
290+
// System.out.println("Right VPID Error: " + dt.getRightVPIDerror());
291+
// System.out.println("Left Enc Rate: " + dt.getLeftEncRate());
292+
// System.out.println("Right Enc Rate: " + dt.getRightEncRate());
293+
//
294+
// System.out.println("Left Talon Speed: " + rmap.dtLeftMaster.get());
295+
// System.out.println("Right Talon Speed: " + rmap.dtRightMaster.get());
281296

282297
// System.out.println("Left Enc Dist " + dt.getLeftDist());
283298
// System.out.println("Right Enc Dist " + dt.getRightDist());

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

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -5,6 +5,8 @@
55
public interface SmartDashboardInterface {
66
public double getConst(String key, double def);
77

8+
public void putConst(String key, double def);
9+
810
public void putData(String string, PIDController controller);
911

1012
public void putNumber(String string, double d);

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

Lines changed: 113 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,113 @@
1+
package org.usfirst.frc.team199.Robot2018.commands;
2+
3+
import java.util.ArrayList;
4+
5+
import org.usfirst.frc.team199.Robot2018.Robot;
6+
import org.usfirst.frc.team199.Robot2018.SmartDashboardInterface;
7+
import org.usfirst.frc.team199.Robot2018.subsystems.DrivetrainInterface;
8+
9+
import com.kauailabs.navx.frc.AHRS;
10+
11+
import edu.wpi.first.wpilibj.Timer;
12+
import edu.wpi.first.wpilibj.command.Command;
13+
14+
/**
15+
* Do an experiment to calculate the time constant for PIDTurn, and place value
16+
* in preferences as "TurnTimeConstant". Only runs if we're in testing mode.
17+
*/
18+
public class FindTurnTimeConstant extends Command {
19+
private DrivetrainInterface dt;
20+
private SmartDashboardInterface sd;
21+
private AHRS gyro;
22+
private Robot robot;
23+
private Timer tim = new Timer();
24+
private ArrayList<DataPoint> dataPoints = new ArrayList<DataPoint>();
25+
26+
private class DataPoint {
27+
public double seconds;
28+
public double degreesPerSecond;
29+
30+
public DataPoint(double seconds, double degreesPerSecond) {
31+
this.seconds = seconds;
32+
this.degreesPerSecond = degreesPerSecond;
33+
}
34+
35+
public String toString() {
36+
return "Time: " + seconds + " seconds; rotational velocity: " + degreesPerSecond + " degrees per second";
37+
}
38+
}
39+
40+
/**
41+
* Constructs this command.
42+
*/
43+
public FindTurnTimeConstant(Robot robot, DrivetrainInterface dt, AHRS gyro, SmartDashboardInterface sd) {
44+
if (Robot.dt != null) {
45+
requires(Robot.dt);
46+
}
47+
this.robot = robot;
48+
this.dt = dt;
49+
this.gyro = gyro;
50+
this.sd = sd;
51+
}
52+
53+
// Called just before this Command runs the first time
54+
protected void initialize() {
55+
System.out.println("FindTurnTimeConstant is in init()");
56+
if (!robot.isTest()) {
57+
System.out.println("but you're not in test mode.");
58+
return;
59+
}
60+
61+
System.out.println("and you're in test mode");
62+
tim.start();
63+
// spin at full speed
64+
dt.arcadeDrive(0, 1);
65+
}
66+
67+
// Called repeatedly when this Command is scheduled to run
68+
protected void execute() {
69+
// add a new data point with the current time and rotational velocity
70+
DataPoint newMeasurement = new DataPoint(tim.get(), gyro.getRate());
71+
System.out.println(newMeasurement);
72+
dataPoints.add(newMeasurement);
73+
}
74+
75+
// Make this return true when this Command no longer needs to run execute()
76+
protected boolean isFinished() {
77+
int numberOfDataPoints = dataPoints.size();
78+
// true if the last and second-to-last rotational velocities measured are within
79+
// 5 degrees per second squared of each other. or if it's not a test, finish
80+
// immediately
81+
if (numberOfDataPoints < 2) {
82+
return false;
83+
}
84+
return (Math.abs(dataPoints.get(numberOfDataPoints - 1).degreesPerSecond
85+
- dataPoints.get(numberOfDataPoints - 2).degreesPerSecond) < 5) || !robot.isTest();
86+
}
87+
88+
// Called once after isFinished returns true
89+
protected void end() {
90+
// stop moving
91+
dt.arcadeDrive(0, 0);
92+
93+
DataPoint lastDataPoint = dataPoints.get(dataPoints.size() - 1);
94+
System.out.println("Finished when " + lastDataPoint);
95+
// the rotvel of the data point we're looking for to find the time constant
96+
double magicValue = lastDataPoint.degreesPerSecond * (1 - (1 / Math.E));
97+
98+
for (DataPoint datum : dataPoints) {
99+
// if we've past the magic value, we're done
100+
if (Math.abs(datum.degreesPerSecond) > Math.abs(magicValue)) {
101+
System.out.println("The magic value is " + datum);
102+
Robot.putConst("TurnTimeConstant", datum.seconds);
103+
// stop checking data
104+
break;
105+
}
106+
}
107+
}
108+
109+
// Called when another command which requires one or more of the same
110+
// subsystems is scheduled to run
111+
protected void interrupted() {
112+
}
113+
}

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

Lines changed: 0 additions & 21 deletions
Original file line numberDiff line numberDiff line change
@@ -70,27 +70,6 @@ public PIDMove(double target, double[] point, DrivetrainInterface dt, SmartDashb
7070
double kF = 1 / (maxSpeed * sd.getConst("Default PID Update Time", 0.05)) / maxSpeed;
7171

7272
moveController = new PIDController(kP, kI, kD, kF, avg, this);
73-
// {
74-
/**
75-
* Move Velocity: V = sqrt(8TGd) / (R*m) where T = max torque of wheels G = gear
76-
* ratio d = distance remaining R = radius of wheels m = mass
77-
*/
78-
// @Override
79-
// protected double calculateFeedForward() {
80-
// double originalFF = super.calculateFeedForward();
81-
// double feedForwardConst = dt.getPIDMoveConstant();
82-
// double error = getError();
83-
// return (Math.signum(error) * feedForwardConst * Math.sqrt(Math.abs(error)) +
84-
// originalFF)
85-
// / dt.getCurrentMaxSpeed();
86-
// }
87-
88-
// @Override
89-
// protected double getContinuousError(double error) {
90-
// return Math.signum(error) *
91-
// Math.sqrt(Math.abs(super.getContinuousError(error)));
92-
// }
93-
// };
9473
sd.putData("Move PID", moveController);
9574
}
9675

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

Lines changed: 6 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -71,23 +71,12 @@ public PIDTurn(double target, double[] point, DrivetrainInterface dt, SmartDashb
7171
// calculates the maximum turning speed in degrees/sec based on the max linear
7272
// speed in inches/s and the distance (inches) between sides of the DT
7373
double maxTurnSpeed = dt.getCurrentMaxSpeed() * 360 / (Math.PI * getDistanceBetweenWheels());
74-
double kf = 1 / (maxTurnSpeed * sd.getConst("Default PID Update Time", 0.05));
75-
turnController = new PIDController(sd.getConst("TurnkP", 1), sd.getConst("TurnkI", 0), sd.getConst("TurnkD", 0),
76-
kf, ahrs, this) {
77-
/**
78-
* Turn Velocity: V = 4r sqrt((T*G*theta) / (R*m)) where r = half of distance
79-
* between wheels T = max torque of wheels G = gear ratio theta = rotational
80-
* distance to end of turn R = radius of wheels m = mass
81-
*/
82-
@Override
83-
protected double calculateFeedForward() {
84-
double originalFF = super.calculateFeedForward();
85-
double feedForwardConst = dt.getPIDTurnConstant();
86-
double error = getError();
87-
return feedForwardConst * (getDistanceBetweenWheels() / 2) * Math.signum(error)
88-
* Math.sqrt(Math.abs(error)) + originalFF;
89-
}
90-
};
74+
double r = Robot.getConst("TurnPidR", 3.0);
75+
double kP = r / Robot.rmap.getDrivetrainTimeConstant() / maxTurnSpeed;
76+
double kI = 0;
77+
double kD = r / maxTurnSpeed;
78+
double kF = 1 / (maxTurnSpeed * sd.getConst("Default PID Update Time", 0.05));
79+
turnController = new PIDController(kP, kI, kD, kF, ahrs, this);
9180
// tim = new Timer();
9281
sd.putData("Turn PID", turnController);
9382
}

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

Lines changed: 19 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -244,6 +244,21 @@ public void teleopDrive() {
244244
*/
245245
@Override
246246
public void arcadeDrive(double speed, double turn) {
247+
if (Robot.getBool("Arcade Drive Default Setup", true)) {
248+
if (Robot.oi.leftJoy.getRawButton(1)) {
249+
speed *= Robot.getConst("Speed Slow Ratio", 0.5);
250+
}
251+
if (Robot.oi.rightJoy.getRawButton(1)) {
252+
turn *= Robot.getConst("Turn Slow Ratio", 0.5);
253+
}
254+
} else {
255+
if (Robot.oi.rightJoy.getRawButton(1)) {
256+
speed *= Robot.getConst("Speed Slow Ratio", 0.5);
257+
}
258+
if (Robot.oi.leftJoy.getRawButton(1)) {
259+
turn *= Robot.getConst("Turn Slow Ratio", 0.5);
260+
}
261+
}
247262
robotDrive.arcadeDrive(speed, turn, Robot.getBool("Square Drive Values", false));
248263
}
249264

@@ -257,6 +272,10 @@ public void arcadeDrive(double speed, double turn) {
257272
*/
258273
@Override
259274
public void tankDrive(double leftSpeed, double rightSpeed) {
275+
if (Robot.oi.leftJoy.getRawButton(1) || Robot.oi.rightJoy.getRawButton(1)) {
276+
leftSpeed *= Robot.getConst("Speed Slow Ratio", 0.5);
277+
rightSpeed *= Robot.getConst("Speed Slow Ratio", 0.5);
278+
}
260279
robotDrive.tankDrive(leftSpeed, rightSpeed, Robot.getBool("Square Drive Values", false));
261280
}
262281

‎docs/controllers.txt‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -26,7 +26,7 @@ controller indices
2626
fake news
2727
left joystick
2828
buttons
29-
1
29+
1 Slow forward-backward movement in arcade, or all movement in tank
3030
2
3131
3
3232
4
@@ -36,14 +36,14 @@ controller indices
3636
8
3737
9
3838
10
39-
11
39+
11 Run expirement to find turning time constant (FindTurnTimeConstant)
4040
axes
4141
0 X axis
4242
1 Y axis (forward is negative)
4343
2 Z axis (knob at base)
4444
right joystick
4545
buttons
46-
1
46+
1 Slow turning movement in arcade, or all movement in tank
4747
2
4848
3
4949
4

0 commit comments

Comments
 (0)