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

Commit 7573d5b

Browse files
nelly-yynelly-yy
committed
Added auto lift command and necessary functions for it
The distances may need to be adjusted as cad only had estimates for heights. Besides that, teleop control will be via holding buttons for up/down and auto will have preset commands for certain heights.
1 parent 5854469 commit 7573d5b

4 files changed

Lines changed: 148 additions & 1 deletion

File tree

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

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -33,7 +33,12 @@
3333
public class RobotMap {
3434

3535
public static WPI_TalonSRX intakeMotor;
36+
3637
public static WPI_TalonSRX liftMotor;
38+
public static Encoder liftEnc;
39+
public static DigitalSource liftEncPort1;
40+
public static DigitalSource liftEncPort2;
41+
3742
public static WPI_TalonSRX climberMotor;
3843

3944
public static DigitalSource leftEncPort1;
@@ -96,6 +101,10 @@ public RobotMap() {
96101
configSRX(intakeMotor);
97102
liftMotor = new WPI_TalonSRX(getPort("LiftTalonSRX", 5));
98103
configSRX(liftMotor);
104+
liftEncPort1 = new DigitalInput(getPort("1LiftEnc",0));
105+
liftEncPort2 = new DigitalInput(getPort("2LiftEnc",0));
106+
liftEnc = new Encoder(liftEncPort1,liftEncPort2);
107+
liftEnc.setPIDSourceType(PIDSourceType.kDisplacement);
99108
climberMotor = new WPI_TalonSRX(getPort("ClimberTalonSRX", 6));
100109
configSRX(climberMotor);
101110

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

Lines changed: 110 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,110 @@
1+
package org.usfirst.frc.team199.Robot2018.commands;
2+
3+
import org.usfirst.frc.team199.Robot2018.Robot;
4+
import org.usfirst.frc.team199.Robot2018.RobotMap;
5+
import org.usfirst.frc.team199.Robot2018.subsystems.LiftInterface;
6+
import org.usfirst.frc.team199.Robot2018.subsystems.LiftInterface.Position;
7+
8+
import edu.wpi.first.wpilibj.PIDController;
9+
import edu.wpi.first.wpilibj.PIDOutput;
10+
import edu.wpi.first.wpilibj.command.Command;
11+
12+
/**
13+
*
14+
*/
15+
public class AutoLift extends Command implements PIDOutput{
16+
17+
// All distances are - 1 foot for initial height of intake and + 3 inches for wiggle room for dropping cubes
18+
//Distance to switch
19+
// 18.75 inches in starting position
20+
private final double SWITCH_DIST = 9.75;
21+
//Distance to scale
22+
// 5 feet starting
23+
private final double SCALE_DIST = 51;
24+
//Distance to bar
25+
// 7 feet starting; bar distance should be changed because I'm not aware how climber mech will be positioned
26+
private final double BAR_DIST = 72;
27+
private double desiredDist = 0;
28+
private double currDist = 0;
29+
private LiftInterface lift;
30+
31+
private PIDController liftController;
32+
33+
public AutoLift(Position stage, LiftInterface lift) {
34+
// Use requires() here to declare subsystem dependencies
35+
// eg. requires(chassis);
36+
this.lift = lift;
37+
requires(Robot.lift);
38+
switch(lift.getCurrPos()) {
39+
case GROUND:
40+
currDist = 0;
41+
break;
42+
case SWITCH:
43+
currDist = SWITCH_DIST;
44+
break;
45+
case SCALE:
46+
currDist = SCALE_DIST;
47+
break;
48+
case BAR:
49+
currDist = BAR_DIST;
50+
break;
51+
}
52+
switch(stage) {
53+
case GROUND:
54+
desiredDist = -currDist;
55+
break;
56+
case SWITCH:
57+
desiredDist = SWITCH_DIST - currDist;
58+
break;
59+
case SCALE:
60+
desiredDist = SCALE_DIST - currDist;
61+
break;
62+
case BAR:
63+
desiredDist = BAR_DIST - currDist;
64+
break;
65+
}
66+
67+
liftController = new PIDController(Robot.getConst("LiftkP", 1), Robot.getConst("LiftkI", 0),
68+
Robot.getConst("LiftkD", 0),RobotMap.liftEnc, this);
69+
}
70+
71+
// Called just before this Command runs the first time
72+
protected void initialize() {
73+
lift.resetEnc();
74+
// input is in inches
75+
//liftController.setInputRange(-Robot.getConst("Max High Speed", 204), Robot.getConst("Max High Speed", 204));
76+
// output in "motor units" (arcade and tank only accept values [-1, 1]
77+
liftController.setOutputRange(-1.0, 1.0);
78+
liftController.setContinuous(false);
79+
//liftController.setAbsoluteTolerance(Robot.getConst("MoveTolerance", 2));
80+
liftController.setSetpoint(desiredDist);
81+
liftController.enable();
82+
}
83+
84+
// Called repeatedly when this Command is scheduled to run
85+
protected void execute() {
86+
}
87+
88+
// Make this return true when this Command no longer needs to run execute()
89+
protected boolean isFinished() {
90+
return liftController.onTarget();
91+
}
92+
93+
// Called once after isFinished returns true
94+
protected void end() {
95+
liftController.disable();
96+
liftController.free();
97+
}
98+
99+
// Called when another command which requires one or more of the same
100+
// subsystems is scheduled to run
101+
protected void interrupted() {
102+
end();
103+
}
104+
105+
@Override
106+
public void pidWrite(double output) {
107+
lift.runMotor(output);
108+
}
109+
110+
}

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

Lines changed: 17 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,9 +1,11 @@
11
package org.usfirst.frc.team199.Robot2018.subsystems;
22

33
import org.usfirst.frc.team199.Robot2018.RobotMap;
4+
import org.usfirst.frc.team199.Robot2018.subsystems.LiftInterface.Position;
45

56
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
67

8+
import edu.wpi.first.wpilibj.Encoder;
79
import edu.wpi.first.wpilibj.command.Subsystem;
810

911
/**
@@ -12,7 +14,7 @@
1214
public class Lift extends Subsystem implements LiftInterface {
1315

1416
private final WPI_TalonSRX liftMotor = RobotMap.liftMotor;
15-
17+
private final Encoder liftEnc = RobotMap.liftEnc;
1618
private Position targetPosition = Position.GROUND;
1719

1820
/**
@@ -64,5 +66,19 @@ public void runMotor(double output) {
6466
liftMotor.set(output);
6567
}
6668

69+
/**
70+
* Returns the position the lift is currently at
71+
* @return pos - current position
72+
*/
73+
public Position getCurrPos() {
74+
return targetPosition;
75+
}
76+
/**
77+
* Resets the encoder
78+
*/
79+
public void resetEnc() {
80+
liftEnc.reset();
81+
}
82+
6783
}
6884

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

Lines changed: 12 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -36,10 +36,22 @@ public enum Position {
3636
* @param offset - distance up or down from position
3737
*/
3838
public void goToPosition(Position position, double offset);
39+
3940
/**
4041
* Runs lift motor at specified speed
4142
* @param speed - desired speed to run at
4243
*/
4344
public void runMotor(double speed);
4445

46+
/**
47+
* Returns the position the lift is currently at
48+
* @return pos - current position
49+
*/
50+
public Position getCurrPos();
51+
52+
/**
53+
* Resets the encoder
54+
*/
55+
public void resetEnc();
56+
4557
}

0 commit comments

Comments
 (0)