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

Commit 0904738

Browse files
DriverStationComputerDriverStationComputer
committed
sfr working code (not breaking rn :))
1 parent 5840827 commit 0904738

11 files changed

Lines changed: 145 additions & 78 deletions

File tree

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

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

1010
import org.usfirst.frc.team199.Robot2018.commands.AutoLift;
11-
import org.usfirst.frc.team199.Robot2018.commands.CloseIntake;
1211
import org.usfirst.frc.team199.Robot2018.commands.FindTurnTimeConstant;
1312
import org.usfirst.frc.team199.Robot2018.commands.IntakeCube;
1413
import org.usfirst.frc.team199.Robot2018.commands.MoveLift;
1514
import org.usfirst.frc.team199.Robot2018.commands.MoveLiftWithPID;
16-
import org.usfirst.frc.team199.Robot2018.commands.OpenIntake;
1715
import org.usfirst.frc.team199.Robot2018.commands.OuttakeCube;
1816
import org.usfirst.frc.team199.Robot2018.commands.PIDMove;
1917
import org.usfirst.frc.team199.Robot2018.commands.PIDTurn;
@@ -23,6 +21,7 @@
2321
import org.usfirst.frc.team199.Robot2018.commands.ShiftHighGear;
2422
import org.usfirst.frc.team199.Robot2018.commands.ShiftLowGear;
2523
import org.usfirst.frc.team199.Robot2018.commands.StopIntake;
24+
import org.usfirst.frc.team199.Robot2018.commands.ToggleIntake;
2625
import org.usfirst.frc.team199.Robot2018.commands.UpdatePIDConstants;
2726
import org.usfirst.frc.team199.Robot2018.subsystems.LiftInterface.LiftHeight;
2827

@@ -66,6 +65,7 @@ public class OI {
6665
private JoystickButton outakeCubeButton;
6766
private JoystickButton toggleLeftIntakeButton;
6867
private JoystickButton toggleRightIntakeButton;
68+
private JoystickButton toggleIntakeButton;
6969
private JoystickButton stopIntakeButton;
7070

7171
public int getButton(String key, int def) {
@@ -98,11 +98,6 @@ public OI(Robot robot) {
9898
findTurnTimeConstantButton
9999
.whenPressed(new FindTurnTimeConstant(robot, Robot.dt, Robot.rmap.fancyGyro, Robot.sd));
100100

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-
106101
testLiftPID = new JoystickButton(leftJoy, getButton("Test Lift PID", 5));
107102
testLiftPID.whenPressed(
108103
new AutoLift(Robot.lift, SmartDashboard.getString("Lift Targ Height", LiftHeight.SWITCH.toString())));
@@ -124,30 +119,41 @@ public OI(Robot robot) {
124119
moveLiftDownButton.whileHeld(new MoveLift(Robot.lift, false));
125120

126121
manipulator = new Joystick(2);
127-
if (manipulator.getButtonCount() == 0) {
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");
130-
} else {
131-
closeIntakeButton = new JoystickButton(manipulator, getButton("Close Intake Button", 1));
132-
closeIntakeButton.whenPressed(new CloseIntake());
133-
openIntakeButton = new JoystickButton(manipulator, getButton("Open Intake Button", 2));
134-
openIntakeButton.whenPressed(new OpenIntake());
135-
136-
intakeCubeButton = new JoystickButton(manipulator, getButton("Intake Button", 5));
137-
intakeCubeButton.whenPressed(new IntakeCube());
138-
outakeCubeButton = new JoystickButton(manipulator, getButton("Outake Button", 6));
139-
outakeCubeButton.whenPressed(new OuttakeCube());
140-
141-
stopIntakeButton = new JoystickButton(manipulator, getButton("Stop Intake Button", 3));
142-
stopIntakeButton.whenPressed(new StopIntake());
143-
144-
// toggleLeftIntakeButton = new JoystickButton(manipulator, getButton("Toggle
145-
// Left Intake Button", 3));
146-
// toggleLeftIntakeButton.whenPressed(new ToggleLeftIntake());
147-
// toggleRightIntakeButton = new JoystickButton(manipulator, getButton("Toggle
148-
// Right Intake Button", 4));
149-
// toggleRightIntakeButton.whenPressed(new ToggleRightIntake());
150-
}
122+
// if (manipulator.getButtonCount() == 0) {
123+
// System.err.println(
124+
// "ERROR: manipulator does not appear to be plugged in. Disabling intake code.
125+
// Restart code with manipulator plugged in to enable intake code");
126+
// } else {
127+
128+
// closeIntakeButton = new JoystickButton(manipulator, getButton("Close Intake
129+
// Button", 1));
130+
// closeIntakeButton.whenPressed(new CloseIntake());
131+
// openIntakeButton = new JoystickButton(manipulator, getButton("Open Intake
132+
// Button", 2));
133+
// openIntakeButton.whenPressed(new OpenIntake());
134+
135+
moveLiftPIDUpButton = new JoystickButton(manipulator, getButton("Run Lift PID Up", 4));
136+
moveLiftPIDUpButton.whileHeld(new MoveLiftWithPID(Robot.lift, true));
137+
moveLiftPIDDownButton = new JoystickButton(manipulator, getButton("Run Lift PID Down", 1));
138+
moveLiftPIDDownButton.whileHeld(new MoveLiftWithPID(Robot.lift, false));
139+
140+
toggleIntakeButton = new JoystickButton(manipulator, getButton("Toggle Intake Button", 2));
141+
toggleIntakeButton.whenPressed(new ToggleIntake());
142+
143+
stopIntakeButton = new JoystickButton(manipulator, getButton("Stop Intake Button", 3));
144+
stopIntakeButton.whenPressed(new StopIntake());
145+
146+
intakeCubeButton = new JoystickButton(manipulator, getButton("Intake Button", 5));
147+
intakeCubeButton.whenPressed(new IntakeCube());
148+
outakeCubeButton = new JoystickButton(manipulator, getButton("Outake Button", 6));
149+
outakeCubeButton.whenPressed(new OuttakeCube());
150+
// toggleLeftIntakeButton = new JoystickButton(manipulator, getButton("Toggle
151+
// Left Intake Button", 3));
152+
// toggleLeftIntakeButton.whenPressed(new ToggleLeftIntake());
153+
// toggleRightIntakeButton = new JoystickButton(manipulator, getButton("Toggle
154+
// Right Intake Button", 4));
155+
// toggleRightIntakeButton.whenPressed(new ToggleRightIntake());
156+
// }
151157

152158
}
153159

‎Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/scriptupload/scriptupload.py‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -2,7 +2,7 @@
22
from networktables import NetworkTables
33
import __future__
44
import time
5-
NetworkTables.initialize(server='10.1.99.2')
5+
NetworkTables.initialize(server='172.22.11.2')
66
prefs = NetworkTables.getTable("Preferences")
77

88
go = False #true when file has been successfully read

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

Lines changed: 13 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -20,22 +20,25 @@ public DefaultIntake() {
2020
// Called just before this Command runs the first time
2121
@Override
2222
protected void initialize() {
23-
try {
24-
Robot.oi.manipulator.getRawAxis(1);
25-
} catch (NullPointerException e) {
26-
System.err.println("[ERROR] Manipulator not plugged in.");
27-
manipulatorPluggedIn = false;
28-
}
23+
// try {
24+
// Robot.oi.manipulator.getRawAxis(1);
25+
// } catch (NullPointerException e) {
26+
// System.err.println("[ERROR] Manipulator not plugged in.");
27+
// manipulatorPluggedIn = false;
28+
// }
2929
}
3030

3131
// Called repeatedly when this Command is scheduled to run
3232
@Override
3333
protected void execute() {
3434
// 1 and 5 represent the axes' index in driver station
35-
if (manipulatorPluggedIn) {
36-
Robot.intakeEject.runLeftIntake(Robot.oi.manipulator.getRawAxis(1));
37-
Robot.intakeEject.runRightIntake(Robot.oi.manipulator.getRawAxis(5));
38-
}
35+
// if (manipulatorPluggedIn) {
36+
// Robot.intakeEject.runLeftIntake(Robot.oi.manipulator.getRawAxis(1));
37+
// Robot.intakeEject.runRightIntake(Robot.oi.manipulator.getRawAxis(5));
38+
// }
39+
Robot.intakeEject.runLeftIntake(Robot.getConst("Intake Holding Speed", 0.15));
40+
Robot.intakeEject.runRightIntake(Robot.getConst("Intake Holding Speed", 0.15));
41+
3942
}
4043

4144
// Make this return true when this Command no longer needs to run execute()

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

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -17,6 +17,7 @@ public class IntakeCube extends Command {
1717
public IntakeCube() {
1818
// Use requires() here to declare subsystem dependencies
1919
// eg. requires(chassis);
20+
// requires(Robot.intakeEject);
2021
tim = new Timer();
2122
}
2223

@@ -36,6 +37,7 @@ protected void execute() {
3637
if (Robot.intakeEject.hasCube()) {
3738
if (!overDraw) {
3839
overDraw = true;
40+
// tim.reset();
3941
tim.start();
4042
}
4143
} else {

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

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -15,6 +15,7 @@ public class OuttakeCube extends Command {
1515
public OuttakeCube() {
1616
// Use requires() here to declare subsystem dependencies
1717
// eg. requires(chassis);
18+
requires(Robot.intakeEject);
1819
}
1920

2021
// Called just before this Command runs the first time

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

Lines changed: 6 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -23,6 +23,7 @@ public class PIDMove extends Command implements PIDOutput {
2323
private SmartDashboardInterface sd;
2424
private double[] point;
2525
private boolean absolute;
26+
private boolean hasInitialized = false;
2627

2728
/**
2829
* Constructs this command with a new PIDController. Sets all of the
@@ -120,6 +121,7 @@ public PIDMove(double[] point, DrivetrainInterface dt, SmartDashboardInterface s
120121
*/
121122
@Override
122123
public void initialize() {
124+
hasInitialized = true;
123125
if (absolute) {
124126
double dx = point[0] - AutoUtils.state.getX();
125127
double dy = point[1] - AutoUtils.state.getY();
@@ -174,7 +176,7 @@ protected boolean isFinished() {
174176
@Override
175177
protected void end() {
176178
moveController.disable();
177-
System.out.println("End");
179+
System.out.println("PIDMove End");
178180
// moveController.free();
179181

180182
double angle = Math.toRadians(AutoUtils.state.getRot());
@@ -212,6 +214,9 @@ protected void interrupted() {
212214
*/
213215
@Override
214216
public void pidWrite(double output) {
217+
if (!hasInitialized) {
218+
return;
219+
}
215220
dt.arcadeDrive(output, 0);
216221
sd.putNumber("Move PID Output", output);
217222
}

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

Lines changed: 7 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -28,6 +28,7 @@ public class PIDTurn extends Command implements PIDOutput {
2828
private double[] point;
2929
private boolean turnToPoint;
3030
private boolean absoluteRotation;
31+
private boolean hasInitialized = false;
3132

3233
/**
3334
* Constructs this command with a new PIDController. Sets all of the
@@ -148,6 +149,7 @@ public PIDTurn(double[] point, DrivetrainInterface dt, SmartDashboardInterface s
148149
*/
149150
@Override
150151
protected void initialize() {
152+
hasInitialized = true;
151153
// calculate pid constants
152154

153155
// max turn speed from FindTurnTimeConstant, converted to degrees
@@ -231,7 +233,7 @@ protected void execute() {
231233
*/
232234
@Override
233235
protected boolean isFinished() {
234-
System.out.println("isFinished");
236+
// System.out.println("isFinished");
235237
return (turnController.onTarget() && Math.abs(dt.getGyroRate()) < 1);
236238
// return turnController.onTarget()
237239
// && Math.abs(dt.getLeftEncRate()) <= Robot.getConst("Maximum Velocity When
@@ -248,7 +250,7 @@ protected boolean isFinished() {
248250
@Override
249251
protected void end() {
250252
turnController.disable();
251-
System.out.println("end");
253+
System.out.println("PIDTurn end");
252254
sd.putNumber("Turn PID Result", turnController.get());
253255
sd.putNumber("Turn PID Error", turnController.getError());
254256
// turnController.free();
@@ -279,6 +281,9 @@ protected void interrupted() {
279281
*/
280282
@Override
281283
public void pidWrite(double output) {
284+
if (!hasInitialized) {
285+
return;
286+
}
282287
dt.arcadeDrive(0, output);
283288
SmartDashboard.putNumber("Turn PID Output", output);
284289
}

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

Lines changed: 23 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,23 @@
1+
package org.usfirst.frc.team199.Robot2018.commands;
2+
3+
import org.usfirst.frc.team199.Robot2018.Robot;
4+
5+
import edu.wpi.first.wpilibj.command.InstantCommand;
6+
7+
/**
8+
*
9+
*/
10+
public class ToggleIntake extends InstantCommand {
11+
12+
public ToggleIntake() {
13+
// Use requires() here to declare subsystem dependencies
14+
// eg. requires(chassis);
15+
super();
16+
}
17+
18+
// Called just before this Command runs the first time
19+
@Override
20+
protected void initialize() {
21+
Robot.intakeEject.toggleIntake();
22+
}
23+
}

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

Lines changed: 31 additions & 32 deletions
Original file line numberDiff line numberDiff line change
@@ -26,49 +26,48 @@ public UpdateLiftPosition(Lift lift) {
2626
// Called just before this Command runs the first time
2727
@Override
2828
protected void initialize() {
29-
try {
30-
Robot.oi.manipulator.getRawAxis(1);
31-
} catch (NullPointerException e) {
32-
System.err.println("[ERROR] Manipulator not plugged in.");
33-
manipulatorPluggedIn = false;
34-
}
29+
// try {
30+
// Robot.oi.manipulator.getRawAxis(1);
31+
// } catch (NullPointerException e) {
32+
// System.err.println("[ERROR] Manipulator not plugged in.");
33+
// manipulatorPluggedIn = false;
34+
// }
3535
goToGround = false;
3636
}
3737

3838
// Called repeatedly when this Command is scheduled to run
3939
@Override
4040
protected void execute() {
41-
if (manipulatorPluggedIn) {
42-
int angle = Robot.oi.manipulator.getPOV();
41+
// if (manipulatorPluggedIn) {
42+
int angle = Robot.oi.manipulator.getPOV();
4343

44-
System.out.println("POV Reading: " + angle);
44+
System.out.println("POV Reading: " + angle);
4545

46-
if (angle == 180) {
47-
desiredPos = LiftHeight.HOLD_CUBE;
48-
goToGround = true;
49-
} else if (angle == 270) {
50-
desiredPos = LiftHeight.HOLD_CUBE;
51-
goToGround = false;
52-
} else if (angle != -1) {
53-
desiredPos = LiftHeight.SWITCH;
54-
goToGround = false;
55-
}
46+
if (angle == 180) {
47+
desiredPos = LiftHeight.HOLD_CUBE;
48+
goToGround = true;
49+
} else if (angle == 270) {
50+
desiredPos = LiftHeight.HOLD_CUBE;
51+
goToGround = false;
52+
} else if (angle != -1) {
53+
desiredPos = LiftHeight.SWITCH;
54+
goToGround = false;
55+
}
5656

57-
if (goToGround || angle != -1) {
58-
desiredDist = lift.getDesiredDistFromPos(desiredPos);
59-
lift.setSetpoint(desiredDist);
60-
}
57+
if (goToGround || angle != -1) {
58+
desiredDist = lift.getDesiredDistFromPos(desiredPos);
59+
lift.setSetpoint(desiredDist);
60+
}
6161

62-
if (goToGround && lift.onTarget() && lift.getSpeed() <= 0.1) {
63-
desiredPos = LiftHeight.GROUND;
64-
desiredDist = lift.getDesiredDistFromPos(desiredPos);
65-
lift.setSetpoint(desiredDist);
66-
goToGround = false;
67-
}
68-
System.out.println("Desired Pos: " + desiredPos);
69-
System.out.println("Desired Dist: " + desiredDist);
70-
System.out.println("Current Dist: " + lift.getHeight());
62+
if (goToGround && lift.onTarget() && lift.getSpeed() <= 0.1) {
63+
desiredPos = LiftHeight.GROUND;
64+
desiredDist = lift.getDesiredDistFromPos(desiredPos);
65+
lift.setSetpoint(desiredDist);
66+
goToGround = false;
7167
}
68+
System.out.println("Desired Pos: " + desiredPos);
69+
System.out.println("Desired Dist: " + desiredDist);
70+
System.out.println("Current Dist: " + lift.getHeight());
7271
}
7372

7473
// Make this return true when this Command no longer needs to run execute()

0 commit comments

Comments
 (0)