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

Commit da6d0f0

Browse files
Merge branch 'kevins-auto'
2 parents eed3d76 + edf9025 commit da6d0f0

4 files changed

Lines changed: 27 additions & 8 deletions

File tree

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

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -25,6 +25,7 @@
2525
import edu.wpi.first.wpilibj.CameraServer;
2626
import edu.wpi.first.wpilibj.DriverStation;
2727
import edu.wpi.first.wpilibj.IterativeRobot;
28+
import edu.wpi.first.wpilibj.PIDController;
2829
import edu.wpi.first.wpilibj.Preferences;
2930
import edu.wpi.first.wpilibj.command.Command;
3031
import edu.wpi.first.wpilibj.command.Scheduler;
@@ -69,6 +70,14 @@ public double getConst(String key, double def) {
6970
}
7071
return pref.getDouble("Const/" + key, def);
7172
}
73+
74+
public void putData(String string, PIDController controller) {
75+
SmartDashboard.putData(string, controller);
76+
}
77+
78+
public void putNumber(String string, double d) {
79+
SmartDashboard.putNumber(string, d);
80+
}
7281
/*
7382
* if (!SmartDashboard.containsKey("Const/" + key)) { if
7483
* (!SmartDashboard.putNumber("Const/" + key, def)) {
Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,11 @@
11
package org.usfirst.frc.team199.Robot2018;
22

3+
import edu.wpi.first.wpilibj.PIDController;
4+
35
public interface SmartDashboardInterface {
46
public double getConst(String key, double def);
7+
8+
public void putData(String string, PIDController controller);
9+
10+
public void putNumber(String string, double d);
511
}

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

Lines changed: 7 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -9,7 +9,6 @@
99
import edu.wpi.first.wpilibj.PIDOutput;
1010
import edu.wpi.first.wpilibj.PIDSource;
1111
import edu.wpi.first.wpilibj.command.Command;
12-
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
1312

1413
/**
1514
* Drives the robot a certain target distance using PID. Implements PIDOutput in
@@ -21,6 +20,7 @@ public class PIDMove extends Command implements PIDOutput {
2120
private DrivetrainInterface dt;
2221
private PIDController moveController;
2322
private PIDSource avg;
23+
private SmartDashboardInterface sd;
2424

2525
/**
2626
* Constructs this command with a new PIDController. Sets all of the
@@ -43,13 +43,14 @@ public PIDMove(double targ, DrivetrainInterface dt, SmartDashboardInterface sd,
4343
target = targ;
4444
this.dt = dt;
4545
this.avg = avg;
46+
this.sd = sd;
4647
if (Robot.dt != null) {
4748
requires(Robot.dt);
4849
}
4950
double kf = 1 / (dt.getCurrentMaxSpeed() * sd.getConst("Default PID Update Time", 0.05));
5051
moveController = new PIDController(sd.getConst("MovekP", 0.1), sd.getConst("MovekI", 0),
5152
sd.getConst("MovekD", 0), kf, avg, this);
52-
SmartDashboard.putData("Move PID", moveController);
53+
sd.putData("Move PID", moveController);
5354
}
5455

5556
/**
@@ -78,6 +79,7 @@ public PIDMove(double[] point, DrivetrainInterface dt, SmartDashboardInterface s
7879
this.target = dist;
7980
this.dt = dt;
8081
this.avg = avg;
82+
this.sd = sd;
8183
if (Robot.dt != null) {
8284
requires(Robot.dt);
8385
}
@@ -114,8 +116,8 @@ public void initialize() {
114116
@Override
115117
protected void execute() {
116118
System.out.println("Enc Avg Dist: " + avg.pidGet());
117-
SmartDashboard.putNumber("Move PID Result", moveController.get());
118-
SmartDashboard.putNumber("Move PID Error", moveController.getError());
119+
sd.putNumber("Move PID Result", moveController.get());
120+
sd.putNumber("Move PID Error", moveController.getError());
119121
}
120122

121123
/**
@@ -173,6 +175,6 @@ protected void interrupted() {
173175
@Override
174176
public void pidWrite(double output) {
175177
dt.arcadeDrive(output, 0);
176-
SmartDashboard.putNumber("Move PID Output", output);
178+
sd.putNumber("Move PID Output", output);
177179
}
178180
}

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

Lines changed: 5 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -24,6 +24,7 @@ public class PIDTurn extends Command implements PIDOutput {
2424
private PIDSource ahrs;
2525
private Timer tim;
2626
private double lastTime;
27+
private SmartDashboardInterface sd;
2728

2829
/**
2930
* Constructs this command with a new PIDController. Sets all of the
@@ -49,6 +50,7 @@ public PIDTurn(double targ, DrivetrainInterface dt, SmartDashboardInterface sd,
4950
// Use requires() here to declare subsystem dependencies
5051
this.dt = dt;
5152
this.ahrs = ahrs;
53+
this.sd = sd;
5254

5355
if (absolute) {
5456
target = targ - AutoUtils.position.getRot();
@@ -128,7 +130,7 @@ public PIDTurn(double[] point, DrivetrainInterface dt, SmartDashboardInterface s
128130
turnController = new PIDController(sd.getConst("TurnkP", 1), sd.getConst("TurnkI", 0), sd.getConst("TurnkD", 0),
129131
kf, ahrs, this);
130132
// tim = new Timer();
131-
SmartDashboard.putData("Turn PID", turnController);
133+
sd.putData("Turn PID", turnController);
132134
}
133135

134136
/**
@@ -203,8 +205,8 @@ protected boolean isFinished() {
203205
protected void end() {
204206
turnController.disable();
205207
System.out.println("end");
206-
SmartDashboard.putNumber("Turn PID Result", turnController.get());
207-
SmartDashboard.putNumber("Turn PID Error", turnController.getError());
208+
sd.putNumber("Turn PID Result", turnController.get());
209+
sd.putNumber("Turn PID Error", turnController.getError());
208210
// turnController.free();
209211

210212
AutoUtils.position.changeRot(dt.getAHRSAngle());

0 commit comments

Comments
 (0)