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

Commit 80dd09e

Browse files
kevinzwangkevinzwang
authored
Auto testing (#2)
* use sd interface instead of sd in PIDMove and PIDTurn * testing auto changes relative vs absolute = problems moved math in PIDMove and PIDTurn to initialize instead of in the constructor added try catch for manipulator in DefaultIntake command (Daniel P, Kevin W, Laura M, Dean B)
1 parent eed3d76 commit 80dd09e

12 files changed

Lines changed: 306 additions & 63 deletions

File tree

‎Robot2018/lib/User_Libraries.properties‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,2 +1,2 @@
1-
#Sun Feb 18 17:40:43 PST 2018
1+
#Mon Feb 19 20:57:02 PST 2018
22
C\:\\Users\\dean\\git\\RobotCode2018\\Robot2018\\lib\\libpathfinderjava.so=4087ee38c338446b8305f67716260df4

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

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -86,7 +86,7 @@ public OI() {
8686
MoveLiftUpButton.whileHeld(new RunLift(Robot.lift, true));
8787
MoveLiftDownButton.whileHeld(new RunLift(Robot.lift, false));
8888

89-
// manipulator = new Joystick(2);
89+
manipulator = new Joystick(2);
9090
// closeIntake = new JoystickButton(manipulator, getButton("Close Intake
9191
// Button", 1));
9292
// closeIntake.whenPressed(new CloseIntake());

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

Lines changed: 17 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -12,8 +12,8 @@
1212
import java.util.Map;
1313

1414
import org.usfirst.frc.team199.Robot2018.autonomous.AutoUtils;
15+
import org.usfirst.frc.team199.Robot2018.autonomous.Position;
1516
import org.usfirst.frc.team199.Robot2018.commands.Autonomous;
16-
import org.usfirst.frc.team199.Robot2018.commands.Autonomous.Position;
1717
import org.usfirst.frc.team199.Robot2018.commands.Autonomous.Strategy;
1818
import org.usfirst.frc.team199.Robot2018.commands.ShiftLowGear;
1919
import org.usfirst.frc.team199.Robot2018.subsystems.Climber;
@@ -22,9 +22,9 @@
2222
import org.usfirst.frc.team199.Robot2018.subsystems.IntakeEject;
2323
import org.usfirst.frc.team199.Robot2018.subsystems.Lift;
2424

25-
import edu.wpi.first.wpilibj.CameraServer;
2625
import edu.wpi.first.wpilibj.DriverStation;
2726
import edu.wpi.first.wpilibj.IterativeRobot;
27+
import edu.wpi.first.wpilibj.PIDController;
2828
import edu.wpi.first.wpilibj.Preferences;
2929
import edu.wpi.first.wpilibj.command.Command;
3030
import edu.wpi.first.wpilibj.command.Scheduler;
@@ -53,7 +53,7 @@ public class Robot extends IterativeRobot {
5353
public static Map<String, ArrayList<String[]>> autoScripts;
5454

5555
Command autonomousCommand;
56-
SendableChooser<Position> posChooser = new SendableChooser<Position>();
56+
SendableChooser<Autonomous.Position> posChooser = new SendableChooser<Autonomous.Position>();
5757
Map<String, SendableChooser<Strategy>> stratChoosers = new HashMap<String, SendableChooser<Strategy>>();
5858
String[] fmsPossibilities = { "LL", "LR", "RL", "RR" };
5959

@@ -69,6 +69,14 @@ public double getConst(String key, double def) {
6969
}
7070
return pref.getDouble("Const/" + key, def);
7171
}
72+
73+
public void putData(String string, PIDController controller) {
74+
SmartDashboard.putData(string, controller);
75+
}
76+
77+
public void putNumber(String string, double d) {
78+
SmartDashboard.putNumber(string, d);
79+
}
7280
/*
7381
* if (!SmartDashboard.containsKey("Const/" + key)) { if
7482
* (!SmartDashboard.putNumber("Const/" + key, def)) {
@@ -109,7 +117,7 @@ public void robotInit() {
109117
oi = new OI();
110118

111119
// auto position chooser
112-
for (Position p : Position.values()) {
120+
for (Autonomous.Position p : Autonomous.Position.values()) {
113121
posChooser.addObject(p.getSDName(), p);
114122
}
115123
SmartDashboard.putData("Starting Position", posChooser);
@@ -131,8 +139,8 @@ public void robotInit() {
131139
autoScripts = AutoUtils.parseScriptFile(Preferences.getInstance().getString("autoscripts", ""));
132140

133141
listen = new Listener();
134-
CameraServer.getInstance().startAutomaticCapture(0);
135-
CameraServer.getInstance().startAutomaticCapture(1);
142+
// CameraServer.getInstance().startAutomaticCapture(0);
143+
// CameraServer.getInstance().startAutomaticCapture(1);
136144
}
137145

138146
/**
@@ -157,9 +165,11 @@ public void disabledPeriodic() {
157165
*/
158166
@Override
159167
public void autonomousInit() {
168+
dt.resetAHRS();
169+
AutoUtils.position = new Position(0, 0, 0);
160170
Scheduler.getInstance().add(new ShiftLowGear());
161171
String fmsInput = DriverStation.getInstance().getGameSpecificMessage();
162-
Position startPos = posChooser.getSelected();
172+
Autonomous.Position startPos = posChooser.getSelected();
163173
double autoDelay = SmartDashboard.getNumber("Auto Delay", 0);
164174

165175
Map<String, Strategy> strategies = new HashMap<String, Strategy>();

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

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/autonomous/AutoUtils.java‎

Lines changed: 6 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -6,7 +6,7 @@
66

77
public class AutoUtils {
88

9-
public static Position position = new Position(0, 0, 0);
9+
public static Position position;
1010

1111
/**
1212
* Parses the inputted script file into a map of scripts
@@ -25,6 +25,7 @@ public static Map<String, ArrayList<String[]>> parseScriptFile(String scriptFile
2525
String currScriptName = "";
2626

2727
int count = 0;
28+
int errorCount = 0;
2829
for (String line : lines) {
2930
count++;
3031
// remove comments
@@ -75,6 +76,8 @@ public static Map<String, ArrayList<String[]>> parseScriptFile(String scriptFile
7576
if (isValidCommand(instruction, args, count)) {
7677
String[] command = { instruction, args };
7778
currScript.add(command);
79+
} else {
80+
errorCount++;
7881
}
7982
}
8083

@@ -86,6 +89,8 @@ public static Map<String, ArrayList<String[]>> parseScriptFile(String scriptFile
8689
// remove the stray one in the beginning
8790
autoScripts.remove("");
8891

92+
System.out.println("[INFO] Successfuly parsed " + count + " lines with " + errorCount + " errors.");
93+
8994
return autoScripts;
9095
}
9196

‎Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/scriptupload/scriptupload.py‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,7 @@
11
#nt setup
22
from networktables import NetworkTables
33
import __future__
4+
import time
45
NetworkTables.initialize(server='10.1.99.2')
56
prefs = NetworkTables.getTable("Preferences")
67

@@ -30,6 +31,8 @@
3031
prefs.putString("autoscripts", oneline)
3132
print("Uploading %s as a String[] to key \"autoscripts\"" % filename)
3233

34+
time.sleep(0.1)
35+
3336
#checks if the key has been filled
3437
tester = "" #a variable to check if autoscripts is None
3538

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

Lines changed: 5 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -17,17 +17,18 @@
1717
*/
1818
public class AutoMoveTo extends CommandGroup {
1919

20-
public AutoMoveTo(String[] args, DrivetrainInterface dt, SmartDashboardInterface sd, PIDSource pidMoveSrc) {
20+
public AutoMoveTo(String[] args, DrivetrainInterface dt, SmartDashboardInterface sd, PIDSource pidMoveSrc,
21+
PIDSource pidTurnSource) {
2122
// requires(Drivetrain);
2223
double rotation;
2324
double[] point = { 0, 0 };
2425
for (String arg : args) {
2526
if (AutoUtils.isDouble(arg)) {
2627
rotation = Double.valueOf(arg);
27-
addSequential(new PIDTurn(rotation, dt, sd, pidMoveSrc, true));
28+
addSequential(new PIDTurn(rotation, dt, sd, pidTurnSource, true));
2829
} else if (AutoUtils.isPoint(arg)) {
2930
point = AutoUtils.parsePoint(arg);
30-
addSequential(new PIDTurn(point, dt, sd, pidMoveSrc));
31+
addSequential(new PIDTurn(point, dt, sd, pidTurnSource));
3132
addSequential(new PIDMove(point, dt, sd, pidMoveSrc));
3233
} else {
3334
throw new IllegalArgumentException();
@@ -36,6 +37,6 @@ public AutoMoveTo(String[] args, DrivetrainInterface dt, SmartDashboardInterface
3637
}
3738

3839
public AutoMoveTo(String[] args) {
39-
this(args, Robot.dt, Robot.sd, Robot.dt.getDistEncAvg());
40+
this(args, Robot.dt, Robot.sd, Robot.dt.getDistEncAvg(), Robot.dt.getGyro());
4041
}
4142
}

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

Lines changed: 12 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -9,6 +9,8 @@
99
*/
1010
public class DefaultIntake extends Command {
1111

12+
private boolean manipulatorPluggedIn = true;
13+
1214
public DefaultIntake() {
1315
// Use requires() here to declare subsystem dependencies
1416
// eg. requires(chassis);
@@ -17,13 +19,21 @@ public DefaultIntake() {
1719

1820
// Called just before this Command runs the first time
1921
protected void initialize() {
22+
try {
23+
Robot.oi.manipulator.getRawAxis(1);
24+
} catch (NullPointerException e) {
25+
System.err.println("[ERROR] Manipulator not plugged in.");
26+
manipulatorPluggedIn = false;
27+
}
2028
}
2129

2230
// Called repeatedly when this Command is scheduled to run
2331
protected void execute() {
2432
// 1 and 5 represent the axes' index in driver station
25-
Robot.intakeEject.runLeftIntake(Robot.oi.manipulator.getRawAxis(1));
26-
Robot.intakeEject.runRightIntake(Robot.oi.manipulator.getRawAxis(5));
33+
if (manipulatorPluggedIn) {
34+
Robot.intakeEject.runLeftIntake(Robot.oi.manipulator.getRawAxis(1));
35+
Robot.intakeEject.runRightIntake(Robot.oi.manipulator.getRawAxis(5));
36+
}
2737
}
2838

2939
// Make this return true when this Command no longer needs to run execute()

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

Lines changed: 19 additions & 10 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,9 @@ public class PIDMove extends Command implements PIDOutput {
2120
private DrivetrainInterface dt;
2221
private PIDController moveController;
2322
private PIDSource avg;
23+
private SmartDashboardInterface sd;
24+
private double pointX;
25+
private double pointY;
2426

2527
/**
2628
* Constructs this command with a new PIDController. Sets all of the
@@ -43,13 +45,14 @@ public PIDMove(double targ, DrivetrainInterface dt, SmartDashboardInterface sd,
4345
target = targ;
4446
this.dt = dt;
4547
this.avg = avg;
48+
this.sd = sd;
4649
if (Robot.dt != null) {
4750
requires(Robot.dt);
4851
}
4952
double kf = 1 / (dt.getCurrentMaxSpeed() * sd.getConst("Default PID Update Time", 0.05));
5053
moveController = new PIDController(sd.getConst("MovekP", 0.1), sd.getConst("MovekI", 0),
5154
sd.getConst("MovekD", 0), kf, avg, this);
52-
SmartDashboard.putData("Move PID", moveController);
55+
sd.putData("Move PID", moveController);
5356
}
5457

5558
/**
@@ -70,20 +73,19 @@ public PIDMove(double targ, DrivetrainInterface dt, SmartDashboardInterface sd,
7073
* the PIDSorceAverage of the DT's two Encoders
7174
*/
7275
public PIDMove(double[] point, DrivetrainInterface dt, SmartDashboardInterface sd, PIDSource avg) {
73-
double dx = point[0] - AutoUtils.position.getX();
74-
double dy = point[1] - AutoUtils.position.getY();
76+
pointX = point[0];
77+
pointY = point[1];
7578

76-
double dist = Math.sqrt(dx * dx + dy * dy); // pythagorean theorem to find distance
77-
78-
this.target = dist;
7979
this.dt = dt;
8080
this.avg = avg;
81+
this.sd = sd;
8182
if (Robot.dt != null) {
8283
requires(Robot.dt);
8384
}
8485
double kf = 1 / (dt.getCurrentMaxSpeed() * sd.getConst("Default PID Update Time", 0.05));
8586
moveController = new PIDController(sd.getConst("MovekP", 1), sd.getConst("MovekI", 0), sd.getConst("MovekD", 0),
8687
kf, avg, this);
88+
sd.putData("Move PID", moveController);
8789
}
8890

8991
/**
@@ -92,6 +94,12 @@ public PIDMove(double[] point, DrivetrainInterface dt, SmartDashboardInterface s
9294
*/
9395
@Override
9496
public void initialize() {
97+
double dx = pointX - AutoUtils.position.getX();
98+
double dy = pointY - AutoUtils.position.getY();
99+
100+
double dist = Math.sqrt(dx * dx + dy * dy); // pythagorean theorem to find distance
101+
this.target = dist;
102+
95103
dt.resetDistEncs();
96104
moveController.disable();
97105
// input is in inches
@@ -100,6 +108,7 @@ public void initialize() {
100108
moveController.setOutputRange(-1.0, 1.0);
101109
moveController.setContinuous(false);
102110
moveController.setAbsoluteTolerance(Robot.getConst("MoveTolerance", 0.1));
111+
System.out.println("move target = " + target);
103112
moveController.setSetpoint(target);
104113

105114
moveController.enable();
@@ -114,8 +123,8 @@ public void initialize() {
114123
@Override
115124
protected void execute() {
116125
System.out.println("Enc Avg Dist: " + avg.pidGet());
117-
SmartDashboard.putNumber("Move PID Result", moveController.get());
118-
SmartDashboard.putNumber("Move PID Error", moveController.getError());
126+
sd.putNumber("Move PID Result", moveController.get());
127+
sd.putNumber("Move PID Error", moveController.getError());
119128
}
120129

121130
/**
@@ -173,6 +182,6 @@ protected void interrupted() {
173182
@Override
174183
public void pidWrite(double output) {
175184
dt.arcadeDrive(output, 0);
176-
SmartDashboard.putNumber("Move PID Output", output);
185+
sd.putNumber("Move PID Output", output);
177186
}
178187
}

0 commit comments

Comments
 (0)