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

Commit ce120cd

Browse files
brettlebrettle
authored
Merge pull request #92 from DriverStationComputer/vel-pid-2
DriverStation MegaMerge
2 parents 091b6b2 + 17eb98f commit ce120cd

31 files changed

+1186
-356
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/lib/User_Libraries.properties‎

Lines changed: 0 additions & 2 deletions
This file was deleted.

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

Lines changed: 104 additions & 36 deletions
Original file line numberDiff line numberDiff line change
@@ -7,6 +7,11 @@
77

88
package org.usfirst.frc.team199.Robot2018;
99

10+
import org.usfirst.frc.team199.Robot2018.commands.CloseIntake;
11+
import org.usfirst.frc.team199.Robot2018.commands.FindTurnTimeConstant;
12+
import org.usfirst.frc.team199.Robot2018.commands.IntakeCube;
13+
import org.usfirst.frc.team199.Robot2018.commands.OpenIntake;
14+
import org.usfirst.frc.team199.Robot2018.commands.OuttakeCube;
1015
import org.usfirst.frc.team199.Robot2018.commands.PIDMove;
1116
import org.usfirst.frc.team199.Robot2018.commands.PIDTurn;
1217
import org.usfirst.frc.team199.Robot2018.commands.ResetEncoders;
@@ -15,6 +20,8 @@
1520
import org.usfirst.frc.team199.Robot2018.commands.ShiftDriveType;
1621
import org.usfirst.frc.team199.Robot2018.commands.ShiftHighGear;
1722
import org.usfirst.frc.team199.Robot2018.commands.ShiftLowGear;
23+
import org.usfirst.frc.team199.Robot2018.commands.ToggleLeftIntake;
24+
import org.usfirst.frc.team199.Robot2018.commands.ToggleRightIntake;
1825
import org.usfirst.frc.team199.Robot2018.commands.UpdatePIDConstants;
1926

2027
import edu.wpi.first.wpilibj.Joystick;
@@ -32,24 +39,28 @@ public class OI {
3239
*/
3340

3441
public Joystick leftJoy;
42+
public Joystick rightJoy;
3543
private JoystickButton shiftLowGearButton;
3644
private JoystickButton shiftHighGearButton;
3745
private JoystickButton shiftDriveTypeButton;
38-
private JoystickButton PIDMoveButton;
39-
private JoystickButton PIDTurnButton;
46+
private JoystickButton pIDMoveButton;
47+
private JoystickButton pIDTurnButton;
4048
private JoystickButton resetEncButton;
41-
private JoystickButton MoveLiftUpButton;
42-
private JoystickButton MoveLiftDownButton;
43-
public Joystick rightJoy;
49+
private JoystickButton moveLiftUpButton;
50+
private JoystickButton moveLiftDownButton;
51+
private JoystickButton findTurnTimeConstantButton;
4452
private JoystickButton updatePIDConstantsButton;
4553
private JoystickButton updateEncoderDPPButton;
54+
4655
public Joystick manipulator;
47-
private JoystickButton closeIntake;
48-
private JoystickButton openIntake;
49-
private JoystickButton raiseIntake;
50-
private JoystickButton lowerIntake;
51-
private JoystickButton intake;
52-
private JoystickButton outake;
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;
5364

5465
public int getButton(String key, int def) {
5566
if (!SmartDashboard.containsKey("Button/" + key)) {
@@ -61,20 +72,24 @@ public int getButton(String key, int def) {
6172
return (int) SmartDashboard.getNumber("Button/" + key, def);
6273
}
6374

64-
public OI() {
75+
public OI(Robot robot) {
6576
leftJoy = new Joystick(0);
6677
shiftDriveTypeButton = new JoystickButton(leftJoy, getButton("Shift Drive Type", 2));
6778
shiftDriveTypeButton.whenPressed(new ShiftDriveType());
68-
PIDMoveButton = new JoystickButton(leftJoy, getButton("PID Move", 7));
69-
PIDMoveButton
79+
pIDMoveButton = new JoystickButton(leftJoy, getButton("PID Move", 7));
80+
pIDMoveButton
7081
.whenPressed(new PIDMove(Robot.sd.getConst("Move Targ", 24), Robot.dt, Robot.sd, RobotMap.distEncAvg));
71-
PIDTurnButton = new JoystickButton(leftJoy, getButton("PID Turn", 8));
82+
pIDTurnButton = new JoystickButton(leftJoy, getButton("PID Turn", 8));
7283
// PIDTurnButton.whenPressed(new PIDTurn(Robot.getConst("Turn Targ", 90),
7384
// Robot.dt, Robot.sd RobotMap.fancyGyro));
74-
PIDTurnButton
85+
pIDTurnButton
7586
.whenReleased(new PIDTurn(Robot.getConst("Turn Targ", 90), Robot.dt, Robot.sd, RobotMap.fancyGyro));
7687
resetEncButton = new JoystickButton(leftJoy, getButton("Reset Dist Enc", 10));
7788
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));
7893

7994
rightJoy = new Joystick(1);
8095
shiftHighGearButton = new JoystickButton(rightJoy, getButton("Shift High Gear", 3));
@@ -85,27 +100,80 @@ public OI() {
85100
updatePIDConstantsButton.whenPressed(new UpdatePIDConstants());
86101
updateEncoderDPPButton = new JoystickButton(rightJoy, getButton("Get Encoder Dist Per Pulse", 9));
87102
updateEncoderDPPButton.whenPressed(new SetDistancePerPulse());
88-
MoveLiftUpButton = new JoystickButton(rightJoy, getButton("Run Lift Motor Up", 10));
89-
MoveLiftDownButton = new JoystickButton(rightJoy, getButton("Run Lift Motor Down", 11));
90-
MoveLiftUpButton.whileHeld(new RunLift(Robot.lift, true));
91-
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));
92107

93108
manipulator = new Joystick(2);
94-
// closeIntake = new JoystickButton(manipulator, getButton("Close Intake
95-
// Button", 1));
96-
// closeIntake.whenPressed(new CloseIntake());
97-
// openIntake = new JoystickButton(manipulator, getButton("Open Intake Button",
98-
// 2));
99-
// openIntake.whenPressed(new OpenIntake());
100-
// raiseIntake = new JoystickButton(manipulator, getButton("Raise Intake
101-
// Button", 3));
102-
// raiseIntake.whenPressed(new RaiseIntake());
103-
// lowerIntake = new JoystickButton(manipulator, getButton("Lower Intake
104-
// Button", 4));
105-
// lowerIntake.whenPressed(new LowerIntake());
106-
// intake = new JoystickButton(manipulator, getButton("Intake Button", 5));
107-
// intake.whenPressed(new IntakeCube());
108-
// outake = new JoystickButton(manipulator, getButton("Outake Button", 6));
109-
// outake.whenPressed(new OutakeCube());
109+
if (manipulator.getButtonCount() == 0) {
110+
System.out.println("Manipulator not plugged in!");
111+
} else {
112+
closeIntakeButton = new JoystickButton(manipulator, getButton("Close Intake Button", 1));
113+
closeIntakeButton.whenPressed(new CloseIntake());
114+
openIntakeButton = new JoystickButton(manipulator, getButton("Open Intake Button", 2));
115+
openIntakeButton.whenPressed(new OpenIntake());
116+
// raiseIntake = new JoystickButton(manipulator, getButton("Raise Intake
117+
// Button", 3));
118+
// raiseIntake.whenPressed(new RaiseIntake());
119+
// lowerIntake = new JoystickButton(manipulator, getButton("Lower Intake
120+
// Button", 4));
121+
// lowerIntake.whenPressed(new LowerIntake());
122+
intakeCubeButton = new JoystickButton(manipulator, getButton("Intake Button", 5));
123+
intakeCubeButton.whenPressed(new IntakeCube());
124+
outakeCubeButton = new JoystickButton(manipulator, getButton("Outake Button", 6));
125+
outakeCubeButton.whenPressed(new OuttakeCube());
126+
toggleLeftIntakeButton = new JoystickButton(manipulator, getButton("Toggle Left Intake Button", 3));
127+
toggleLeftIntakeButton.whenPressed(new ToggleLeftIntake());
128+
toggleRightIntakeButton = new JoystickButton(manipulator, getButton("Toggle Right Intake Button", 4));
129+
toggleRightIntakeButton.whenPressed(new ToggleRightIntake());
130+
}
131+
}
132+
133+
// /**
134+
// * Returns the getY from leftJoy squared (preserving sign)
135+
// *
136+
// * @return The y value squared
137+
// */
138+
// public double squareLeftY() {
139+
// return leftJoy.getY() * leftJoy.getY() * Math.signum(leftJoy.getY());
140+
// }
141+
//
142+
// /**
143+
// * Returns the getY from rightJoy squared (preserving sign)
144+
// *
145+
// * @return The y value squared
146+
// */
147+
// public double squareRightY() {
148+
// return rightJoy.getY() * rightJoy.getY() * Math.signum(rightJoy.getY());
149+
// }
150+
//
151+
// /**
152+
// * Returns the getX from leftJoy squared (preserving sign)
153+
// *
154+
// * @return The x value squared
155+
// */
156+
// public double squareLeftX() {
157+
// return leftJoy.getX() * leftJoy.getX() * Math.signum(leftJoy.getX());
158+
// }
159+
//
160+
// /**
161+
// * Returns the getX from rightJoy squared (preserving sign)
162+
// *
163+
// * @return The x value squared
164+
// */
165+
// public double squareRightX() {
166+
// return rightJoy.getX() * rightJoy.getX() * Math.signum(rightJoy.getX());
167+
// }
168+
169+
/**
170+
* Used to square joystick values while keeping sign
171+
*
172+
* @param joyVal
173+
* The joystick value to stick
174+
* @return The squared joystick value with same sign
175+
*/
176+
public double squareValueKeepSign(double joyVal) {
177+
return joyVal * joyVal * Math.signum(joyVal);
110178
}
111179
}

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

Lines changed: 60 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -12,9 +12,10 @@
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;
18+
import org.usfirst.frc.team199.Robot2018.commands.CloseIntake;
1819
import org.usfirst.frc.team199.Robot2018.commands.ShiftLowGear;
1920
import org.usfirst.frc.team199.Robot2018.subsystems.Climber;
2021
import org.usfirst.frc.team199.Robot2018.subsystems.ClimberAssist;
@@ -70,13 +71,26 @@ public double getConst(String key, double def) {
7071
return pref.getDouble("Const/" + key, def);
7172
}
7273

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+
7382
public void putData(String string, PIDController controller) {
7483
SmartDashboard.putData(string, controller);
7584
}
7685

7786
public void putNumber(String string, double d) {
7887
SmartDashboard.putNumber(string, d);
7988
}
89+
90+
public void putBoolean(String string, boolean b) {
91+
SmartDashboard.putBoolean(string, b);
92+
}
93+
8094
/*
8195
* if (!SmartDashboard.containsKey("Const/" + key)) { if
8296
* (!SmartDashboard.putNumber("Const/" + key, def)) {
@@ -90,6 +104,10 @@ public static double getConst(String key, double def) {
90104
return sd.getConst(key, def);
91105
}
92106

107+
public static void putConst(String key, double def) {
108+
sd.putConst(key, def);
109+
}
110+
93111
public static boolean getBool(String key, boolean def) {
94112
Preferences pref = Preferences.getInstance();
95113
if (!pref.containsKey("Bool/" + key)) {
@@ -113,8 +131,8 @@ public void robotInit() {
113131
climberAssist = new ClimberAssist();
114132
intakeEject = new IntakeEject();
115133
lift = new Lift();
116-
dt = new Drivetrain();
117-
oi = new OI();
134+
dt = new Drivetrain(sd);
135+
oi = new OI(this);
118136

119137
// auto position chooser
120138
for (Autonomous.Position p : Autonomous.Position.values()) {
@@ -150,7 +168,7 @@ public void robotInit() {
150168
*/
151169
@Override
152170
public void disabledInit() {
153-
171+
dt.disableVelocityPIDs();
154172
}
155173

156174
@Override
@@ -166,8 +184,9 @@ public void disabledPeriodic() {
166184
@Override
167185
public void autonomousInit() {
168186
dt.resetAHRS();
169-
AutoUtils.position = new Position(0, 0, 0);
187+
AutoUtils.state = new State(0, 0, 0);
170188
Scheduler.getInstance().add(new ShiftLowGear());
189+
Scheduler.getInstance().add(new CloseIntake());
171190
String fmsInput = DriverStation.getInstance().getGameSpecificMessage();
172191
Autonomous.Position startPos = posChooser.getSelected();
173192
double autoDelay = SmartDashboard.getNumber("Auto Delay", 0);
@@ -193,6 +212,9 @@ public void autonomousPeriodic() {
193212

194213
@Override
195214
public void teleopInit() {
215+
System.out.println("In teleopInit()");
216+
dt.resetAHRS();
217+
AutoUtils.state = new State(0, 0, 0);
196218
// This makes sure that the autonomous stops running when
197219
// teleop starts running. If you want the autonomous to
198220
// continue until interrupted by another command, remove
@@ -222,12 +244,21 @@ public void teleopPeriodic() {
222244
// SmartDashboard.putNumber("Right Enc Dist", dt.getRightDist());
223245
// SmartDashboard.putNumber("Avg Enc Dist", dt.getEncAvgDist());
224246
//
247+
248+
System.out.printf("Left: %1$5.2f; Right: %2$5.2f\n", RobotMap.dtLeft.get(), RobotMap.dtRight.get());
249+
225250
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));
226253
}
227254

228255
boolean firstTime = true;
229256

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

233264
/**
@@ -239,26 +270,39 @@ public void testPeriodic() {
239270
// Robot.dt.enableVelocityPIDs();
240271
// firstTime = false;
241272
//// }
242-
// 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));
243284

244285
Scheduler.getInstance().run();
245286

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

253-
SmartDashboard.putNumber("Left Enc Dist", dt.getLeftDist());
254-
SmartDashboard.putNumber("Right Enc Dist", dt.getRightDist());
255-
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());
256300

257301
// dt.dtLeft.set(0.1);
258302
// dt.dtRight.set(-oi.rightJoy.getY());
259303
// dt.dtLeft.set(-oi.leftJoy.getY());
260304
// dt.dtRight.set(-oi.rightJoy.getY());
261305

262-
dt.putVelocityControllersToDashboard();
306+
// dt.putVelocityControllersToDashboard();
263307
}
264308
}

0 commit comments

Comments
 (0)