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

Commit 9a97982

Browse files
DriverStationComputerDriverStationComputer
committed
2 parents 17c2c71 + 06e33e2 commit 9a97982

10 files changed

Lines changed: 281 additions & 71 deletions

File tree

‎Robot2018/.project‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,6 @@
11
<?xml version="1.0" encoding="UTF-8"?>
22
<projectDescription>
3-
<name>DSRobot2018</name>
3+
<name>Robot2018</name>
44
<comment></comment>
55
<projects>
66
</projects>

‎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
@@ -24,8 +24,8 @@
2424

2525
import edu.wpi.first.wpilibj.CameraServer;
2626
import edu.wpi.first.wpilibj.DriverStation;
27+
import edu.wpi.first.wpilibj.IterativeRobot;
2728
import edu.wpi.first.wpilibj.Preferences;
28-
import edu.wpi.first.wpilibj.TimedRobot;
2929
import edu.wpi.first.wpilibj.command.Command;
3030
import edu.wpi.first.wpilibj.command.Scheduler;
3131
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
@@ -38,7 +38,7 @@
3838
* creating this project, you must also update the manifest file in the resource
3939
* directory.
4040
*/
41-
public class Robot extends TimedRobot {
41+
public class Robot extends IterativeRobot {
4242

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

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

7079
public static boolean getBool(String key, boolean def) {
71-
if (!SmartDashboard.containsKey("Bool/" + key)) {
72-
if (!SmartDashboard.putBoolean("Bool/" + key, def)) {
73-
System.err.println("SmartDashboard Key" + "Bool/" + key + "already taken by a different type");
80+
Preferences pref = Preferences.getInstance();
81+
if (!pref.containsKey("Bool/" + key)) {
82+
pref.putBoolean("Bool/" + key, def);
83+
if (pref.getBoolean("Bool/" + key, def) == def) {
84+
System.err.println("pref Key" + "Bool/" + key + "already taken by a different type");
7485
return def;
7586
}
7687
}
77-
return SmartDashboard.getBoolean("Bool/" + key, def);
88+
return pref.getBoolean("Bool/" + key, def);
7889
}
7990

8091
/**
@@ -182,16 +193,20 @@ public void teleopInit() {
182193
public void teleopPeriodic() {
183194
Scheduler.getInstance().run();
184195

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

197212
boolean firstTime = true;

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

Lines changed: 2 additions & 2 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;
@@ -177,7 +177,7 @@ public RobotMap() {
177177
robotDrive = new DifferentialDrive(dtLeft, dtRight);
178178

179179
distEncAvg = new PIDSourceAverage(leftEncDist, rightEncDist);
180-
fancyGyro = new AHRS(SerialPort.Port.kMXP);
180+
fancyGyro = new AHRS(SPI.Port.kMXP);
181181
dtGear = new DoubleSolenoid(getPort("1dtGearSolenoid", 0), getPort("2dtGearSolenoid", 1));
182182
}
183183

‎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/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)