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

Commit 4c38478

Browse files
StudentStudent
Student
authored and
Student
committed
testing move and turn pids, new navx jar
all vpid commentewd out move pid mostly tuned turn very unreliable still having initialize issues
1 parent f8b8af7 commit 4c38478

11 files changed

Lines changed: 285 additions & 77 deletions

File tree

‎Robot2018/.classpath‎

Lines changed: 0 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -16,8 +16,5 @@
1616
<classpathentry kind="lib" path="lib/CTRE_Phoenix-sources.jar"/>
1717
<classpathentry kind="lib" path="lib/CTRE_Phoenix.jar"/>
1818
<classpathentry kind="lib" path="lib/navx_frc.jar"/>
19-
<classpathentry kind="var" path="USERLIBS_DIR/CTRE_Phoenix-sources.jar"/>
20-
<classpathentry kind="var" path="USERLIBS_DIR/CTRE_Phoenix.jar"/>
21-
<classpathentry kind="var" path="USERLIBS_DIR/navx_frc.jar"/>
2219
<classpathentry kind="output" path="bin"/>
2320
</classpath>

‎Robot2018/lib/.gitignore‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1 @@
1+
/User_Libraries.properties

‎Robot2018/lib/navx_frc.jar‎

231 Bytes
Binary file not shown.

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

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -55,9 +55,11 @@ public OI() {
5555
PIDMoveButton = new JoystickButton(leftJoy, getButton("PID Move", 7));
5656
PIDMoveButton.whenPressed(new PIDMove(Robot.getConst("Move Targ", 24), Robot.dt, RobotMap.distEncAvg));
5757
PIDTurnButton = new JoystickButton(leftJoy, getButton("PID Turn", 8));
58-
PIDTurnButton.whenPressed(new PIDTurn(30, Robot.dt, RobotMap.fancyGyro));
58+
// PIDTurnButton.whenPressed(new PIDTurn(Robot.getConst("Turn Targ", 90),
59+
// Robot.dt, RobotMap.fancyGyro));
5960
resetEncButton = new JoystickButton(leftJoy, getButton("Reset Dist Enc", 10));
6061
resetEncButton.whenPressed(new ResetEncoders());
62+
PIDTurnButton.whenReleased(new PIDTurn(Robot.getConst("Turn Targ", 90), Robot.dt, RobotMap.fancyGyro));
6163

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

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

Lines changed: 35 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -23,8 +23,8 @@
2323
import org.usfirst.frc.team199.Robot2018.subsystems.Lift;
2424

2525
import edu.wpi.first.wpilibj.DriverStation;
26+
import edu.wpi.first.wpilibj.IterativeRobot;
2627
import edu.wpi.first.wpilibj.Preferences;
27-
import edu.wpi.first.wpilibj.TimedRobot;
2828
import edu.wpi.first.wpilibj.command.Command;
2929
import edu.wpi.first.wpilibj.command.Scheduler;
3030
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
@@ -37,7 +37,7 @@
3737
* creating this project, you must also update the manifest file in the resource
3838
* directory.
3939
*/
40-
public class Robot extends TimedRobot {
40+
public class Robot extends IterativeRobot {
4141

4242
public static Climber climber;
4343
public static ClimberAssist climberAssist;
@@ -57,23 +57,34 @@ public class Robot extends TimedRobot {
5757
String[] fmsPossibilities = { "LL", "LR", "RL", "RR" };
5858

5959
public static double getConst(String key, double def) {
60-
if (!SmartDashboard.containsKey("Const/" + key)) {
61-
if (!SmartDashboard.putNumber("Const/" + key, def)) {
62-
System.err.println("SmartDashboard Key" + "Const/" + key + "already taken by a different type");
60+
Preferences pref = Preferences.getInstance();
61+
if (!pref.containsKey("Const/" + key)) {
62+
pref.putDouble("Const/" + key, def);
63+
if (pref.getDouble("Const/ + key", def) != def) {
64+
System.err.println("pref Key" + "Const/" + key + "already taken by a different type");
6365
return def;
6466
}
6567
}
66-
return SmartDashboard.getNumber("Const/" + key, def);
68+
return pref.getDouble("Const/" + key, def);
69+
/*
70+
* if (!SmartDashboard.containsKey("Const/" + key)) { if
71+
* (!SmartDashboard.putNumber("Const/" + key, def)) {
72+
* System.err.println("SmartDashboard Key" + "Const/" + key +
73+
* "already taken by a different type"); return def; } } return
74+
* SmartDashboard.getNumber("Const/" + key, def);
75+
*/
6776
}
6877

6978
public static boolean getBool(String key, boolean def) {
70-
if (!SmartDashboard.containsKey("Bool/" + key)) {
71-
if (!SmartDashboard.putBoolean("Bool/" + key, def)) {
72-
System.err.println("SmartDashboard Key" + "Bool/" + key + "already taken by a different type");
79+
Preferences pref = Preferences.getInstance();
80+
if (!pref.containsKey("Bool/" + key)) {
81+
pref.putBoolean("Bool/" + key, def);
82+
if (pref.getBoolean("Bool/" + key, def) == def) {
83+
System.err.println("pref Key" + "Bool/" + key + "already taken by a different type");
7384
return def;
7485
}
7586
}
76-
return SmartDashboard.getBoolean("Bool/" + key, def);
87+
return pref.getBoolean("Bool/" + key, def);
7788
}
7889

7990
/**
@@ -179,16 +190,20 @@ public void teleopInit() {
179190
public void teleopPeriodic() {
180191
Scheduler.getInstance().run();
181192

182-
SmartDashboard.putNumber("Drivetrain/Left VPID Targ", Robot.dt.getLeftVPIDSetpoint());
183-
SmartDashboard.putNumber("Drivetrain/Right VPID Targ", Robot.dt.getRightVPIDSetpoint());
184-
SmartDashboard.putNumber("Left VPID Error", Robot.dt.getLeftVPIDerror());
185-
SmartDashboard.putNumber("Right VPID Error", Robot.dt.getRightVPIDerror());
186-
SmartDashboard.putNumber("Left Enc Rate", Robot.dt.getLeftEncRate());
187-
SmartDashboard.putNumber("Right Enc Rate", Robot.dt.getRightEncRate());
188-
189-
SmartDashboard.putNumber("Left Enc Dist", dt.getLeftDist());
190-
SmartDashboard.putNumber("Right Enc Dist", dt.getRightDist());
191-
SmartDashboard.putNumber("Avg Enc Dist", dt.getEncAvgDist());
193+
// SmartDashboard.putNumber("Drivetrain/Left VPID Targ",
194+
// Robot.dt.getLeftVPIDSetpoint());
195+
// SmartDashboard.putNumber("Drivetrain/Right VPID Targ",
196+
// Robot.dt.getRightVPIDSetpoint());
197+
// SmartDashboard.putNumber("Left VPID Error", Robot.dt.getLeftVPIDerror());
198+
// SmartDashboard.putNumber("Right VPID Error", Robot.dt.getRightVPIDerror());
199+
// SmartDashboard.putNumber("Left Enc Rate", Robot.dt.getLeftEncRate());
200+
// SmartDashboard.putNumber("Right Enc Rate", Robot.dt.getRightEncRate());
201+
//
202+
// SmartDashboard.putNumber("Left Enc Dist", dt.getLeftDist());
203+
// SmartDashboard.putNumber("Right Enc Dist", dt.getRightDist());
204+
// SmartDashboard.putNumber("Avg Enc Dist", dt.getEncAvgDist());
205+
//
206+
// SmartDashboard.putNumber("Angle", dt.getAHRSAngle());
192207
}
193208

194209
boolean firstTime = true;

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

Lines changed: 6 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -19,7 +19,7 @@
1919
import edu.wpi.first.wpilibj.DoubleSolenoid;
2020
import edu.wpi.first.wpilibj.Encoder;
2121
import edu.wpi.first.wpilibj.PIDSourceType;
22-
import edu.wpi.first.wpilibj.SerialPort;
22+
import edu.wpi.first.wpilibj.SPI;
2323
import edu.wpi.first.wpilibj.SpeedControllerGroup;
2424
import edu.wpi.first.wpilibj.VictorSP;
2525
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
@@ -171,12 +171,13 @@ public RobotMap() {
171171
rightVelocityController.setContinuous(false);
172172
rightVelocityController.setAbsoluteTolerance(Robot.getConst("VelocityToleranceRight", 2));
173173

174-
robotDrive = new DifferentialDrive(leftVelocityController, rightVelocityController);
175-
robotDrive.setMaxOutput(Robot.getConst("Max High Speed", 204));
176-
// robotDrive = new DifferentialDrive(dtLeft, dtRight);
174+
// robotDrive = new DifferentialDrive(leftVelocityController,
175+
// rightVelocityController);
176+
// robotDrive.setMaxOutput(Robot.getConst("Max High Speed", 204));
177+
robotDrive = new DifferentialDrive(dtLeft, dtRight);
177178

178179
distEncAvg = new PIDSourceAverage(leftEncDist, rightEncDist);
179-
fancyGyro = new AHRS(SerialPort.Port.kMXP);
180+
fancyGyro = new AHRS(SPI.Port.kMXP);
180181
dtGear = new DoubleSolenoid(getPort("1dtGearSolenoid", 0), getPort("2dtGearSolenoid", 1));
181182
}
182183

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

Lines changed: 7 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -18,6 +18,7 @@ public class PIDMove extends Command implements PIDOutput {
1818
private double target;
1919
private DrivetrainInterface dt;
2020
private PIDController moveController;
21+
private PIDSourceAverage avg;
2122

2223
/**
2324
* Constructs this command with a new PIDController. Sets all of the
@@ -38,6 +39,7 @@ public PIDMove(double targ, DrivetrainInterface dt, PIDSourceAverage avg) {
3839
requires(Robot.dt);
3940
target = targ;
4041
this.dt = dt;
42+
this.avg = avg;
4143
double kf = 1 / (dt.getCurrentMaxSpeed() * Robot.getConst("Default PID Update Time", 0.05));
4244
moveController = new PIDController(Robot.getConst("MovekP", 0), Robot.getConst("MovekI", 0),
4345
Robot.getConst("MovekD", 0), kf, avg, this);
@@ -56,13 +58,13 @@ public void initialize() {
5658
// output in "motor units" (arcade and tank only accept values [-1, 1]
5759
moveController.setOutputRange(-1.0, 1.0);
5860
moveController.setContinuous(false);
59-
moveController.setAbsoluteTolerance(Robot.getConst("MoveTolerance", 2));
60-
moveController.setSetpoint(target);
61+
moveController.setAbsoluteTolerance(Robot.getConst("MoveTolerance", 0.1));
62+
moveController.setSetpoint(Robot.getConst("Move Targ", 24));
6163

6264
SmartDashboard.putData("Move PID", moveController);
6365

6466
moveController.enable();
65-
dt.enableVelocityPIDs();
67+
// dt.enableVelocityPIDs();
6668
}
6769

6870
/**
@@ -72,6 +74,7 @@ public void initialize() {
7274
*/
7375
@Override
7476
protected void execute() {
77+
System.out.println("Enc Avg Dist: " + avg.pidGet());
7578
SmartDashboard.putNumber("Move PID Result", moveController.get());
7679
SmartDashboard.putNumber("Move PID Error", moveController.getError());
7780
}
@@ -95,6 +98,7 @@ protected boolean isFinished() {
9598
@Override
9699
protected void end() {
97100
moveController.disable();
101+
System.out.println("End");
98102
// moveController.free();
99103
}
100104

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

Lines changed: 36 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -7,6 +7,7 @@
77

88
import edu.wpi.first.wpilibj.PIDController;
99
import edu.wpi.first.wpilibj.PIDOutput;
10+
import edu.wpi.first.wpilibj.Timer;
1011
import edu.wpi.first.wpilibj.command.Command;
1112
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
1213

@@ -19,6 +20,9 @@ public class PIDTurn extends Command implements PIDOutput {
1920
private double target;
2021
private DrivetrainInterface dt;
2122
private PIDController turnController;
23+
private AHRS ahrs;
24+
private Timer tim;
25+
private double lastTime;
2226

2327
/**
2428
* Constructs this command with a new PIDController. Sets all of the
@@ -40,13 +44,15 @@ public PIDTurn(double targ, DrivetrainInterface dt, AHRS ahrs) {
4044
requires(Robot.dt);
4145
target = targ;
4246
this.dt = dt;
47+
this.ahrs = ahrs;
4348
// calculates the maximum turning speed in degrees/sec based on the max linear
4449
// speed in inches/s and the distance (inches) between sides of the DT
4550
double maxTurnSpeed = dt.getCurrentMaxSpeed() * 360
4651
/ (Math.PI * Robot.getConst("Distance Between Wheels", 26.25));
4752
double kf = 1 / (maxTurnSpeed * Robot.getConst("Default PID Update Time", 0.05));
48-
turnController = new PIDController(Robot.getConst("TurnkP", 1), Robot.getConst("TurnkI", 0),
49-
Robot.getConst("TurnkD", 0), kf, ahrs, this);
53+
turnController = new PIDController(Robot.getConst("TurnkP", 0.018), Robot.getConst("TurnkI", 0.00003),
54+
Robot.getConst("TurnkD", 0.015), kf, ahrs, this);
55+
// tim = new Timer();
5056
}
5157

5258
/**
@@ -55,18 +61,30 @@ public PIDTurn(double targ, DrivetrainInterface dt, AHRS ahrs) {
5561
*/
5662
@Override
5763
protected void initialize() {
58-
dt.resetAHRS();
5964
turnController.disable();
65+
// dt.enableVelocityPIDs();
66+
System.out.println("initialize2s");
67+
dt.resetAHRS();
68+
System.out.println("after reset");
69+
System.out.println("after disabling");
6070
// input is in degrees
6171
turnController.setInputRange(-180, 180);
6272
// output in "motor units" (arcade and tank only accept values [-1, 1]
6373
turnController.setOutputRange(-1.0, 1.0);
64-
turnController.setContinuous();
74+
turnController.setContinuous(true);
6575
turnController.setAbsoluteTolerance(Robot.getConst("TurnTolerance", 1));
66-
turnController.setSetpoint(target);
76+
double newSetPoint = Robot.getConst("Turn Targ", 90);
77+
while (Math.abs(newSetPoint) > 180) {
78+
newSetPoint = newSetPoint - Math.signum(newSetPoint) * 360;
79+
}
80+
turnController.setSetpoint(newSetPoint);
81+
6782
SmartDashboard.putData("Turn PID", turnController);
83+
6884
turnController.enable();
69-
dt.enableVelocityPIDs();
85+
System.out.println("initialize finished");
86+
// tim.start();
87+
// lastTime = tim.get();
7088
}
7189

7290
/**
@@ -76,8 +94,12 @@ protected void initialize() {
7694
*/
7795
@Override
7896
protected void execute() {
79-
SmartDashboard.putNumber("Turn PID Result", turnController.get());
80-
SmartDashboard.putNumber("Turn PID Error", turnController.getError());
97+
System.out.println("execute");
98+
System.out.println("Angle: " + dt.getAHRSAngle());
99+
// if (tim.get() > lastTime + Robot.getConst("Update Time", 1)) {
100+
// SmartDashboard.putNumber("Angle", dt.getAHRSAngle());
101+
// lastTime = tim.get();
102+
// }
81103
}
82104

83105
/**
@@ -89,7 +111,8 @@ protected void execute() {
89111
*/
90112
@Override
91113
protected boolean isFinished() {
92-
return turnController.onTarget();
114+
System.out.println("isFinished");
115+
return (turnController.onTarget() && Math.abs(ahrs.getRate()) < 1);
93116
}
94117

95118
/**
@@ -100,6 +123,9 @@ protected boolean isFinished() {
100123
@Override
101124
protected void end() {
102125
turnController.disable();
126+
System.out.println("end");
127+
SmartDashboard.putNumber("Turn PID Result", turnController.get());
128+
SmartDashboard.putNumber("Turn PID Error", turnController.getError());
103129
// turnController.free();
104130
}
105131

@@ -125,5 +151,6 @@ protected void interrupted() {
125151
@Override
126152
public void pidWrite(double output) {
127153
dt.arcadeDrive(0, output);
154+
SmartDashboard.putNumber("Turn PID Output", output);
128155
}
129156
}

‎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: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -251,7 +251,7 @@ public void resetAHRS() {
251251
*/
252252
@Override
253253
public double getAHRSAngle() {
254-
return fancyGyro.getAngle();
254+
return fancyGyro.getYaw();
255255
}
256256

257257
/**

0 commit comments

Comments
 (0)