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

Commit a9fdf4d

Browse files
DriverStationComputerDriverStationComputer
committed
lift and intake work as expected :)
1 parent cd525b2 commit a9fdf4d

7 files changed

Lines changed: 133 additions & 31 deletions

File tree

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

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -191,6 +191,7 @@ public void robotInit() {
191191
@Override
192192
public void disabledInit() {
193193
dt.disableVelocityPIDs();
194+
lift.setSetpoint(0);
194195
}
195196

196197
@Override
@@ -283,6 +284,7 @@ public void testInit() {
283284
System.out.println("In testInit()");
284285
dt.resetAHRS();
285286
AutoUtils.state = new State(0, 0, 0);
287+
lift.disable();
286288
}
287289

288290
/**

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

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -134,6 +134,7 @@ public RobotMap() {
134134
// liftMotors = new SpeedControllerGroup(liftMotorB, liftMotorA, liftMotorC);
135135
liftMotors = new SpeedControllerGroup(liftMotorC);
136136
liftMotors.setName("Lift", "CIM Motor");
137+
liftMotors.setInverted(true);
137138
LiveWindow.add(liftMotors);
138139

139140
liftEncPort1 = new DigitalInput(getPort("1LiftEnc", 4));

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

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -26,14 +26,14 @@ public MoveLiftWithPID(Lift lift, boolean up) {
2626
// Called just before this Command runs the first time
2727
@Override
2828
protected void initialize() {
29-
setpoint = 0;
30-
lift.setSetpoint(setpoint);
29+
// setpoint = lift.getPIDController().getSetpoint();
3130
}
3231

3332
// Called repeatedly when this Command is scheduled to run
3433
@Override
3534
protected void execute() {
36-
setpoint += dir * Robot.getConst("Lift Move Increment", 1); // inches
35+
setpoint = lift.getPIDController().getSetpoint() + dir * Robot.getConst("Lift Move Increment", 0.25); // inches/0.05
36+
// secs
3737
lift.setSetpoint(setpoint);
3838
}
3939

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

Lines changed: 31 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -15,7 +15,8 @@ public class UpdateLiftPosition extends Command {
1515
private double desiredDist = 0;
1616
private LiftHeight desiredPos;
1717

18-
private boolean manipulatorPluggedIn;
18+
private boolean manipulatorPluggedIn = true;
19+
private boolean goToGround = false;
1920

2021
public UpdateLiftPosition(Lift lift) {
2122
requires(Robot.lift);
@@ -39,18 +40,40 @@ protected void execute() {
3940
if (manipulatorPluggedIn) {
4041
int angle = Robot.oi.manipulator.getPOV();
4142

43+
System.out.println("POV Reading: " + angle);
44+
4245
// 180 degrees is down and -1 is "nothing," so basically default to GROUND,
4346
// anything else is SWITCH
44-
if (angle != 180 && angle != -1) {
47+
// if (angle != 180 && angle != -1) {
48+
// desiredPos = LiftHeight.SWITCH;
49+
// // NOTE: if full lift functionality does become a thing, need to add a couple
50+
// // more if-elses here to account for those enum values
51+
// } else {
52+
// desiredPos = LiftHeight.GROUND;
53+
// }
54+
55+
if (angle == 180) {
56+
desiredPos = LiftHeight.HOLD_CUBE;
57+
goToGround = true;
58+
} else if (angle == 270) {
59+
desiredPos = LiftHeight.HOLD_CUBE;
60+
goToGround = false;
61+
} else if (angle != -1) {
4562
desiredPos = LiftHeight.SWITCH;
46-
// NOTE: if full lift functionality does become a thing, need to add a couple
47-
// more if-elses here to account for those enum values
48-
} else {
49-
desiredPos = LiftHeight.GROUND;
63+
goToGround = false;
5064
}
5165

52-
desiredDist = lift.getDesiredDistFromPos(desiredPos);
53-
lift.setSetpoint(desiredDist);
66+
if (angle != -1) {
67+
desiredDist = lift.getDesiredDistFromPos(desiredPos);
68+
lift.setSetpoint(desiredDist);
69+
}
70+
71+
if (goToGround && lift.onTarget() && lift.getSpeed() <= 0.1) {
72+
desiredPos = LiftHeight.GROUND;
73+
desiredDist = lift.getDesiredDistFromPos(desiredPos);
74+
lift.setSetpoint(desiredDist);
75+
goToGround = false;
76+
}
5477
}
5578
}
5679

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

Lines changed: 47 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -29,19 +29,28 @@ public Lift() {
2929

3030
double maxSpeed = getLiftMaxSpeed();
3131

32-
double r = Robot.getConst("LiftPidR", 3.0);
32+
System.out.println("Lift Time Const: " + getLiftTimeConstant());
33+
34+
double r = Robot.getConst("LiftPidR", 0.3);
3335
double kP = r / getLiftTimeConstant() / maxSpeed;
3436
double kI = 0;
3537
double kD = r / maxSpeed;
3638
double kF = 1 / (maxSpeed * Robot.getConst("Default PID Update Time", 0.05));
3739

3840
PIDController liftController = getPIDController();
3941

40-
liftController.setPID(Robot.getConst("LiftkP", kP), Robot.getConst("LiftkI", kI), Robot.getConst("LiftkD", kD),
41-
Robot.getConst("LiftkF", kF));
42+
// liftController.setPID(Robot.getConst("LiftkP", kP), Robot.getConst("LiftkI",
43+
// kI), Robot.getConst("LiftkD", kD),
44+
// Robot.getConst("LiftkF", kF));
45+
liftController.setPID(kP, kI, kD, kF);
4246

4347
setInputRange(0, Robot.getConst("Lift Max Height", 24));
4448
setOutputRange(-1, 1);
49+
setAbsoluteTolerance(Robot.getConst("Lift Tolerance", 0.8));
50+
51+
// dpp = (pitch circumference of sprocket) / (pulses per rev of output shaft)
52+
double dpp = 2 * Math.PI * getLiftRadius() / 2048;
53+
liftEnc.setDistancePerPulse(dpp);
4554

4655
NUM_STAGES = (int) Robot.getConst("Lift stages", 1);
4756
WIGGLE_ROOM = (int) Robot.getConst("Lift wiggle room", 3.0); // inches
@@ -71,6 +80,9 @@ public double getDesiredDistFromPos(LiftHeight pos) {
7180
case GROUND:
7281
desiredDist = 0;
7382
break;
83+
case HOLD_CUBE:
84+
desiredDist = 4;
85+
break;
7486
case SWITCH:
7587
desiredDist = SWITCH_DIST;
7688
break;
@@ -88,6 +100,13 @@ public double getDesiredDistFromPos(LiftHeight pos) {
88100
return desiredDist;
89101
}
90102

103+
/**
104+
* @return the rate of the lift encoder (in/s)
105+
*/
106+
public double getSpeed() {
107+
return liftEnc.getRate();
108+
}
109+
91110
/**
92111
* Sets the current position in the lift subsystem
93112
*
@@ -124,14 +143,19 @@ public double getLiftSpeed() {
124143
}
125144

126145
/**
127-
* Runs lift motors at specified speed
146+
* Sends specific voltage to lift motor, clamped at max voltage
128147
*
129-
* @param speed
130-
* - desired speed to run at [-1, 1]
148+
* @param output
149+
* - desired voltage to set motor to [-1, 1]
131150
*/
132151
@Override
133152
public void runMotor(double output) {
134-
liftMotors.set(output);
153+
double absMax = Robot.getConst("Lift Max Voltage", 0.5);
154+
double out = output;
155+
if (Math.abs(out) > absMax) {
156+
out = Math.signum(out) * absMax;
157+
}
158+
liftMotors.set(out);
135159
}
136160

137161
/**
@@ -169,11 +193,11 @@ public double getWiggleRoom() {
169193
}
170194

171195
/**
172-
* @return the max speed of the lift
196+
* @return the max speed of the lift (in/s)
173197
*/
174198
public double getLiftMaxSpeed() {
175-
/** @todo find lift max speed and set default below */
176-
return Robot.getConst("Lift Max Speed", 0);
199+
double maxSpd = RobotMap.getOmegaMax() / getLiftGearRatio() / 60 * 2 * Math.PI * getLiftRadius();
200+
return Robot.getConst("Lift Max Speed", maxSpd);
177201
}
178202

179203
/**
@@ -194,7 +218,7 @@ protected double returnPIDInput() {
194218
protected void usePIDOutput(double output) {
195219
double out = output;
196220
double spd = liftEnc.getRate();
197-
out += Robot.getConst("Lift: Necessary Voltage", 0);
221+
out += Robot.getConst("Lift: Necessary Voltage", 0.125);
198222
runMotor(out);
199223
}
200224

@@ -206,13 +230,22 @@ protected void usePIDOutput(double output) {
206230
*/
207231
public double getLiftTimeConstant() {
208232
double gearReduction = getLiftGearRatio();
209-
double radius = getLiftRadius();
233+
double radius = convertInToM(getLiftRadius());
210234
double timeConstant = RobotMap.getOmegaMax() / gearReduction / 60 * 2 * Math.PI
211235
* RobotMap.convertNtokG(getLiftedWeight()) * radius * radius
212236
/ (RobotMap.getStallTorque() * gearReduction);
213237
return timeConstant;
214238
}
215239

240+
/**
241+
* @param inches
242+
* - an amount in inches
243+
* @return the equivalent amount in meters
244+
*/
245+
public double convertInToM(double inches) {
246+
return inches * 2.54 / 100;
247+
}
248+
216249
/**
217250
* @return the gear ratio of the lift gearbox
218251
*/
@@ -221,7 +254,7 @@ public double getLiftGearRatio() {
221254
}
222255

223256
/**
224-
* @return the lift sprocket's radius
257+
* @return the lift sprocket's radius (inches)
225258
*/
226259
public double getLiftRadius() {
227260
// 15 tooth, #35 sprocket
@@ -233,6 +266,6 @@ public double getLiftRadius() {
233266
* lift components, NOT cube
234267
*/
235268
public double getLiftedWeight() {
236-
return Robot.getConst("Weight of Lifted Stuff", 53.11);
269+
return Robot.getConst("Weight of Lifted Stuff", 62.91);
237270
}
238271
}

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

Lines changed: 4 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -8,14 +8,17 @@ public interface LiftInterface {
88
public void initDefaultCommand();
99

1010
public enum LiftHeight {
11-
GROUND, SWITCH, SCALE, BAR;
11+
GROUND, HOLD_CUBE, SWITCH, SCALE, BAR;
1212

1313
public static LiftHeight toLH(String str) {
1414
LiftHeight lh = null;
1515
switch (str) {
1616
case "GROUND":
1717
lh = GROUND;
1818
break;
19+
case "HOLD_CUBE":
20+
lh = HOLD_CUBE;
21+
break;
1922
case "SWITCH":
2023
lh = SWITCH;
2124
break;

‎shuffleboard.json‎

Lines changed: 45 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -88,7 +88,8 @@
8888
"_type": "Graph",
8989
"_source0": "network_table:///SmartDashboard/Drivetrain/Left VPID Targ",
9090
"_title": "/SmartDashboard/Drivetrain/Left VPID Targ",
91-
"Visible time": 30.0
91+
"Visible time": 30.0,
92+
"SmartDashboard/Drivetrain/Left VPID Targ visible": true
9293
}
9394
},
9495
"4,0": {
@@ -100,7 +101,8 @@
100101
"_type": "Graph",
101102
"_source0": "network_table:///SmartDashboard/Drivetrain/Right VPID Targ",
102103
"_title": "/SmartDashboard/Drivetrain/Right VPID Targ",
103-
"Visible time": 30.0
104+
"Visible time": 30.0,
105+
"SmartDashboard/Drivetrain/Right VPID Targ visible": true
104106
}
105107
},
106108
"4,3": {
@@ -384,7 +386,8 @@
384386
"_type": "Graph",
385387
"_source0": "network_table:///SmartDashboard/Drivetrain/Left VPID Targ",
386388
"_title": "/SmartDashboard/Drivetrain/Left VPID Targ",
387-
"Visible time": 10.0
389+
"Visible time": 10.0,
390+
"SmartDashboard/Drivetrain/Left VPID Targ visible": true
388391
}
389392
},
390393
"5,2": {
@@ -396,7 +399,8 @@
396399
"_type": "Graph",
397400
"_source0": "network_table:///SmartDashboard/Drivetrain/Right VPID Targ",
398401
"_title": "/SmartDashboard/Drivetrain/Right VPID Targ",
399-
"Visible time": 10.0
402+
"Visible time": 10.0,
403+
"SmartDashboard/Drivetrain/Right VPID Targ visible": true
400404
}
401405
},
402406
"5,1": {
@@ -947,7 +951,43 @@
947951
"showGrid": true,
948952
"hgap": 16.0,
949953
"vgap": 16.0,
950-
"tiles": {}
954+
"tiles": {
955+
"0,1": {
956+
"size": [
957+
2,
958+
1
959+
],
960+
"content": {
961+
"_type": "Encoder",
962+
"_source0": "network_table:///LiveWindow/Lift/Encoder",
963+
"_title": "LiveWindow/Lift/Encoder"
964+
}
965+
},
966+
"0,0": {
967+
"size": [
968+
2,
969+
1
970+
],
971+
"content": {
972+
"_type": "Speed Controller",
973+
"_source0": "network_table:///LiveWindow/Lift/CIM Motor",
974+
"_title": "LiveWindow/Lift/CIM Motor",
975+
"controllable": false,
976+
"orientation": "HORIZONTAL"
977+
}
978+
},
979+
"0,2": {
980+
"size": [
981+
6,
982+
2
983+
],
984+
"content": {
985+
"_type": "Network Table Tree",
986+
"_source0": "network_table:///LiveWindow/Ungrouped/Scheduler",
987+
"_title": "LiveWindow/Ungrouped/Scheduler"
988+
}
989+
}
990+
}
951991
}
952992
}
953993
]

0 commit comments

Comments
 (0)