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

Commit 1320869

Browse files
DriverStationComputerDriverStationComputer
committed
Merge branch 'lift' of https://github.com/lhmcgann/RobotCode2018.git into lift
2 parents 8c9d053 + d83e7c7 commit 1320869

16 files changed

Lines changed: 577 additions & 397 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(new LiftToPosition(Robot.lift,
108+
SmartDashboard.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: 26 additions & 38 deletions
Original file line numberDiff line numberDiff line change
@@ -59,6 +59,7 @@ public class Robot extends IterativeRobot {
5959
String[] fmsPossibilities = { "LL", "LR", "RL", "RR" };
6060

6161
public static SmartDashboardInterface sd = new SmartDashboardInterface() {
62+
@Override
6263
public double getConst(String key, double def) {
6364
Preferences pref = Preferences.getInstance();
6465
if (!pref.containsKey("Const/" + key)) {
@@ -71,6 +72,20 @@ public double getConst(String key, double def) {
7172
return pref.getDouble("Const/" + key, def);
7273
}
7374

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+
88+
@Override
7489
public void putConst(String key, double def) {
7590
Preferences pref = Preferences.getInstance();
7691
pref.putDouble("Const/" + key, def);
@@ -79,14 +94,17 @@ public void putConst(String key, double def) {
7994
}
8095
}
8196

97+
@Override
8298
public void putData(String string, PIDController controller) {
8399
SmartDashboard.putData(string, controller);
84100
}
85101

102+
@Override
86103
public void putNumber(String string, double d) {
87104
SmartDashboard.putNumber(string, d);
88105
}
89106

107+
@Override
90108
public void putBoolean(String string, boolean b) {
91109
SmartDashboard.putBoolean(string, b);
92110
}
@@ -104,6 +122,10 @@ public static double getConst(String key, double def) {
104122
return sd.getConst(key, def);
105123
}
106124

125+
public static String getString(String key, String def) {
126+
return sd.getString(key, def);
127+
}
128+
107129
public static void putConst(String key, double def) {
108130
sd.putConst(key, def);
109131
}
@@ -184,6 +206,7 @@ public void disabledPeriodic() {
184206
@Override
185207
public void autonomousInit() {
186208
dt.resetAHRS();
209+
lift.resetEnc();
187210
AutoUtils.state = new State(0, 0, 0);
188211
Scheduler.getInstance().add(new ShiftLowGear());
189212
Scheduler.getInstance().add(new CloseIntake());
@@ -267,43 +290,8 @@ public void testInit() {
267290
*/
268291
@Override
269292
public void testPeriodic() {
270-
// if(firstTime) {
271-
// Robot.dt.enableVelocityPIDs();
272-
// firstTime = false;
273-
//// }
274-
// dt.getLeftVPID().setConsts(getConst("VelocityLeftkI", 0), 0,
275-
// getConst("VelocityLeftkD", rmap.calcDefkD(dt.getCurrentMaxSpeed())),
276-
// /* 1 / dt.getCurrentMaxSpeed() */Robot.getConst("VelocityLeftkF",
277-
// 1 / Robot.getConst("Max Low Speed", 84)));
278-
// dt.getRightVPID().setConsts(getConst("VelocityRightkI", 0), 0,
279-
// getConst("VelocityRightkD", rmap.calcDefkD(dt.getCurrentMaxSpeed())),
280-
// /* 1 / dt.getCurrentMaxSpeed() */Robot.getConst("VelocityRightkF",
281-
// 1 / Robot.getConst("Max Low Speed", 84)));
282-
// dt.resetAllVelocityPIDConsts();
283-
284-
// dt.setVPIDs(getConst("VPID Test Set", 0.5));
285-
286-
Scheduler.getInstance().run();
287-
288-
// System.out.println("Left VPID Targ: " + dt.getLeftVPIDOutput());
289-
// System.out.println("Right VPID Targ: " + dt.getRightVPIDOutput());
290-
// System.out.println("Left VPID Error: " + dt.getLeftVPIDerror());
291-
// System.out.println("Right VPID Error: " + dt.getRightVPIDerror());
292-
// System.out.println("Left Enc Rate: " + dt.getLeftEncRate());
293-
// System.out.println("Right Enc Rate: " + dt.getRightEncRate());
294-
//
295-
// System.out.println("Left Talon Speed: " + rmap.dtLeftMaster.get());
296-
// System.out.println("Right Talon Speed: " + rmap.dtRightMaster.get());
297-
298-
// System.out.println("Left Enc Dist " + dt.getLeftDist());
299-
// System.out.println("Right Enc Dist " + dt.getRightDist());
300-
// System.out.println("Avg Enc Dist" + dt.getEncAvgDist());
301-
302-
// dt.dtLeft.set(0.1);
303-
// dt.dtRight.set(-oi.rightJoy.getY());
304-
// dt.dtLeft.set(-oi.leftJoy.getY());
305-
// dt.dtRight.set(-oi.rightJoy.getY());
306-
307-
// dt.putVelocityControllersToDashboard();
293+
// Scheduler.getInstance().run();
294+
// lift.disable();
295+
// lift.runMotor(SmartDashboard.getNumber("Voltage to Lift", 0));
308296
}
309297
}

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

Lines changed: 16 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -43,7 +43,7 @@ public class RobotMap {
4343
public static DigitalSource liftEncPort1;
4444
public static DigitalSource liftEncPort2;
4545

46-
//public static WPI_TalonSRX climberMotor;
46+
// public static WPI_TalonSRX climberMotor;
4747

4848
public static VictorSP leftIntakeMotor;
4949
public static VictorSP rightIntakeMotor;
@@ -123,7 +123,7 @@ private void configSPX(WPI_VictorSPX mc) {
123123

124124
public RobotMap() {
125125
pdp = new PowerDistributionPanel();
126-
126+
127127
liftMotorA = new WPI_VictorSPX(getPort("LiftVictorSPX", 5));
128128
configSPX(liftMotorA);
129129
liftMotorB = new WPI_TalonSRX(getPort("1LiftTalonSRX", 6));
@@ -252,7 +252,10 @@ public double getRadius() {
252252
return Robot.getConst("Radius of Drivetrain Wheel", 0.0635);
253253
}
254254

255-
public double getOmegaMax() {
255+
/**
256+
* @return the RPM max of a CIM
257+
*/
258+
public static double getOmegaMax() {
256259
return Robot.getConst("Omega Max", 5330);
257260
}
258261

@@ -264,7 +267,10 @@ public double getCycleTime() {
264267
return Robot.getConst("Code cycle time", 0.05);
265268
}
266269

267-
public double getStallTorque() {
270+
/**
271+
* @return the stall torque of a CIM
272+
*/
273+
public static double getStallTorque() {
268274
return Robot.getConst("Stall Torque", 2.41);
269275
}
270276

@@ -273,7 +279,12 @@ private double convertLbsTokG(double lbs) {
273279
return lbs * 0.45359237;
274280
}
275281

276-
private double convertNtokG(double newtons) {
282+
/**
283+
* @param the
284+
* weight (in Newtons)
285+
* @return the equivalent mass (in kg)
286+
*/
287+
public static double convertNtokG(double newtons) {
277288
// weight / accel due to grav = kg
278289
return newtons / 9.81;
279290
}

‎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: 0 additions & 114 deletions
This file was deleted.

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

Lines changed: 6 additions & 26 deletions
Original file line numberDiff line numberDiff line change
@@ -1,36 +1,16 @@
11
package org.usfirst.frc.team199.Robot2018.commands;
22

3-
import edu.wpi.first.wpilibj.command.Command;
3+
import org.usfirst.frc.team199.Robot2018.Robot;
4+
5+
import edu.wpi.first.wpilibj.command.CommandGroup;
46

57
/**
68
*
79
*/
8-
public class EjectToExchange extends Command {
10+
public class EjectToExchange extends CommandGroup {
911

1012
public EjectToExchange() {
11-
// Use requires() here to declare subsystem dependencies
12-
// eg. requires(chassis);
13-
}
14-
15-
// Called just before this Command runs the first time
16-
protected void initialize() {
17-
}
18-
19-
// Called repeatedly when this Command is scheduled to run
20-
protected void execute() {
21-
}
22-
23-
// Make this return true when this Command no longer needs to run execute()
24-
protected boolean isFinished() {
25-
return false;
26-
}
27-
28-
// Called once after isFinished returns true
29-
protected void end() {
30-
}
31-
32-
// Called when another command which requires one or more of the same
33-
// subsystems is scheduled to run
34-
protected void interrupted() {
13+
addSequential(new LiftToPosition(Robot.lift, "GROUND"));
14+
addSequential(new OuttakeCube());
3515
}
3616
}

0 commit comments

Comments
 (0)