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

Commit ae3344f

Browse files
lhmcgannlhmcgann
committed
lots of Lift functionalities added
implemented LiftToPosition, MoveLift, MoveLiftWithPID added buttons for the new commands (w/ some finagling for LiftToPosition params) see https://trello.com/c/7OOvkOJG/167-implement-pid-on-lift-encoder for details in checklist
1 parent 93926ff commit ae3344f

10 files changed

Lines changed: 300 additions & 42 deletions

File tree

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

Lines changed: 18 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -10,19 +10,22 @@
1010
import org.usfirst.frc.team199.Robot2018.commands.CloseIntake;
1111
import org.usfirst.frc.team199.Robot2018.commands.FindTurnTimeConstant;
1212
import org.usfirst.frc.team199.Robot2018.commands.IntakeCube;
13+
import org.usfirst.frc.team199.Robot2018.commands.LiftToPosition;
14+
import org.usfirst.frc.team199.Robot2018.commands.MoveLift;
15+
import org.usfirst.frc.team199.Robot2018.commands.MoveLiftWithPID;
1316
import org.usfirst.frc.team199.Robot2018.commands.OpenIntake;
1417
import org.usfirst.frc.team199.Robot2018.commands.OuttakeCube;
1518
import org.usfirst.frc.team199.Robot2018.commands.PIDMove;
1619
import org.usfirst.frc.team199.Robot2018.commands.PIDTurn;
1720
import org.usfirst.frc.team199.Robot2018.commands.ResetEncoders;
18-
import org.usfirst.frc.team199.Robot2018.commands.RunLift;
1921
import org.usfirst.frc.team199.Robot2018.commands.SetDistancePerPulse;
2022
import org.usfirst.frc.team199.Robot2018.commands.ShiftDriveType;
2123
import org.usfirst.frc.team199.Robot2018.commands.ShiftHighGear;
2224
import org.usfirst.frc.team199.Robot2018.commands.ShiftLowGear;
2325
import org.usfirst.frc.team199.Robot2018.commands.ToggleLeftIntake;
2426
import org.usfirst.frc.team199.Robot2018.commands.ToggleRightIntake;
2527
import org.usfirst.frc.team199.Robot2018.commands.UpdatePIDConstants;
28+
import org.usfirst.frc.team199.Robot2018.subsystems.LiftInterface.LiftHeight;
2629

2730
import edu.wpi.first.wpilibj.Joystick;
2831
import edu.wpi.first.wpilibj.buttons.JoystickButton;
@@ -48,6 +51,9 @@ public class OI {
4851
private JoystickButton resetEncButton;
4952
private JoystickButton moveLiftUpButton;
5053
private JoystickButton moveLiftDownButton;
54+
private JoystickButton moveLiftPIDUpButton;
55+
private JoystickButton moveLiftPIDDownButton;
56+
private JoystickButton testLiftPID;
5157
private JoystickButton findTurnTimeConstantButton;
5258
private JoystickButton updatePIDConstantsButton;
5359
private JoystickButton updateEncoderDPPButton;
@@ -92,6 +98,15 @@ public OI(Robot robot) {
9298
findTurnTimeConstantButton
9399
.whenPressed(new FindTurnTimeConstant(robot, Robot.dt, Robot.rmap.fancyGyro, Robot.sd));
94100

101+
moveLiftPIDUpButton = new JoystickButton(leftJoy, getButton("Run Lift PID Up", 3));
102+
moveLiftPIDUpButton.whileHeld(new MoveLiftWithPID(Robot.lift, true));
103+
moveLiftPIDDownButton = new JoystickButton(leftJoy, getButton("Run Lift PID Down", 4));
104+
moveLiftPIDDownButton.whileHeld(new MoveLiftWithPID(Robot.lift, false));
105+
106+
testLiftPID = new JoystickButton(leftJoy, getButton("Test Lift PID", 5));
107+
testLiftPID.whenPressed(
108+
new LiftToPosition(Robot.lift, Robot.getString("Lift Targ Height", LiftHeight.SWITCH.toString())));
109+
95110
rightJoy = new Joystick(1);
96111
shiftHighGearButton = new JoystickButton(rightJoy, getButton("Shift High Gear", 4));
97112
shiftHighGearButton.whenPressed(new ShiftHighGear());
@@ -104,9 +119,9 @@ public OI(Robot robot) {
104119
updateEncoderDPPButton.whenPressed(new SetDistancePerPulse());
105120

106121
moveLiftUpButton = new JoystickButton(rightJoy, getButton("Run Lift Motor Up", 10));
107-
moveLiftUpButton.whileHeld(new RunLift(Robot.lift, true));
122+
moveLiftUpButton.whileHeld(new MoveLift(Robot.lift, true));
108123
moveLiftDownButton = new JoystickButton(rightJoy, getButton("Run Lift Motor Down", 11));
109-
moveLiftDownButton.whileHeld(new RunLift(Robot.lift, false));
124+
moveLiftDownButton.whileHeld(new MoveLift(Robot.lift, false));
110125

111126
manipulator = new Joystick(2);
112127
if (manipulator.getButtonCount() == 0) {

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

Lines changed: 18 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -72,6 +72,19 @@ public double getConst(String key, double def) {
7272
return pref.getDouble("Const/" + key, def);
7373
}
7474

75+
@Override
76+
public String getString(String key, String def) {
77+
Preferences pref = Preferences.getInstance();
78+
if (!pref.containsKey("String/" + key)) {
79+
pref.putString("String/" + key, def);
80+
if (pref.getString("String/ + key", def) != def) {
81+
System.err.println("pref Key" + "String/" + key + "already taken by a different type");
82+
return def;
83+
}
84+
}
85+
return pref.getString("String/" + key, def);
86+
}
87+
7588
@Override
7689
public void putConst(String key, double def) {
7790
Preferences pref = Preferences.getInstance();
@@ -109,6 +122,10 @@ public static double getConst(String key, double def) {
109122
return sd.getConst(key, def);
110123
}
111124

125+
public static String getString(String key, String def) {
126+
return sd.getString(key, def);
127+
}
128+
112129
public static void putConst(String key, double def) {
113130
sd.putConst(key, def);
114131
}
@@ -135,8 +152,7 @@ public void robotInit() {
135152
climber = new Climber();
136153
climberAssist = new ClimberAssist();
137154
intakeEject = new IntakeEject();
138-
lift = new Lift("Lift", getConst("LiftkP", 0.1), getConst("LiftkI", 0), getConst("LiftkD", 0),
139-
getConst("LiftkF", 0.1));
155+
lift = new Lift();
140156
dt = new Drivetrain(sd);
141157
oi = new OI(this);
142158

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

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -12,4 +12,6 @@ public interface SmartDashboardInterface {
1212
public void putNumber(String string, double d);
1313

1414
public void putBoolean(String string, boolean b);
15+
16+
public String getString(String key, String def);
1517
}

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

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -3,7 +3,7 @@
33
import org.usfirst.frc.team199.Robot2018.Robot;
44
import org.usfirst.frc.team199.Robot2018.RobotMap;
55
import org.usfirst.frc.team199.Robot2018.subsystems.LiftInterface;
6-
import org.usfirst.frc.team199.Robot2018.subsystems.LiftInterface.Position;
6+
import org.usfirst.frc.team199.Robot2018.subsystems.LiftInterface.LiftHeight;
77

88
import edu.wpi.first.wpilibj.PIDController;
99
import edu.wpi.first.wpilibj.PIDOutput;
@@ -28,11 +28,11 @@ public class AutoLift extends Command implements PIDOutput {
2828
private double desiredDist = 0;
2929
private double currDist;
3030
private LiftInterface lift;
31-
private Position desiredPos;
31+
private LiftHeight desiredPos;
3232

3333
private PIDController liftController;
3434

35-
public AutoLift(Position stage, LiftInterface lift) {
35+
public AutoLift(LiftHeight stage, LiftInterface lift) {
3636
this.lift = lift;
3737
requires(Robot.lift);
3838

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

Lines changed: 63 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,63 @@
1+
package org.usfirst.frc.team199.Robot2018.commands;
2+
3+
import org.usfirst.frc.team199.Robot2018.Robot;
4+
import org.usfirst.frc.team199.Robot2018.subsystems.Lift;
5+
import org.usfirst.frc.team199.Robot2018.subsystems.LiftInterface.LiftHeight;
6+
7+
import edu.wpi.first.wpilibj.command.Command;
8+
9+
/**
10+
*
11+
*/
12+
public class LiftToPosition extends Command {
13+
14+
private Lift lift;
15+
private LiftHeight pos;
16+
17+
public LiftToPosition(Lift lift, String goal) {
18+
requires(Robot.lift);
19+
this.lift = lift;
20+
pos = LiftHeight.toLH(goal);
21+
}
22+
23+
// Called just before this Command runs the first time
24+
@Override
25+
protected void initialize() {
26+
// There's no way to reset the Lift's PID constants after constructing it, so if
27+
// you change them you must Restart Robot Code in DS.
28+
double setpoint = lift.getDesiredDistFromPos(pos);
29+
lift.setSetpoint(setpoint);
30+
System.out.println("Target Height: " + setpoint);
31+
lift.enable();
32+
System.out.println("Enabled");
33+
}
34+
35+
// Called repeatedly when this Command is scheduled to run
36+
@Override
37+
protected void execute() {
38+
System.out.println("Current Height: " + lift.getHeight());
39+
}
40+
41+
// Make this return true when this Command no longer needs to run execute()
42+
@Override
43+
protected boolean isFinished() {
44+
return lift.onTarget();
45+
}
46+
47+
// Called once after isFinished returns true
48+
@Override
49+
protected void end() {
50+
lift.disable();
51+
System.out.println("Disabled");
52+
// keep stopLift() after disable() so they don't conflict
53+
lift.stopLift();
54+
System.out.println("Stopped");
55+
}
56+
57+
// Called when another command which requires one or more of the same
58+
// subsystems is scheduled to run
59+
@Override
60+
protected void interrupted() {
61+
end();
62+
}
63+
}

Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/RunLift.java renamed to Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/MoveLift.java

Lines changed: 14 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -1,51 +1,56 @@
11
package org.usfirst.frc.team199.Robot2018.commands;
22

33
import org.usfirst.frc.team199.Robot2018.Robot;
4-
import org.usfirst.frc.team199.Robot2018.subsystems.LiftInterface;
4+
import org.usfirst.frc.team199.Robot2018.subsystems.Lift;
55

66
import edu.wpi.first.wpilibj.command.Command;
77

88
/**
9-
*
9+
* Use this command to test the Lift without PID
1010
*/
11-
public class RunLift extends Command {
11+
public class MoveLift extends Command {
1212

13-
private LiftInterface lift;
14-
private final double SPEED = 0.05;
13+
private Lift lift;
14+
private double SPEED;
1515
private int dir;
1616

17-
public RunLift(LiftInterface lift, boolean up) {
18-
// Use requires() here to declare subsystem dependencies
19-
// eg. requires(chassis);
20-
this.lift = lift;
17+
public MoveLift(Lift lift, boolean up) {
2118
requires(Robot.lift);
19+
this.lift = lift;
2220
if (up)
2321
dir = 1;
2422
else
2523
dir = -1;
2624
}
2725

2826
// Called just before this Command runs the first time
27+
@Override
2928
protected void initialize() {
29+
lift.disable();
30+
SPEED = Robot.getConst("Lift Speed", 0.05);
3031
}
3132

3233
// Called repeatedly when this Command is scheduled to run
34+
@Override
3335
protected void execute() {
3436
lift.runMotor(SPEED * dir);
3537
}
3638

3739
// Make this return true when this Command no longer needs to run execute()
40+
@Override
3841
protected boolean isFinished() {
3942
return false;
4043
}
4144

4245
// Called once after isFinished returns true
46+
@Override
4347
protected void end() {
4448
lift.stopLift();
4549
}
4650

4751
// Called when another command which requires one or more of the same
4852
// subsystems is scheduled to run
53+
@Override
4954
protected void interrupted() {
5055
end();
5156
}

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

Lines changed: 62 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,62 @@
1+
package org.usfirst.frc.team199.Robot2018.commands;
2+
3+
import org.usfirst.frc.team199.Robot2018.Robot;
4+
import org.usfirst.frc.team199.Robot2018.subsystems.Lift;
5+
6+
import edu.wpi.first.wpilibj.command.Command;
7+
8+
/**
9+
* Use this command to test the Lift with PID
10+
*/
11+
public class MoveLiftWithPID extends Command {
12+
13+
private Lift lift;
14+
private double setpoint;
15+
private int dir;
16+
17+
public MoveLiftWithPID(Lift lift, boolean up) {
18+
requires(Robot.lift);
19+
this.lift = lift;
20+
if (up)
21+
dir = 1;
22+
else
23+
dir = -1;
24+
}
25+
26+
// Called just before this Command runs the first time
27+
@Override
28+
protected void initialize() {
29+
setpoint = 0;
30+
lift.setSetpoint(setpoint);
31+
lift.enable();
32+
}
33+
34+
// Called repeatedly when this Command is scheduled to run
35+
@Override
36+
protected void execute() {
37+
setpoint += dir * Robot.getConst("Lift Move Increment", 1); // inches
38+
lift.setSetpoint(setpoint);
39+
}
40+
41+
// Make this return true when this Command no longer needs to run execute()
42+
@Override
43+
protected boolean isFinished() {
44+
// always false bc whileHeld
45+
return false;
46+
}
47+
48+
// Called once after isFinished returns true
49+
@Override
50+
protected void end() {
51+
lift.disable();
52+
// keep stopLift() after disable() so they don't conflict
53+
lift.stopLift();
54+
}
55+
56+
// Called when another command which requires one or more of the same
57+
// subsystems is scheduled to run
58+
@Override
59+
protected void interrupted() {
60+
end();
61+
}
62+
}

0 commit comments

Comments
 (0)