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

Commit b2cc9ff

Browse files
dfang314dfang314
committed
fixed merge conflicts, deleted raise/lower intake, and used get for the booleans
2 parents 9b6ad73 + 0cf1806 commit b2cc9ff

30 files changed

+1103
-481
lines changed

‎Robot2018/lib/.gitignore‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1 +1 @@
1-
/User_Libraries.properties
1+
User_Libraries.properties

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

Lines changed: 91 additions & 39 deletions
Original file line numberDiff line numberDiff line change
@@ -8,10 +8,10 @@
88
package org.usfirst.frc.team199.Robot2018;
99

1010
import org.usfirst.frc.team199.Robot2018.commands.CloseIntake;
11+
import org.usfirst.frc.team199.Robot2018.commands.FindTurnTimeConstant;
1112
import org.usfirst.frc.team199.Robot2018.commands.IntakeCube;
12-
import org.usfirst.frc.team199.Robot2018.commands.LowerIntake;
1313
import org.usfirst.frc.team199.Robot2018.commands.OpenIntake;
14-
import org.usfirst.frc.team199.Robot2018.commands.OutakeCube;
14+
import org.usfirst.frc.team199.Robot2018.commands.OuttakeCube;
1515
import org.usfirst.frc.team199.Robot2018.commands.PIDMove;
1616
import org.usfirst.frc.team199.Robot2018.commands.PIDTurn;
1717
import org.usfirst.frc.team199.Robot2018.commands.ResetEncoders;
@@ -39,26 +39,28 @@ public class OI {
3939
*/
4040

4141
public Joystick leftJoy;
42+
public Joystick rightJoy;
4243
private JoystickButton shiftLowGearButton;
4344
private JoystickButton shiftHighGearButton;
4445
private JoystickButton shiftDriveTypeButton;
45-
private JoystickButton PIDMoveButton;
46-
private JoystickButton PIDTurnButton;
46+
private JoystickButton pIDMoveButton;
47+
private JoystickButton pIDTurnButton;
4748
private JoystickButton resetEncButton;
48-
private JoystickButton MoveLiftUpButton;
49-
private JoystickButton MoveLiftDownButton;
50-
public Joystick rightJoy;
49+
private JoystickButton moveLiftUpButton;
50+
private JoystickButton moveLiftDownButton;
51+
private JoystickButton findTurnTimeConstantButton;
5152
private JoystickButton updatePIDConstantsButton;
5253
private JoystickButton updateEncoderDPPButton;
54+
5355
public Joystick manipulator;
54-
private JoystickButton closeIntake;
55-
private JoystickButton openIntake;
56-
private JoystickButton raiseIntake;
57-
private JoystickButton lowerIntake;
58-
private JoystickButton intake;
59-
private JoystickButton outake;
60-
private JoystickButton toggleLeftIntake;
61-
private JoystickButton toggleRightIntake;
56+
private JoystickButton closeIntakeButton;
57+
private JoystickButton openIntakeButton;
58+
private JoystickButton raiseIntakeButton;
59+
private JoystickButton lowerIntakeButton;
60+
private JoystickButton intakeCubeButton;
61+
private JoystickButton outakeCubeButton;
62+
private JoystickButton toggleLeftIntakeButton;
63+
private JoystickButton toggleRightIntakeButton;
6264

6365
public int getButton(String key, int def) {
6466
if (!SmartDashboard.containsKey("Button/" + key)) {
@@ -70,20 +72,24 @@ public int getButton(String key, int def) {
7072
return (int) SmartDashboard.getNumber("Button/" + key, def);
7173
}
7274

73-
public OI() {
75+
public OI(Robot robot) {
7476
leftJoy = new Joystick(0);
7577
shiftDriveTypeButton = new JoystickButton(leftJoy, getButton("Shift Drive Type", 2));
7678
shiftDriveTypeButton.whenPressed(new ShiftDriveType());
77-
PIDMoveButton = new JoystickButton(leftJoy, getButton("PID Move", 7));
78-
PIDMoveButton
79+
pIDMoveButton = new JoystickButton(leftJoy, getButton("PID Move", 7));
80+
pIDMoveButton
7981
.whenPressed(new PIDMove(Robot.sd.getConst("Move Targ", 24), Robot.dt, Robot.sd, RobotMap.distEncAvg));
80-
PIDTurnButton = new JoystickButton(leftJoy, getButton("PID Turn", 8));
82+
pIDTurnButton = new JoystickButton(leftJoy, getButton("PID Turn", 8));
8183
// PIDTurnButton.whenPressed(new PIDTurn(Robot.getConst("Turn Targ", 90),
8284
// Robot.dt, Robot.sd RobotMap.fancyGyro));
83-
PIDTurnButton
85+
pIDTurnButton
8486
.whenReleased(new PIDTurn(Robot.getConst("Turn Targ", 90), Robot.dt, Robot.sd, RobotMap.fancyGyro));
8587
resetEncButton = new JoystickButton(leftJoy, getButton("Reset Dist Enc", 10));
8688
resetEncButton.whenPressed(new ResetEncoders());
89+
findTurnTimeConstantButton = new JoystickButton(leftJoy, getButton("Find Turn Time Constant", 11));
90+
// the command will only run in test mode
91+
findTurnTimeConstantButton
92+
.whenPressed(new FindTurnTimeConstant(robot, Robot.dt, Robot.rmap.fancyGyro, Robot.sd));
8793

8894
rightJoy = new Joystick(1);
8995
shiftHighGearButton = new JoystickButton(rightJoy, getButton("Shift High Gear", 3));
@@ -94,36 +100,82 @@ public OI() {
94100
updatePIDConstantsButton.whenPressed(new UpdatePIDConstants());
95101
updateEncoderDPPButton = new JoystickButton(rightJoy, getButton("Get Encoder Dist Per Pulse", 9));
96102
updateEncoderDPPButton.whenPressed(new SetDistancePerPulse());
97-
MoveLiftUpButton = new JoystickButton(rightJoy, getButton("Run Lift Motor Up", 10));
98-
MoveLiftDownButton = new JoystickButton(rightJoy, getButton("Run Lift Motor Down", 11));
99-
MoveLiftUpButton.whileHeld(new RunLift(Robot.lift, true));
100-
MoveLiftDownButton.whileHeld(new RunLift(Robot.lift, false));
103+
moveLiftUpButton = new JoystickButton(rightJoy, getButton("Run Lift Motor Up", 10));
104+
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));
101107

102108
manipulator = new Joystick(2);
103109
if (manipulator.getButtonCount() == 0) {
104110
System.err.println(
105111
"ERROR: manipulator does not appear to be plugged in. Disabling intake code. Restart code with manipulator plugged in to enable intake code");
106112
} else {
107-
108-
closeIntake = new JoystickButton(manipulator, getButton("Close Intake Button", 1));
109-
closeIntake.whenPressed(new CloseIntake());
110-
openIntake = new JoystickButton(manipulator, getButton("Open Intake Button", 2));
111-
openIntake.whenPressed(new OpenIntake());
113+
closeIntakeButton = new JoystickButton(manipulator, getButton("Close Intake Button", 1));
114+
closeIntakeButton.whenPressed(new CloseIntake());
115+
openIntakeButton = new JoystickButton(manipulator, getButton("Open Intake Button", 2));
116+
openIntakeButton.whenPressed(new OpenIntake());
112117
// raiseIntake = new JoystickButton(manipulator, getButton("Raise Intake
113118
// Button", 3));
114119
// raiseIntake.whenPressed(new RaiseIntake());
115120
// lowerIntake = new JoystickButton(manipulator, getButton("Lower Intake
116121
// Button", 4));
117-
lowerIntake.whenPressed(new LowerIntake());
118-
intake = new JoystickButton(manipulator, getButton("Intake Button", 5));
119-
intake.whenPressed(new IntakeCube());
120-
outake = new JoystickButton(manipulator, getButton("Outake Button", 6));
121-
outake.whenPressed(new OutakeCube());
122-
toggleLeftIntake = new JoystickButton(manipulator, getButton("Toggle Left Intake Button", 3));
123-
toggleLeftIntake.whenPressed(new ToggleLeftIntake());
124-
toggleRightIntake = new JoystickButton(manipulator, getButton("Toggle Right Intake Button", 4));
125-
toggleRightIntake.whenPressed(new ToggleRightIntake());
126-
122+
// lowerIntake.whenPressed(new LowerIntake());
123+
intakeCubeButton = new JoystickButton(manipulator, getButton("Intake Button", 5));
124+
intakeCubeButton.whenPressed(new IntakeCube());
125+
outakeCubeButton = new JoystickButton(manipulator, getButton("Outake Button", 6));
126+
outakeCubeButton.whenPressed(new OuttakeCube());
127+
toggleLeftIntakeButton = new JoystickButton(manipulator, getButton("Toggle Left Intake Button", 3));
128+
toggleLeftIntakeButton.whenPressed(new ToggleLeftIntake());
129+
toggleRightIntakeButton = new JoystickButton(manipulator, getButton("Toggle Right Intake Button", 4));
130+
toggleRightIntakeButton.whenPressed(new ToggleRightIntake());
127131
}
132+
133+
}
134+
135+
// /**
136+
// * Returns the getY from leftJoy squared (preserving sign)
137+
// *
138+
// * @return The y value squared
139+
// */
140+
// public double squareLeftY() {
141+
// return leftJoy.getY() * leftJoy.getY() * Math.signum(leftJoy.getY());
142+
// }
143+
//
144+
// /**
145+
// * Returns the getY from rightJoy squared (preserving sign)
146+
// *
147+
// * @return The y value squared
148+
// */
149+
// public double squareRightY() {
150+
// return rightJoy.getY() * rightJoy.getY() * Math.signum(rightJoy.getY());
151+
// }
152+
//
153+
// /**
154+
// * Returns the getX from leftJoy squared (preserving sign)
155+
// *
156+
// * @return The x value squared
157+
// */
158+
// public double squareLeftX() {
159+
// return leftJoy.getX() * leftJoy.getX() * Math.signum(leftJoy.getX());
160+
// }
161+
//
162+
// /**
163+
// * Returns the getX from rightJoy squared (preserving sign)
164+
// *
165+
// * @return The x value squared
166+
// */
167+
// public double squareRightX() {
168+
// return rightJoy.getX() * rightJoy.getX() * Math.signum(rightJoy.getX());
169+
// }
170+
171+
/**
172+
* Used to square joystick values while keeping sign
173+
*
174+
* @param joyVal
175+
* The joystick value to stick
176+
* @return The squared joystick value with same sign
177+
*/
178+
public double squareValueKeepSign(double joyVal) {
179+
return joyVal * joyVal * Math.signum(joyVal);
128180
}
129181
}

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

Lines changed: 58 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -12,7 +12,7 @@
1212
import java.util.Map;
1313

1414
import org.usfirst.frc.team199.Robot2018.autonomous.AutoUtils;
15-
import org.usfirst.frc.team199.Robot2018.autonomous.Position;
15+
import org.usfirst.frc.team199.Robot2018.autonomous.State;
1616
import org.usfirst.frc.team199.Robot2018.commands.Autonomous;
1717
import org.usfirst.frc.team199.Robot2018.commands.Autonomous.Strategy;
1818
import org.usfirst.frc.team199.Robot2018.commands.CloseIntake;
@@ -71,13 +71,26 @@ public double getConst(String key, double def) {
7171
return pref.getDouble("Const/" + key, def);
7272
}
7373

74+
public void putConst(String key, double def) {
75+
Preferences pref = Preferences.getInstance();
76+
pref.putDouble("Const/" + key, def);
77+
if (pref.getDouble("Const/ + key", def) != def) {
78+
System.err.println("pref Key" + "Const/" + key + "already taken by a different type");
79+
}
80+
}
81+
7482
public void putData(String string, PIDController controller) {
7583
SmartDashboard.putData(string, controller);
7684
}
7785

7886
public void putNumber(String string, double d) {
7987
SmartDashboard.putNumber(string, d);
8088
}
89+
90+
public void putBoolean(String string, boolean b) {
91+
SmartDashboard.putBoolean(string, b);
92+
}
93+
8194
/*
8295
* if (!SmartDashboard.containsKey("Const/" + key)) { if
8396
* (!SmartDashboard.putNumber("Const/" + key, def)) {
@@ -91,6 +104,10 @@ public static double getConst(String key, double def) {
91104
return sd.getConst(key, def);
92105
}
93106

107+
public static void putConst(String key, double def) {
108+
sd.putConst(key, def);
109+
}
110+
94111
public static boolean getBool(String key, boolean def) {
95112
Preferences pref = Preferences.getInstance();
96113
if (!pref.containsKey("Bool/" + key)) {
@@ -114,8 +131,8 @@ public void robotInit() {
114131
climberAssist = new ClimberAssist();
115132
intakeEject = new IntakeEject();
116133
lift = new Lift();
117-
dt = new Drivetrain();
118-
oi = new OI();
134+
dt = new Drivetrain(sd);
135+
oi = new OI(this);
119136

120137
// auto position chooser
121138
for (Autonomous.Position p : Autonomous.Position.values()) {
@@ -151,7 +168,7 @@ public void robotInit() {
151168
*/
152169
@Override
153170
public void disabledInit() {
154-
171+
dt.disableVelocityPIDs();
155172
}
156173

157174
@Override
@@ -167,7 +184,7 @@ public void disabledPeriodic() {
167184
@Override
168185
public void autonomousInit() {
169186
dt.resetAHRS();
170-
AutoUtils.position = new Position(0, 0, 0);
187+
AutoUtils.state = new State(0, 0, 0);
171188
Scheduler.getInstance().add(new ShiftLowGear());
172189
Scheduler.getInstance().add(new CloseIntake());
173190
String fmsInput = DriverStation.getInstance().getGameSpecificMessage();
@@ -195,6 +212,9 @@ public void autonomousPeriodic() {
195212

196213
@Override
197214
public void teleopInit() {
215+
System.out.println("In teleopInit()");
216+
dt.resetAHRS();
217+
AutoUtils.state = new State(0, 0, 0);
198218
// This makes sure that the autonomous stops running when
199219
// teleop starts running. If you want the autonomous to
200220
// continue until interrupted by another command, remove
@@ -224,12 +244,21 @@ public void teleopPeriodic() {
224244
// SmartDashboard.putNumber("Right Enc Dist", dt.getRightDist());
225245
// SmartDashboard.putNumber("Avg Enc Dist", dt.getEncAvgDist());
226246
//
247+
248+
System.out.printf("Left: %1$5.2f; Right: %2$5.2f\n", RobotMap.dtLeft.get(), RobotMap.dtRight.get());
249+
227250
SmartDashboard.putNumber("Angle", dt.getAHRSAngle());
251+
SmartDashboard.putNumber("Left Current draw", rmap.pdp.getCurrent(4));
252+
SmartDashboard.putNumber("Right Current draw", rmap.pdp.getCurrent(11));
228253
}
229254

230255
boolean firstTime = true;
231256

257+
@Override
232258
public void testInit() {
259+
System.out.println("In testInit()");
260+
dt.resetAHRS();
261+
AutoUtils.state = new State(0, 0, 0);
233262
}
234263

235264
/**
@@ -241,26 +270,39 @@ public void testPeriodic() {
241270
// Robot.dt.enableVelocityPIDs();
242271
// firstTime = false;
243272
//// }
244-
// Robot.dt.setVPIDs(Robot.getConst("VPID Test Set", 0.5));
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));
245284

246285
Scheduler.getInstance().run();
247286

248-
SmartDashboard.putNumber("Drivetrain/Left VPID Targ", Robot.dt.getLeftVPIDSetpoint());
249-
SmartDashboard.putNumber("Drivetrain/Right VPID Targ", Robot.dt.getRightVPIDSetpoint());
250-
SmartDashboard.putNumber("Left VPID Error", Robot.dt.getLeftVPIDerror());
251-
SmartDashboard.putNumber("Right VPID Error", Robot.dt.getRightVPIDerror());
252-
SmartDashboard.putNumber("Left Enc Rate", Robot.dt.getLeftEncRate());
253-
SmartDashboard.putNumber("Right Enc Rate", Robot.dt.getRightEncRate());
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());
254296

255-
SmartDashboard.putNumber("Left Enc Dist", dt.getLeftDist());
256-
SmartDashboard.putNumber("Right Enc Dist", dt.getRightDist());
257-
SmartDashboard.putNumber("Avg Enc Dist", dt.getEncAvgDist());
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());
258300

259301
// dt.dtLeft.set(0.1);
260302
// dt.dtRight.set(-oi.rightJoy.getY());
261303
// dt.dtLeft.set(-oi.leftJoy.getY());
262304
// dt.dtRight.set(-oi.rightJoy.getY());
263305

264-
dt.putVelocityControllersToDashboard();
306+
// dt.putVelocityControllersToDashboard();
265307
}
266308
}

0 commit comments

Comments
 (0)