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

Commit e1281cc

Browse files
kevinzwangkevinzwang
committed
Merge branch 'master' of https://github.com/DriverStationComputer/RobotCode2018 into better-auto
2 parents 92ecf84 + e7b22d9 commit e1281cc

25 files changed

Lines changed: 914 additions & 1380 deletions

‎CompSD.xml‎

Lines changed: 72 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,72 @@
1+
<xml version="1.0">
2+
<dashboard>
3+
<widget field="High Gear" type="Boolean" class="edu.wpi.first.smartdashboard.gui.elements.BooleanBox">
4+
<location x="14" y="4"/>
5+
<width>93</width>
6+
<height>78</height>
7+
</widget>
8+
<static-widget class="edu.wpi.first.smartdashboard.gui.elements.CameraServerViewer">
9+
<location x="830" y="4"/>
10+
<width>529</width>
11+
<height>399</height>
12+
<property name="Camera Choice" value="USB Camera 1"/>
13+
</static-widget>
14+
<static-widget class="edu.wpi.first.smartdashboard.gui.elements.CameraServerViewer">
15+
<location x="324" y="17"/>
16+
<width>502</width>
17+
<height>383</height>
18+
<property name="Camera Choice" value=""/>
19+
</static-widget>
20+
<widget field="Turn PID" type="PIDController" class="edu.wpi.first.smartdashboard.gui.elements.PIDEditor">
21+
<location x="182" y="426"/>
22+
</widget>
23+
<static-widget class="edu.wpi.first.smartdashboard.gui.elements.RobotPreferences">
24+
<location x="6" y="87"/>
25+
<width>284</width>
26+
<height>205</height>
27+
</static-widget>
28+
<widget field="LR" type="String Chooser" class="edu.wpi.first.smartdashboard.gui.elements.Chooser">
29+
<location x="31" y="2"/>
30+
</widget>
31+
<widget field="LL" type="String Chooser" class="edu.wpi.first.smartdashboard.gui.elements.Chooser">
32+
<location x="16" y="12"/>
33+
</widget>
34+
<widget field="Left PID Controller" type="PIDController" class="edu.wpi.first.smartdashboard.gui.elements.PIDEditor">
35+
<location x="51" y="417"/>
36+
</widget>
37+
<widget field="PIDController[1]" type="PIDController" class="edu.wpi.first.smartdashboard.gui.elements.PIDEditor">
38+
<location x="134" y="229"/>
39+
</widget>
40+
<widget field="RR" type="String Chooser" class="edu.wpi.first.smartdashboard.gui.elements.Chooser">
41+
<location x="4" y="11"/>
42+
</widget>
43+
<widget field="Starting Position" type="String Chooser" class="edu.wpi.first.smartdashboard.gui.elements.Chooser">
44+
<location x="11" y="16"/>
45+
</widget>
46+
<widget field="Move PID" type="PIDController" class="edu.wpi.first.smartdashboard.gui.elements.PIDEditor">
47+
<location x="32" y="467"/>
48+
</widget>
49+
<widget field="Right PID Controller" type="PIDController" class="edu.wpi.first.smartdashboard.gui.elements.PIDEditor">
50+
<location x="-26" y="264"/>
51+
<width>159</width>
52+
</widget>
53+
<widget field="RL" type="String Chooser" class="edu.wpi.first.smartdashboard.gui.elements.Chooser">
54+
<location x="13" y="30"/>
55+
</widget>
56+
</dashboard>
57+
<live-window>
58+
</live-window>
59+
<hidden field="VPID Right kI"/>
60+
<hidden field="VPID Left kI"/>
61+
<hidden field="Turn PID Error"/>
62+
<hidden field="Auto Delay"/>
63+
<hidden field="Move PID Error"/>
64+
<hidden field="Angle"/>
65+
<hidden field="Turn PID Result"/>
66+
<hidden field="VPID kF"/>
67+
<hidden field="Move PID Output"/>
68+
<hidden field="VPID Right kP"/>
69+
<hidden field="VPID Left kP"/>
70+
<hidden field="Turn PID Output"/>
71+
<hidden field="Move PID Result"/>
72+
</xml>

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

Lines changed: 23 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -7,22 +7,26 @@
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;
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;
25+
import org.usfirst.frc.team199.Robot2018.commands.StopIntake;
2326
import org.usfirst.frc.team199.Robot2018.commands.ToggleLeftIntake;
2427
import org.usfirst.frc.team199.Robot2018.commands.ToggleRightIntake;
2528
import org.usfirst.frc.team199.Robot2018.commands.UpdatePIDConstants;
29+
import org.usfirst.frc.team199.Robot2018.subsystems.LiftInterface.LiftHeight;
2630

2731
import edu.wpi.first.wpilibj.Joystick;
2832
import edu.wpi.first.wpilibj.buttons.JoystickButton;
@@ -48,6 +52,9 @@ public class OI {
4852
private JoystickButton resetEncButton;
4953
private JoystickButton moveLiftUpButton;
5054
private JoystickButton moveLiftDownButton;
55+
private JoystickButton moveLiftPIDUpButton;
56+
private JoystickButton moveLiftPIDDownButton;
57+
private JoystickButton testLiftPID;
5158
private JoystickButton findTurnTimeConstantButton;
5259
private JoystickButton updatePIDConstantsButton;
5360
private JoystickButton updateEncoderDPPButton;
@@ -61,6 +68,7 @@ public class OI {
6168
private JoystickButton outakeCubeButton;
6269
private JoystickButton toggleLeftIntakeButton;
6370
private JoystickButton toggleRightIntakeButton;
71+
private JoystickButton stopIntakeButton;
6472

6573
public int getButton(String key, int def) {
6674
if (!SmartDashboard.containsKey("Button/" + key)) {
@@ -92,6 +100,15 @@ public OI(Robot robot) {
92100
findTurnTimeConstantButton
93101
.whenPressed(new FindTurnTimeConstant(robot, Robot.dt, Robot.rmap.fancyGyro, Robot.sd));
94102

103+
moveLiftPIDUpButton = new JoystickButton(leftJoy, getButton("Run Lift PID Up", 3));
104+
moveLiftPIDUpButton.whileHeld(new MoveLiftWithPID(Robot.lift, true));
105+
moveLiftPIDDownButton = new JoystickButton(leftJoy, getButton("Run Lift PID Down", 4));
106+
moveLiftPIDDownButton.whileHeld(new MoveLiftWithPID(Robot.lift, false));
107+
108+
testLiftPID = new JoystickButton(leftJoy, getButton("Test Lift PID", 5));
109+
testLiftPID.whenPressed(
110+
new AutoLift(Robot.lift, SmartDashboard.getString("Lift Targ Height", LiftHeight.SWITCH.toString())));
111+
95112
rightJoy = new Joystick(1);
96113
shiftHighGearButton = new JoystickButton(rightJoy, getButton("Shift High Gear", 4));
97114
shiftHighGearButton.whenPressed(new ShiftHighGear());
@@ -104,9 +121,12 @@ public OI(Robot robot) {
104121
updateEncoderDPPButton.whenPressed(new SetDistancePerPulse());
105122

106123
moveLiftUpButton = new JoystickButton(rightJoy, getButton("Run Lift Motor Up", 10));
107-
moveLiftUpButton.whileHeld(new RunLift(Robot.lift, true));
124+
moveLiftUpButton.whileHeld(new MoveLift(Robot.lift, true));
108125
moveLiftDownButton = new JoystickButton(rightJoy, getButton("Run Lift Motor Down", 11));
109-
moveLiftDownButton.whileHeld(new RunLift(Robot.lift, false));
126+
moveLiftDownButton.whileHeld(new MoveLift(Robot.lift, false));
127+
128+
stopIntakeButton = new JoystickButton(rightJoy, getButton("Stop Intake Button", 6));
129+
stopIntakeButton.whenPressed(new StopIntake());
110130

111131
manipulator = new Joystick(2);
112132
if (manipulator.getButtonCount() == 0) {

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

Lines changed: 33 additions & 41 deletions
Original file line numberDiff line numberDiff line change
@@ -23,6 +23,7 @@
2323
import org.usfirst.frc.team199.Robot2018.subsystems.IntakeEject;
2424
import org.usfirst.frc.team199.Robot2018.subsystems.Lift;
2525

26+
import edu.wpi.first.wpilibj.CameraServer;
2627
import edu.wpi.first.wpilibj.DriverStation;
2728
import edu.wpi.first.wpilibj.IterativeRobot;
2829
import edu.wpi.first.wpilibj.PIDController;
@@ -59,6 +60,7 @@ public class Robot extends IterativeRobot {
5960
String[] fmsPossibilities = { "LL", "LR", "RL", "RR" };
6061

6162
public static SmartDashboardInterface sd = new SmartDashboardInterface() {
63+
@Override
6264
public double getConst(String key, double def) {
6365
Preferences pref = Preferences.getInstance();
6466
if (!pref.containsKey("Const/" + key)) {
@@ -71,6 +73,20 @@ public double getConst(String key, double def) {
7173
return pref.getDouble("Const/" + key, def);
7274
}
7375

76+
@Override
77+
public String getString(String key, String def) {
78+
Preferences pref = Preferences.getInstance();
79+
if (!pref.containsKey("String/" + key)) {
80+
pref.putString("String/" + key, def);
81+
if (pref.getString("String/ + key", def) != def) {
82+
System.err.println("pref Key" + "String/" + key + "already taken by a different type");
83+
return def;
84+
}
85+
}
86+
return pref.getString("String/" + key, def);
87+
}
88+
89+
@Override
7490
public void putConst(String key, double def) {
7591
Preferences pref = Preferences.getInstance();
7692
pref.putDouble("Const/" + key, def);
@@ -79,14 +95,17 @@ public void putConst(String key, double def) {
7995
}
8096
}
8197

98+
@Override
8299
public void putData(String string, PIDController controller) {
83100
SmartDashboard.putData(string, controller);
84101
}
85102

103+
@Override
86104
public void putNumber(String string, double d) {
87105
SmartDashboard.putNumber(string, d);
88106
}
89107

108+
@Override
90109
public void putBoolean(String string, boolean b) {
91110
SmartDashboard.putBoolean(string, b);
92111
}
@@ -104,6 +123,10 @@ public static double getConst(String key, double def) {
104123
return sd.getConst(key, def);
105124
}
106125

126+
public static String getString(String key, String def) {
127+
return sd.getString(key, def);
128+
}
129+
107130
public static void putConst(String key, double def) {
108131
sd.putConst(key, def);
109132
}
@@ -157,8 +180,9 @@ public void robotInit() {
157180
autoScripts = AutoUtils.parseScriptFile(Preferences.getInstance().getString("autoscripts", ""));
158181

159182
listen = new Listener();
160-
// CameraServer.getInstance().startAutomaticCapture(0);
161-
// CameraServer.getInstance().startAutomaticCapture(1);
183+
lift.resetEnc();
184+
CameraServer.getInstance().startAutomaticCapture(0);
185+
CameraServer.getInstance().startAutomaticCapture(1);
162186
}
163187

164188
/**
@@ -169,6 +193,7 @@ public void robotInit() {
169193
@Override
170194
public void disabledInit() {
171195
dt.disableVelocityPIDs();
196+
lift.setSetpoint(0);
172197
}
173198

174199
@Override
@@ -245,7 +270,8 @@ public void teleopPeriodic() {
245270
// SmartDashboard.putNumber("Avg Enc Dist", dt.getEncAvgDist());
246271
//
247272

248-
System.out.printf("Left: %1$5.2f; Right: %2$5.2f\n", RobotMap.dtLeft.get(), RobotMap.dtRight.get());
273+
// System.out.printf("Left: %1$5.2f; Right: %2$5.2f\n", RobotMap.dtLeft.get(),
274+
// RobotMap.dtRight.get());
249275

250276
SmartDashboard.putNumber("Angle", dt.getAHRSAngle());
251277
SmartDashboard.putNumber("Left Current draw", rmap.pdp.getCurrent(4));
@@ -259,50 +285,16 @@ public void testInit() {
259285
System.out.println("In testInit()");
260286
dt.resetAHRS();
261287
AutoUtils.state = new State(0, 0, 0);
288+
lift.disable();
262289
}
263290

264291
/**
265292
* This function is called periodically during test mode
266293
*/
267294
@Override
268295
public void testPeriodic() {
269-
// if(firstTime) {
270-
// Robot.dt.enableVelocityPIDs();
271-
// firstTime = false;
272-
//// }
273-
// dt.getLeftVPID().setConsts(getConst("VelocityLeftkI", 0), 0,
274-
// getConst("VelocityLeftkD", rmap.calcDefkD(dt.getCurrentMaxSpeed())),
275-
// /* 1 / dt.getCurrentMaxSpeed() */Robot.getConst("VelocityLeftkF",
276-
// 1 / Robot.getConst("Max Low Speed", 84)));
277-
// dt.getRightVPID().setConsts(getConst("VelocityRightkI", 0), 0,
278-
// getConst("VelocityRightkD", rmap.calcDefkD(dt.getCurrentMaxSpeed())),
279-
// /* 1 / dt.getCurrentMaxSpeed() */Robot.getConst("VelocityRightkF",
280-
// 1 / Robot.getConst("Max Low Speed", 84)));
281-
// dt.resetAllVelocityPIDConsts();
282-
283-
// dt.setVPIDs(getConst("VPID Test Set", 0.5));
284-
285-
Scheduler.getInstance().run();
286-
287-
// System.out.println("Left VPID Targ: " + dt.getLeftVPIDOutput());
288-
// System.out.println("Right VPID Targ: " + dt.getRightVPIDOutput());
289-
// System.out.println("Left VPID Error: " + dt.getLeftVPIDerror());
290-
// System.out.println("Right VPID Error: " + dt.getRightVPIDerror());
291-
// System.out.println("Left Enc Rate: " + dt.getLeftEncRate());
292-
// System.out.println("Right Enc Rate: " + dt.getRightEncRate());
293-
//
294-
// System.out.println("Left Talon Speed: " + rmap.dtLeftMaster.get());
295-
// System.out.println("Right Talon Speed: " + rmap.dtRightMaster.get());
296-
297-
// System.out.println("Left Enc Dist " + dt.getLeftDist());
298-
// System.out.println("Right Enc Dist " + dt.getRightDist());
299-
// System.out.println("Avg Enc Dist" + dt.getEncAvgDist());
300-
301-
// dt.dtLeft.set(0.1);
302-
// dt.dtRight.set(-oi.rightJoy.getY());
303-
// dt.dtLeft.set(-oi.leftJoy.getY());
304-
// dt.dtRight.set(-oi.rightJoy.getY());
305-
306-
// dt.putVelocityControllersToDashboard();
296+
// Scheduler.getInstance().run();
297+
// lift.disable();
298+
// lift.runMotor(SmartDashboard.getNumber("Voltage to Lift", 0));
307299
}
308300
}

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

Lines changed: 25 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -23,6 +23,7 @@
2323
import edu.wpi.first.wpilibj.SPI;
2424
import edu.wpi.first.wpilibj.SpeedControllerGroup;
2525
import edu.wpi.first.wpilibj.VictorSP;
26+
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
2627
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
2728

2829
/**
@@ -43,7 +44,7 @@ public class RobotMap {
4344
public static DigitalSource liftEncPort1;
4445
public static DigitalSource liftEncPort2;
4546

46-
//public static WPI_TalonSRX climberMotor;
47+
// public static WPI_TalonSRX climberMotor;
4748

4849
public static VictorSP leftIntakeMotor;
4950
public static VictorSP rightIntakeMotor;
@@ -123,18 +124,25 @@ private void configSPX(WPI_VictorSPX mc) {
123124

124125
public RobotMap() {
125126
pdp = new PowerDistributionPanel();
126-
127+
127128
liftMotorA = new WPI_VictorSPX(getPort("LiftVictorSPX", 5));
128129
configSPX(liftMotorA);
129130
liftMotorB = new WPI_TalonSRX(getPort("1LiftTalonSRX", 6));
130131
configSRX(liftMotorB);
131132
liftMotorC = new WPI_TalonSRX(getPort("2LiftTalonSRX", 7));
132133
configSRX(liftMotorC);
133-
liftMotors = new SpeedControllerGroup(liftMotorB, liftMotorA, liftMotorC);
134+
// liftMotors = new SpeedControllerGroup(liftMotorB, liftMotorA, liftMotorC);
135+
liftMotors = new SpeedControllerGroup(liftMotorC);
136+
liftMotors.setName("Lift", "CIM Motor");
137+
liftMotors.setInverted(true);
138+
LiveWindow.add(liftMotors);
139+
134140
liftEncPort1 = new DigitalInput(getPort("1LiftEnc", 4));
135141
liftEncPort2 = new DigitalInput(getPort("2LiftEnc", 5));
136142
liftEnc = new Encoder(liftEncPort1, liftEncPort2);
137143
liftEnc.setPIDSourceType(PIDSourceType.kDisplacement);
144+
liftEnc.setName("Lift", "Encoder");
145+
LiveWindow.add(liftEnc);
138146

139147
leftIntakeMotor = new VictorSP(getPort("IntakeLeftVictorSP", 8));
140148
rightIntakeMotor = new VictorSP(getPort("IntakeRightVictorSP", 9));
@@ -252,7 +260,10 @@ public double getRadius() {
252260
return Robot.getConst("Radius of Drivetrain Wheel", 0.0635);
253261
}
254262

255-
public double getOmegaMax() {
263+
/**
264+
* @return the RPM max of a CIM
265+
*/
266+
public static double getOmegaMax() {
256267
return Robot.getConst("Omega Max", 5330);
257268
}
258269

@@ -264,7 +275,10 @@ public double getCycleTime() {
264275
return Robot.getConst("Code cycle time", 0.05);
265276
}
266277

267-
public double getStallTorque() {
278+
/**
279+
* @return the stall torque of a CIM
280+
*/
281+
public static double getStallTorque() {
268282
return Robot.getConst("Stall Torque", 2.41);
269283
}
270284

@@ -273,7 +287,12 @@ private double convertLbsTokG(double lbs) {
273287
return lbs * 0.45359237;
274288
}
275289

276-
private double convertNtokG(double newtons) {
290+
/**
291+
* @param the
292+
* weight (in Newtons)
293+
* @return the equivalent mass (in kg)
294+
*/
295+
public static double convertNtokG(double newtons) {
277296
// weight / accel due to grav = kg
278297
return newtons / 9.81;
279298
}

‎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
}

0 commit comments

Comments
 (0)