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

Commit a7a86b4

Browse files
DriverStationComputerDriverStationComputer
committed
PID Move testing
getting to the point where the robot moves using PIDMove still needs way more testing and tuning
1 parent 2146a50 commit a7a86b4

6 files changed

Lines changed: 82 additions & 7 deletions

File tree

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

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

1010
import org.usfirst.frc.team199.Robot2018.commands.PIDMove;
1111
import org.usfirst.frc.team199.Robot2018.commands.PIDTurn;
12+
import org.usfirst.frc.team199.Robot2018.commands.ResetEncoders;
1213
import org.usfirst.frc.team199.Robot2018.commands.SetDistancePerPulse;
1314
import org.usfirst.frc.team199.Robot2018.commands.ShiftDriveType;
1415
import org.usfirst.frc.team199.Robot2018.commands.ShiftHighGear;
@@ -31,18 +32,19 @@ public class OI {
3132
private JoystickButton shiftDriveTypeButton;
3233
private JoystickButton PIDMoveButton;
3334
private JoystickButton PIDTurnButton;
35+
private JoystickButton resetEncButton;
3436
public Joystick rightJoy;
3537
private JoystickButton updatePIDConstantsButton;
3638
private JoystickButton updateEncoderDPPButton;
3739
public Joystick manipulator;
38-
40+
3941
public int getButton(String key, int def) {
4042
if (!SmartDashboard.containsKey("Button/" + key)) {
4143
if (!SmartDashboard.putNumber("Button/" + key, def)) {
4244
System.err.println("SmartDashboard Key" + "Button/" + key + "already taken by a different type");
4345
return def;
4446
}
45-
}
47+
}
4648
return (int) SmartDashboard.getNumber("Button/" + key, def);
4749
}
4850

@@ -51,9 +53,11 @@ public OI() {
5153
shiftDriveTypeButton = new JoystickButton(leftJoy, getButton("Shift Drive Type", 2));
5254
shiftDriveTypeButton.whenPressed(new ShiftDriveType());
5355
PIDMoveButton = new JoystickButton(leftJoy, getButton("PID Move", 7));
54-
PIDMoveButton.whenPressed(new PIDMove(40, Robot.dt, RobotMap.distEncAvg));
56+
PIDMoveButton.whenPressed(new PIDMove(Robot.getConst("Move Targ", 24), Robot.dt, RobotMap.distEncAvg));
5557
PIDTurnButton = new JoystickButton(leftJoy, getButton("PID Turn", 8));
5658
PIDTurnButton.whenPressed(new PIDTurn(30, Robot.dt, RobotMap.fancyGyro));
59+
resetEncButton = new JoystickButton(leftJoy, getButton("Reset Dist Enc", 10));
60+
resetEncButton.whenPressed(new ResetEncoders());
5761

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

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

Lines changed: 21 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -178,10 +178,24 @@ public void teleopInit() {
178178
@Override
179179
public void teleopPeriodic() {
180180
Scheduler.getInstance().run();
181+
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());
181192
}
182193

183194
boolean firstTime = true;
184195

196+
public void testInit() {
197+
}
198+
185199
/**
186200
* This function is called periodically during test mode
187201
*/
@@ -192,13 +206,20 @@ public void testPeriodic() {
192206
// firstTime = false;
193207
//// }
194208
// Robot.dt.setVPIDs(Robot.getConst("VPID Test Set", 0.5));
209+
210+
Scheduler.getInstance().run();
211+
195212
SmartDashboard.putNumber("Drivetrain/Left VPID Targ", Robot.dt.getLeftVPIDSetpoint());
196213
SmartDashboard.putNumber("Drivetrain/Right VPID Targ", Robot.dt.getRightVPIDSetpoint());
197214
SmartDashboard.putNumber("Left VPID Error", Robot.dt.getLeftVPIDerror());
198215
SmartDashboard.putNumber("Right VPID Error", Robot.dt.getRightVPIDerror());
199216
SmartDashboard.putNumber("Left Enc Rate", Robot.dt.getLeftEncRate());
200217
SmartDashboard.putNumber("Right Enc Rate", Robot.dt.getRightEncRate());
201218

219+
SmartDashboard.putNumber("Left Enc Dist", dt.getLeftDist());
220+
SmartDashboard.putNumber("Right Enc Dist", dt.getRightDist());
221+
SmartDashboard.putNumber("Avg Enc Dist", dt.getEncAvgDist());
222+
202223
// dt.dtLeft.set(0.1);
203224
// dt.dtRight.set(-oi.rightJoy.getY());
204225
// dt.dtLeft.set(-oi.leftJoy.getY());

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

Lines changed: 10 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -39,7 +39,7 @@ public PIDMove(double targ, DrivetrainInterface dt, PIDSourceAverage avg) {
3939
target = targ;
4040
this.dt = dt;
4141
double kf = 1 / (dt.getCurrentMaxSpeed() * Robot.getConst("Default PID Update Time", 0.05));
42-
moveController = new PIDController(Robot.getConst("MovekP", 1), Robot.getConst("MovekI", 0),
42+
moveController = new PIDController(Robot.getConst("MovekP", 0), Robot.getConst("MovekI", 0),
4343
Robot.getConst("MovekD", 0), kf, avg, this);
4444
}
4545

@@ -50,15 +50,19 @@ public PIDMove(double targ, DrivetrainInterface dt, PIDSourceAverage avg) {
5050
@Override
5151
public void initialize() {
5252
dt.resetDistEncs();
53+
moveController.disable();
5354
// input is in inches
5455
moveController.setInputRange(-Robot.getConst("Max High Speed", 204), Robot.getConst("Max High Speed", 204));
5556
// output in "motor units" (arcade and tank only accept values [-1, 1]
5657
moveController.setOutputRange(-1.0, 1.0);
5758
moveController.setContinuous(false);
5859
moveController.setAbsoluteTolerance(Robot.getConst("MoveTolerance", 2));
5960
moveController.setSetpoint(target);
61+
62+
SmartDashboard.putData("Move PID", moveController);
63+
6064
moveController.enable();
61-
SmartDashboard.putData(moveController);
65+
dt.enableVelocityPIDs();
6266
}
6367

6468
/**
@@ -68,6 +72,8 @@ public void initialize() {
6872
*/
6973
@Override
7074
protected void execute() {
75+
SmartDashboard.putNumber("Move PID Result", moveController.get());
76+
SmartDashboard.putNumber("Move PID Error", moveController.getError());
7177
}
7278

7379
/**
@@ -89,7 +95,7 @@ protected boolean isFinished() {
8995
@Override
9096
protected void end() {
9197
moveController.disable();
92-
moveController.free();
98+
// moveController.free();
9399
}
94100

95101
/**
@@ -115,5 +121,6 @@ protected void interrupted() {
115121
@Override
116122
public void pidWrite(double output) {
117123
dt.arcadeDrive(output, 0);
124+
SmartDashboard.putNumber("Move PID Output", output);
118125
}
119126
}

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

Lines changed: 6 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -8,6 +8,7 @@
88
import edu.wpi.first.wpilibj.PIDController;
99
import edu.wpi.first.wpilibj.PIDOutput;
1010
import edu.wpi.first.wpilibj.command.Command;
11+
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
1112

1213
/**
1314
* Turns the robot to a certain target bearing using PID. Implements PIDOutput
@@ -63,7 +64,9 @@ protected void initialize() {
6364
turnController.setContinuous();
6465
turnController.setAbsoluteTolerance(Robot.getConst("TurnTolerance", 1));
6566
turnController.setSetpoint(target);
67+
SmartDashboard.putData("Turn PID", turnController);
6668
turnController.enable();
69+
dt.enableVelocityPIDs();
6770
}
6871

6972
/**
@@ -73,6 +76,8 @@ protected void initialize() {
7376
*/
7477
@Override
7578
protected void execute() {
79+
SmartDashboard.putNumber("Turn PID Result", turnController.get());
80+
SmartDashboard.putNumber("Turn PID Error", turnController.getError());
7681
}
7782

7883
/**
@@ -95,7 +100,7 @@ protected boolean isFinished() {
95100
@Override
96101
protected void end() {
97102
turnController.disable();
98-
turnController.free();
103+
// turnController.free();
99104
}
100105

101106
/**

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

Lines changed: 22 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,22 @@
1+
package org.usfirst.frc.team199.Robot2018.commands;
2+
3+
import org.usfirst.frc.team199.Robot2018.Robot;
4+
5+
import edu.wpi.first.wpilibj.command.InstantCommand;
6+
7+
/**
8+
*
9+
*/
10+
public class ResetEncoders extends InstantCommand {
11+
12+
public ResetEncoders() {
13+
// Use requires() here to declare subsystem dependencies
14+
// eg. requires(chassis);
15+
super();
16+
}
17+
18+
// Called just before this Command runs the first time
19+
protected void initialize() {
20+
Robot.dt.resetDistEncs();
21+
}
22+
}

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

Lines changed: 16 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -52,6 +52,22 @@ public void initDefaultCommand() {
5252
setDefaultCommand(new TeleopDrive());
5353
}
5454

55+
public PIDSourceAverage getDistEncAvg() {
56+
return distEncAvg;
57+
}
58+
59+
public double getEncAvgDist() {
60+
return distEncAvg.pidGet();
61+
}
62+
63+
public double getLeftDist() {
64+
return leftEncDist.getDistance();
65+
}
66+
67+
public double getRightDist() {
68+
return rightEncDist.getDistance();
69+
}
70+
5571
public void setLeft(double spd) {
5672
dtLeft.set(spd);
5773
}

0 commit comments

Comments
 (0)