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

Commit cf2a53a

Browse files
kevinzwangkevinzwang
committed
Merge branch 'lift' of https://github.com/lhmcgann/RobotCode2018 into lift-auto
2 parents 6d0f590 + 421d2d6 commit cf2a53a

28 files changed

Lines changed: 1392 additions & 428 deletions

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

Lines changed: 28 additions & 8 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;
@@ -76,38 +82,51 @@ public OI(Robot robot) {
7682
leftJoy = new Joystick(0);
7783
shiftDriveTypeButton = new JoystickButton(leftJoy, getButton("Shift Drive Type", 2));
7884
shiftDriveTypeButton.whenPressed(new ShiftDriveType());
85+
7986
pIDMoveButton = new JoystickButton(leftJoy, getButton("PID Move", 7));
8087
pIDMoveButton
8188
.whenPressed(new PIDMove(Robot.sd.getConst("Move Targ", 24), Robot.dt, Robot.sd, RobotMap.distEncAvg));
8289
pIDTurnButton = new JoystickButton(leftJoy, getButton("PID Turn", 8));
83-
// PIDTurnButton.whenPressed(new PIDTurn(Robot.getConst("Turn Targ", 90),
84-
// Robot.dt, Robot.sd RobotMap.fancyGyro));
8590
pIDTurnButton
8691
.whenReleased(new PIDTurn(Robot.getConst("Turn Targ", 90), Robot.dt, Robot.sd, RobotMap.fancyGyro));
92+
8793
resetEncButton = new JoystickButton(leftJoy, getButton("Reset Dist Enc", 10));
8894
resetEncButton.whenPressed(new ResetEncoders());
95+
8996
findTurnTimeConstantButton = new JoystickButton(leftJoy, getButton("Find Turn Time Constant", 11));
9097
// the command will only run in test mode
9198
findTurnTimeConstantButton
9299
.whenPressed(new FindTurnTimeConstant(robot, Robot.dt, Robot.rmap.fancyGyro, Robot.sd));
93100

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+
94110
rightJoy = new Joystick(1);
95-
shiftHighGearButton = new JoystickButton(rightJoy, getButton("Shift High Gear", 3));
111+
shiftHighGearButton = new JoystickButton(rightJoy, getButton("Shift High Gear", 4));
96112
shiftHighGearButton.whenPressed(new ShiftHighGear());
97-
shiftLowGearButton = new JoystickButton(rightJoy, getButton("Shift Low Gear", 2));
113+
shiftLowGearButton = new JoystickButton(rightJoy, getButton("Shift Low Gear", 3));
98114
shiftLowGearButton.whenPressed(new ShiftLowGear());
115+
99116
updatePIDConstantsButton = new JoystickButton(rightJoy, getButton("Get PID Constants", 8));
100117
updatePIDConstantsButton.whenPressed(new UpdatePIDConstants());
101118
updateEncoderDPPButton = new JoystickButton(rightJoy, getButton("Get Encoder Dist Per Pulse", 9));
102119
updateEncoderDPPButton.whenPressed(new SetDistancePerPulse());
120+
103121
moveLiftUpButton = new JoystickButton(rightJoy, getButton("Run Lift Motor Up", 10));
122+
moveLiftUpButton.whileHeld(new MoveLift(Robot.lift, true));
104123
moveLiftDownButton = new JoystickButton(rightJoy, getButton("Run Lift Motor Down", 11));
105-
moveLiftUpButton.whileHeld(new RunLift(Robot.lift, true));
106-
moveLiftDownButton.whileHeld(new RunLift(Robot.lift, false));
124+
moveLiftDownButton.whileHeld(new MoveLift(Robot.lift, false));
107125

108126
manipulator = new Joystick(2);
109127
if (manipulator.getButtonCount() == 0) {
110-
System.out.println("Manipulator not plugged in!");
128+
System.err.println(
129+
"ERROR: manipulator does not appear to be plugged in. Disabling intake code. Restart code with manipulator plugged in to enable intake code");
111130
} else {
112131
closeIntakeButton = new JoystickButton(manipulator, getButton("Close Intake Button", 1));
113132
closeIntakeButton.whenPressed(new CloseIntake());
@@ -128,6 +147,7 @@ public OI(Robot robot) {
128147
toggleRightIntakeButton = new JoystickButton(manipulator, getButton("Toggle Right Intake Button", 4));
129148
toggleRightIntakeButton.whenPressed(new ToggleRightIntake());
130149
}
150+
131151
}
132152

133153
// /**

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

Lines changed: 22 additions & 0 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
}

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

Lines changed: 24 additions & 21 deletions
Original file line numberDiff line numberDiff line change
@@ -9,6 +9,7 @@
99

1010
import org.usfirst.frc.team199.Robot2018.autonomous.PIDSourceAverage;
1111

12+
import com.ctre.phoenix.motorcontrol.NeutralMode;
1213
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
1314
import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX;
1415
import com.kauailabs.navx.frc.AHRS;
@@ -34,19 +35,20 @@ public class RobotMap {
3435

3536
public static PowerDistributionPanel pdp;
3637

37-
public static WPI_TalonSRX liftMotor;
38+
public static WPI_VictorSPX liftMotorA;
39+
public static WPI_TalonSRX liftMotorB;
40+
public static WPI_TalonSRX liftMotorC;
41+
public static SpeedControllerGroup liftMotors;
3842
public static Encoder liftEnc;
3943
public static DigitalSource liftEncPort1;
4044
public static DigitalSource liftEncPort2;
4145

42-
public static WPI_TalonSRX climberMotor;
46+
//public static WPI_TalonSRX climberMotor;
4347

4448
public static VictorSP leftIntakeMotor;
4549
public static VictorSP rightIntakeMotor;
46-
public static DoubleSolenoid leftIntakeVerticalSolenoid;
47-
public static DoubleSolenoid rightIntakeVerticalSolenoid;
48-
public static DoubleSolenoid leftIntakeHorizontalSolenoid;
49-
public static DoubleSolenoid rightIntakeHorizontalSolenoid;
50+
public static DoubleSolenoid leftIntakeSolenoid;
51+
public static DoubleSolenoid rightIntakeSolenoid;
5052

5153
public static DigitalSource leftEncPort1;
5254
public static DigitalSource leftEncPort2;
@@ -95,6 +97,8 @@ private void configSRX(WPI_TalonSRX mc) {
9597
mc.enableCurrentLimit(true);
9698

9799
mc.configNeutralDeadband(Robot.getConst("Motor Deadband", 0.001), kTimeout);
100+
101+
mc.setNeutralMode(NeutralMode.Brake);
98102
}
99103

100104
/**
@@ -113,32 +117,31 @@ private void configSPX(WPI_VictorSPX mc) {
113117
mc.configPeakOutputReverse(-1, kTimeout);
114118

115119
mc.configNeutralDeadband(Robot.getConst("Motor Deadband", 0.001), kTimeout);
120+
121+
mc.setNeutralMode(NeutralMode.Brake);
116122
}
117123

118124
public RobotMap() {
119125
pdp = new PowerDistributionPanel();
120-
121-
liftMotor = new WPI_TalonSRX(getPort("LiftTalonSRX", 7));
122-
configSRX(liftMotor);
126+
127+
liftMotorA = new WPI_VictorSPX(getPort("LiftVictorSPX", 5));
128+
configSPX(liftMotorA);
129+
liftMotorB = new WPI_TalonSRX(getPort("1LiftTalonSRX", 6));
130+
configSRX(liftMotorB);
131+
liftMotorC = new WPI_TalonSRX(getPort("2LiftTalonSRX", 7));
132+
configSRX(liftMotorC);
133+
liftMotors = new SpeedControllerGroup(liftMotorB, liftMotorA, liftMotorC);
123134
liftEncPort1 = new DigitalInput(getPort("1LiftEnc", 4));
124135
liftEncPort2 = new DigitalInput(getPort("2LiftEnc", 5));
125136
liftEnc = new Encoder(liftEncPort1, liftEncPort2);
126137
liftEnc.setPIDSourceType(PIDSourceType.kDisplacement);
127-
climberMotor = new WPI_TalonSRX(getPort("ClimberTalonSRX", 6));
128-
configSRX(climberMotor);
129138

130139
leftIntakeMotor = new VictorSP(getPort("IntakeLeftVictorSP", 8));
131140
rightIntakeMotor = new VictorSP(getPort("IntakeRightVictorSP", 9));
132-
leftIntakeHorizontalSolenoid = new DoubleSolenoid(getPort("IntakeLeftHorizontalSolenoidPort1", 4),
133-
getPort("IntakeLeftHorizontalSolenoidPort2", 5));
134-
rightIntakeHorizontalSolenoid = new DoubleSolenoid(getPort("IntakeRightHorizontalSolenoidPort1", 2),
135-
getPort("IntakeRightHorizontalSolenoidPort2", 3));
136-
// leftIntakeVerticalSolenoid = new
137-
// DoubleSolenoid(getPort("IntakeLeftVerticalSolenoidPort1", 6),
138-
// getPort("IntakeLeftVerticalSolenoidPort2", 7));
139-
// rightIntakeVerticalSolenoid = new
140-
// DoubleSolenoid(getPort("IntakeRightVerticalSolenoidPort1", 8),
141-
// getPort("IntakeRightVerticalSolenoidPort2", 9));
141+
leftIntakeSolenoid = new DoubleSolenoid(getPort("IntakeLeftSolenoidForward", 4),
142+
getPort("IntakeLeftSolenoidReverse", 5));
143+
rightIntakeSolenoid = new DoubleSolenoid(getPort("IntakeRightSolenoidForward", 2),
144+
getPort("IntakeRightSolenoidReverse", 3));
142145

143146
leftEncPort1 = new DigitalInput(getPort("1LeftEnc", 2));
144147
leftEncPort2 = new DigitalInput(getPort("2LeftEnc", 3));

‎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/autonomous/PIDSourceFromTalon.java‎

Lines changed: 47 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,47 @@
1+
package org.usfirst.frc.team199.Robot2018.autonomous;
2+
3+
import com.ctre.phoenix.motorcontrol.ControlMode;
4+
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
5+
6+
import edu.wpi.first.wpilibj.PIDSource;
7+
import edu.wpi.first.wpilibj.PIDSourceType;
8+
9+
public class PIDSourceFromTalon implements PIDSource {
10+
11+
private PIDSourceType type;
12+
private WPI_TalonSRX talon;
13+
14+
public PIDSourceFromTalon(WPI_TalonSRX talon) {
15+
this.talon = talon;
16+
ControlMode mode = talon.getControlMode();
17+
switch (mode.toString()) {
18+
case "Position":
19+
type = PIDSourceType.kDisplacement;
20+
break;
21+
default:
22+
type = PIDSourceType.kRate;
23+
break;
24+
}
25+
}
26+
27+
@Override
28+
public void setPIDSourceType(PIDSourceType pidSource) {
29+
type = pidSource;
30+
}
31+
32+
@Override
33+
public PIDSourceType getPIDSourceType() {
34+
return type;
35+
}
36+
37+
@Override
38+
public double pidGet() {
39+
switch (type.toString()) {
40+
case "kDisplacement":
41+
return talon.getSelectedSensorPosition(0);
42+
default:
43+
return talon.getSelectedSensorVelocity(0);
44+
}
45+
}
46+
47+
}

0 commit comments

Comments
 (0)