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

Commit eb6f6f3

Browse files
DriverStationComputerDriverStationComputer
committed
changes to lift and intake
lift: reverted to old UpdateLiftPos, etc; implemented command group AutoLift for auto functions of going to the ground in 2 stages intake: IntakeCube and OuttakeCube ran motors in reverse direction than what they should have
1 parent 38deb6f commit eb6f6f3

12 files changed

Lines changed: 103 additions & 61 deletions

File tree

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

Lines changed: 6 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -7,10 +7,10 @@
77

88
package org.usfirst.frc.team199.Robot2018;
99

10+
import org.usfirst.frc.team199.Robot2018.commands.AutoLift;
1011
import org.usfirst.frc.team199.Robot2018.commands.CloseIntake;
1112
import org.usfirst.frc.team199.Robot2018.commands.FindTurnTimeConstant;
1213
import org.usfirst.frc.team199.Robot2018.commands.IntakeCube;
13-
import org.usfirst.frc.team199.Robot2018.commands.LiftToPosition;
1414
import org.usfirst.frc.team199.Robot2018.commands.MoveLift;
1515
import org.usfirst.frc.team199.Robot2018.commands.MoveLiftWithPID;
1616
import org.usfirst.frc.team199.Robot2018.commands.OpenIntake;
@@ -106,8 +106,8 @@ public OI(Robot robot) {
106106
moveLiftPIDDownButton.whileHeld(new MoveLiftWithPID(Robot.lift, false));
107107

108108
testLiftPID = new JoystickButton(leftJoy, getButton("Test Lift PID", 5));
109-
testLiftPID.whenPressed(new LiftToPosition(Robot.lift,
110-
SmartDashboard.getString("Lift Targ Height", LiftHeight.SWITCH.toString())));
109+
testLiftPID.whenPressed(
110+
new AutoLift(Robot.lift, SmartDashboard.getString("Lift Targ Height", LiftHeight.SWITCH.toString())));
111111

112112
rightJoy = new Joystick(1);
113113
shiftHighGearButton = new JoystickButton(rightJoy, getButton("Shift High Gear", 4));
@@ -125,6 +125,9 @@ public OI(Robot robot) {
125125
moveLiftDownButton = new JoystickButton(rightJoy, getButton("Run Lift Motor Down", 11));
126126
moveLiftDownButton.whileHeld(new MoveLift(Robot.lift, false));
127127

128+
stopIntakeButton = new JoystickButton(rightJoy, getButton("Stop Intake Button", 6));
129+
stopIntakeButton.whenPressed(new StopIntake());
130+
128131
manipulator = new Joystick(2);
129132
if (manipulator.getButtonCount() == 0) {
130133
System.err.println(
@@ -148,9 +151,6 @@ public OI(Robot robot) {
148151
toggleLeftIntakeButton.whenPressed(new ToggleLeftIntake());
149152
toggleRightIntakeButton = new JoystickButton(manipulator, getButton("Toggle Right Intake Button", 4));
150153
toggleRightIntakeButton.whenPressed(new ToggleRightIntake());
151-
152-
stopIntakeButton = new JoystickButton(manipulator, getButton("Stop Intake Button", 7));
153-
stopIntakeButton.whenPressed(new StopIntake());
154154
}
155155

156156
}

‎Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/AutoLift.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.subsystems.Lift;
4+
import org.usfirst.frc.team199.Robot2018.subsystems.LiftInterface.LiftHeight;
5+
6+
import edu.wpi.first.wpilibj.command.CommandGroup;
7+
8+
/**
9+
*
10+
*/
11+
public class AutoLift extends CommandGroup {
12+
13+
public AutoLift(Lift lift, String height) {
14+
if (height.equals("GROUND")) {
15+
addSequential(new LiftToPosition(lift, LiftHeight.toLH("HOLD_CUBE")));
16+
addSequential(new LiftToPosition(lift, LiftHeight.toLH(height)));
17+
} else {
18+
19+
addSequential(new LiftToPosition(lift, LiftHeight.toLH(height)));
20+
}
21+
}
22+
}

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

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -9,8 +9,8 @@
99
*/
1010
public class EjectToExchange extends CommandGroup {
1111

12-
public EjectToExchange() {
13-
addSequential(new LiftToPosition(Robot.lift, "GROUND"));
14-
addSequential(new OuttakeCube());
15-
}
12+
public EjectToExchange() {
13+
addSequential(new AutoLift(Robot.lift, "GROUND"));
14+
addSequential(new OuttakeCube());
15+
}
1616
}

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

Lines changed: 8 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -9,11 +9,13 @@
99
*/
1010
public class EjectToScale extends CommandGroup {
1111

12-
public EjectToScale() {
13-
addSequential(new LiftToPosition(Robot.lift, "SCALE"));
14-
addSequential(new PIDMove(Robot.getConst("Auto Scale Move Dist", 12), Robot.dt, Robot.sd, Robot.dt.getDistEncAvg()));
12+
public EjectToScale() {
13+
addSequential(new AutoLift(Robot.lift, "SCALE"));
14+
addSequential(
15+
new PIDMove(Robot.getConst("Auto Scale Move Dist", 12), Robot.dt, Robot.sd, Robot.dt.getDistEncAvg()));
1516
addSequential(new OuttakeCube());
16-
addSequential(new PIDMove(-1 * Robot.getConst("Auto Scale Move Dist", 12), Robot.dt, Robot.sd, Robot.dt.getDistEncAvg()));
17-
addSequential(new LiftToPosition(Robot.lift, "GROUND"));
18-
}
17+
addSequential(new PIDMove(-1 * Robot.getConst("Auto Scale Move Dist", 12), Robot.dt, Robot.sd,
18+
Robot.dt.getDistEncAvg()));
19+
addSequential(new AutoLift(Robot.lift, "GROUND"));
20+
}
1921
}

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

Lines changed: 9 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -9,11 +9,13 @@
99
*/
1010
public class EjectToSwitch extends CommandGroup {
1111

12-
public EjectToSwitch() {
13-
addSequential(new LiftToPosition(Robot.lift, "SWITCH"));
14-
addSequential(new PIDMove(Robot.getConst("Auto Switch Move Dist", 12), Robot.dt, Robot.sd, Robot.dt.getDistEncAvg()));
15-
addSequential(new OuttakeCube());
16-
addSequential(new PIDMove(-1 * Robot.getConst("Auto Switch Move Dist", 12), Robot.dt, Robot.sd, Robot.dt.getDistEncAvg()));
17-
addSequential(new LiftToPosition(Robot.lift, "GROUND"));
18-
}
12+
public EjectToSwitch() {
13+
addSequential(new AutoLift(Robot.lift, "SWITCH"));
14+
addSequential(
15+
new PIDMove(Robot.getConst("Auto Switch Move Dist", 12), Robot.dt, Robot.sd, Robot.dt.getDistEncAvg()));
16+
addSequential(new OuttakeCube());
17+
addSequential(new PIDMove(-1 * Robot.getConst("Auto Switch Move Dist", 12), Robot.dt, Robot.sd,
18+
Robot.dt.getDistEncAvg()));
19+
addSequential(new AutoLift(Robot.lift, "GROUND"));
20+
}
1921
}

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

Lines changed: 9 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -4,6 +4,7 @@
44

55
import edu.wpi.first.wpilibj.Timer;
66
import edu.wpi.first.wpilibj.command.Command;
7+
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
78

89
/**
910
*
@@ -20,15 +21,18 @@ public IntakeCube() {
2021
}
2122

2223
// Called just before this Command runs the first time
24+
@Override
2325
protected void initialize() {
2426
tim.reset();
25-
tim.start();
27+
// tim.start();
2628
overDraw = false;
2729
}
2830

2931
// Called repeatedly when this Command is scheduled to run
32+
@Override
3033
protected void execute() {
31-
Robot.intakeEject.runIntake(-1);
34+
Robot.intakeEject.runIntake(1);
35+
SmartDashboard.putBoolean("Has Cube", Robot.intakeEject.hasCube());
3236
if (Robot.intakeEject.hasCube()) {
3337
if (!overDraw) {
3438
overDraw = true;
@@ -42,16 +46,19 @@ protected void execute() {
4246
}
4347

4448
// Make this return true when this Command no longer needs to run execute()
49+
@Override
4550
protected boolean isFinished() {
4651
return tim.get() > Robot.getConst("Has Cube Timeout", 0.5);
4752
}
4853

4954
// Called once after isFinished returns true
55+
@Override
5056
protected void end() {
5157
}
5258

5359
// Called when another command which requires one or more of the same
5460
// subsystems is scheduled to run
61+
@Override
5562
protected void interrupted() {
5663
}
5764
}

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

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -15,25 +15,25 @@ public class LiftToPosition extends Command {
1515
private LiftHeight pos;
1616
private boolean goToGround;
1717

18-
public LiftToPosition(Lift lift, String goal) {
18+
public LiftToPosition(Lift lift, LiftHeight goal) {
1919
requires(Robot.lift);
2020
this.lift = lift;
21-
pos = LiftHeight.toLH(goal);
21+
pos = goal;
2222
}
2323

2424
// Called just before this Command runs the first time
2525
@Override
2626
protected void initialize() {
2727
lift.getPIDController().setPID(Robot.getConst("LiftkP", 0.1), Robot.getConst("LiftkI", 0),
2828
Robot.getConst("LiftkD", 0), Robot.getConst("LiftkF", 0.1));
29+
double setpoint = lift.getDesiredDistFromPos(pos);
30+
lift.setSetpoint(setpoint);
31+
System.out.println("Target Height: " + setpoint);
2932
}
3033

3134
// Called repeatedly when this Command is scheduled to run
3235
@Override
3336
protected void execute() {
34-
double setpoint = lift.getDesiredDistFromPos(pos);
35-
lift.setSetpoint(setpoint);
36-
System.out.println("Target Height: " + setpoint);
3737
System.out.println("Current Height: " + lift.getHeight());
3838
}
3939

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

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -29,7 +29,7 @@ protected void initialize() {
2929
lift.disable();
3030
// below Lift Speed may be for manual testing only, but commented out code 2
3131
// lines below can be used if want max speed
32-
SPEED = Robot.getConst("Lift Speed", 0.05);
32+
SPEED = Robot.getConst("Lift Speed", 0.5);
3333
// SPEED = lift.getLiftMaxSpeed();
3434
}
3535

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

Lines changed: 6 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -18,29 +18,34 @@ public OuttakeCube() {
1818
}
1919

2020
// Called just before this Command runs the first time
21+
@Override
2122
protected void initialize() {
2223
tim = new Timer();
2324
tim.reset();
2425
tim.start();
2526
}
2627

2728
// Called repeatedly when this Command is scheduled to run
29+
@Override
2830
protected void execute() {
29-
Robot.intakeEject.runIntake(1);
31+
Robot.intakeEject.runIntake(-1);
3032
}
3133

3234
// Make this return true when this Command no longer needs to run execute()
35+
@Override
3336
protected boolean isFinished() {
3437
return tim.get() > Robot.getConst("Outake Time", 0.5);
3538
}
3639

3740
// Called once after isFinished returns true
41+
@Override
3842
protected void end() {
3943
Robot.intakeEject.runIntake(0);
4044
}
4145

4246
// Called when another command which requires one or more of the same
4347
// subsystems is scheduled to run
48+
@Override
4449
protected void interrupted() {
4550
end();
4651
}

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

Lines changed: 6 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -59,12 +59,12 @@ protected void execute() {
5959
lift.setSetpoint(desiredDist);
6060
}
6161

62-
// if (goToGround && lift.onTarget() && lift.getSpeed() <= 0.1) {
63-
// desiredPos = LiftHeight.GROUND;
64-
// desiredDist = lift.getDesiredDistFromPos(desiredPos);
65-
// lift.setSetpoint(desiredDist);
66-
// goToGround = false;
67-
// }
62+
if (goToGround && lift.onTarget() && lift.getSpeed() <= 0.1) {
63+
desiredPos = LiftHeight.GROUND;
64+
desiredDist = lift.getDesiredDistFromPos(desiredPos);
65+
lift.setSetpoint(desiredDist);
66+
goToGround = false;
67+
}
6868
}
6969
}
7070

0 commit comments

Comments
 (0)