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

Commit 288358d

Browse files
kevinzwangkevinzwang
committed
2 parents 4c09946 + cc76e30 commit 288358d

9 files changed

Lines changed: 170 additions & 16 deletions

File tree

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

Lines changed: 20 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -7,6 +7,10 @@
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.IntakeCube;
12+
import org.usfirst.frc.team199.Robot2018.commands.OpenIntake;
13+
import org.usfirst.frc.team199.Robot2018.commands.OutakeCube;
1014
import org.usfirst.frc.team199.Robot2018.commands.PIDMove;
1115
import org.usfirst.frc.team199.Robot2018.commands.PIDTurn;
1216
import org.usfirst.frc.team199.Robot2018.commands.ResetEncoders;
@@ -15,6 +19,8 @@
1519
import org.usfirst.frc.team199.Robot2018.commands.ShiftDriveType;
1620
import org.usfirst.frc.team199.Robot2018.commands.ShiftHighGear;
1721
import org.usfirst.frc.team199.Robot2018.commands.ShiftLowGear;
22+
import org.usfirst.frc.team199.Robot2018.commands.ToggleLeftIntake;
23+
import org.usfirst.frc.team199.Robot2018.commands.ToggleRightIntake;
1824
import org.usfirst.frc.team199.Robot2018.commands.UpdatePIDConstants;
1925

2026
import edu.wpi.first.wpilibj.Joystick;
@@ -50,6 +56,8 @@ public class OI {
5056
private JoystickButton lowerIntake;
5157
private JoystickButton intake;
5258
private JoystickButton outake;
59+
private JoystickButton toggleLeftIntake;
60+
private JoystickButton toggleRightIntake;
5361

5462
public int getButton(String key, int def) {
5563
if (!SmartDashboard.containsKey("Button/" + key)) {
@@ -91,21 +99,23 @@ public OI() {
9199
MoveLiftDownButton.whileHeld(new RunLift(Robot.lift, false));
92100

93101
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());
102+
closeIntake = new JoystickButton(manipulator, getButton("Close Intake Button", 1));
103+
closeIntake.whenPressed(new CloseIntake());
104+
openIntake = new JoystickButton(manipulator, getButton("Open Intake Button", 2));
105+
openIntake.whenPressed(new OpenIntake());
100106
// raiseIntake = new JoystickButton(manipulator, getButton("Raise Intake
101107
// Button", 3));
102108
// raiseIntake.whenPressed(new RaiseIntake());
103109
// lowerIntake = new JoystickButton(manipulator, getButton("Lower Intake
104110
// Button", 4));
105111
// 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());
112+
intake = new JoystickButton(manipulator, getButton("Intake Button", 5));
113+
intake.whenPressed(new IntakeCube());
114+
outake = new JoystickButton(manipulator, getButton("Outake Button", 6));
115+
outake.whenPressed(new OutakeCube());
116+
toggleLeftIntake = new JoystickButton(manipulator, getButton("Toggle Left Intake Button", 3));
117+
toggleLeftIntake.whenPressed(new ToggleLeftIntake());
118+
toggleRightIntake = new JoystickButton(manipulator, getButton("Toggle Right Intake Button", 4));
119+
toggleRightIntake.whenPressed(new ToggleRightIntake());
110120
}
111121
}

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

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -15,6 +15,7 @@
1515
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;
@@ -168,6 +169,7 @@ public void autonomousInit() {
168169
dt.resetAHRS();
169170
AutoUtils.state = new State(0, 0, 0);
170171
Scheduler.getInstance().add(new ShiftLowGear());
172+
Scheduler.getInstance().add(new CloseIntake());
171173
String fmsInput = DriverStation.getInstance().getGameSpecificMessage();
172174
Autonomous.Position startPos = posChooser.getSelected();
173175
double autoDelay = SmartDashboard.getNumber("Auto Delay", 0);
@@ -223,6 +225,8 @@ public void teleopPeriodic() {
223225
// SmartDashboard.putNumber("Avg Enc Dist", dt.getEncAvgDist());
224226
//
225227
SmartDashboard.putNumber("Angle", dt.getAHRSAngle());
228+
SmartDashboard.putNumber("Left Current draw", rmap.pdp.getCurrent(4));
229+
SmartDashboard.putNumber("Right Current draw", rmap.pdp.getCurrent(11));
226230
}
227231

228232
boolean firstTime = true;

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

Lines changed: 3 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -195,10 +195,9 @@ public RobotMap() {
195195
rightVelocityController.setContinuous(false);
196196
rightVelocityController.setAbsoluteTolerance(Robot.getConst("VelocityToleranceRight", 2));
197197

198-
// robotDrive = new DifferentialDrive(leftVelocityController,
199-
// rightVelocityController);
200-
// robotDrive.setMaxOutput(Robot.getConst("Max High Speed", 204));
201-
robotDrive = new DifferentialDrive(dtLeft, dtRight);
198+
robotDrive = new DifferentialDrive(leftVelocityController, rightVelocityController);
199+
robotDrive.setMaxOutput(Robot.getConst("Max High Speed", 204));
200+
// robotDrive = new DifferentialDrive(dtLeft, dtRight);
202201

203202
distEncAvg = new PIDSourceAverage(leftEncDist, rightEncDist);
204203
fancyGyro = new AHRS(SPI.Port.kMXP);

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

Lines changed: 19 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -2,30 +2,48 @@
22

33
import org.usfirst.frc.team199.Robot2018.Robot;
44

5+
import edu.wpi.first.wpilibj.Timer;
56
import edu.wpi.first.wpilibj.command.Command;
67

78
/**
89
*
910
*/
1011
public class IntakeCube extends Command {
1112

13+
private Timer tim;
14+
private boolean overDraw;
15+
1216
public IntakeCube() {
1317
// Use requires() here to declare subsystem dependencies
1418
// eg. requires(chassis);
19+
tim = new Timer();
1520
}
1621

1722
// Called just before this Command runs the first time
1823
protected void initialize() {
24+
tim.reset();
25+
tim.start();
26+
overDraw = false;
1927
}
2028

2129
// Called repeatedly when this Command is scheduled to run
2230
protected void execute() {
2331
Robot.intakeEject.runIntake(-1);
32+
if (Robot.intakeEject.hasCube()) {
33+
if (!overDraw) {
34+
overDraw = true;
35+
tim.start();
36+
}
37+
} else {
38+
overDraw = false;
39+
tim.stop();
40+
tim.reset();
41+
}
2442
}
2543

2644
// Make this return true when this Command no longer needs to run execute()
2745
protected boolean isFinished() {
28-
return Robot.intakeEject.hasCube();
46+
return tim.get() > Robot.getConst("Has Cube Timeout", 0.5);
2947
}
3048

3149
// Called once after isFinished returns true

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

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -23,7 +23,7 @@ public TeleopDrive() {
2323
// Called just before this Command runs the first time
2424
@Override
2525
protected void initialize() {
26-
// Robot.dt.enableVelocityPIDs();
26+
Robot.dt.enableVelocityPIDs();
2727
}
2828

2929
// Called repeatedly when this Command is scheduled to run

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

Lines changed: 24 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,24 @@
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 ToggleLeftIntake extends InstantCommand {
11+
12+
public ToggleLeftIntake() {
13+
super();
14+
// Use requires() here to declare subsystem dependencies
15+
// eg. requires(chassis);
16+
requires(Robot.intakeEject);
17+
}
18+
19+
// Called once when the command executes
20+
protected void initialize() {
21+
Robot.intakeEject.toggleLeftIntake();
22+
}
23+
24+
}

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

Lines changed: 24 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,24 @@
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 ToggleRightIntake extends InstantCommand {
11+
12+
public ToggleRightIntake() {
13+
super();
14+
// Use requires() here to declare subsystem dependencies
15+
// eg. requires(chassis);
16+
requires(Robot.intakeEject);
17+
}
18+
19+
// Called once when the command executes
20+
protected void initialize() {
21+
Robot.intakeEject.toggleRightIntake();
22+
}
23+
24+
}

‎Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/IntakeEject.java‎

Lines changed: 65 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -8,6 +8,7 @@
88
import edu.wpi.first.wpilibj.PowerDistributionPanel;
99
import edu.wpi.first.wpilibj.VictorSP;
1010
import edu.wpi.first.wpilibj.command.Subsystem;
11+
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
1112

1213
/**
1314
*
@@ -20,6 +21,8 @@ public class IntakeEject extends Subsystem implements IntakeEjectInterface {
2021
private final DoubleSolenoid rightVerticalSolenoid = RobotMap.rightIntakeVerticalSolenoid;
2122
private final DoubleSolenoid leftHorizontalSolenoid = RobotMap.leftIntakeHorizontalSolenoid;
2223
private final DoubleSolenoid rightHorizontalSolenoid = RobotMap.rightIntakeHorizontalSolenoid;
24+
private boolean rightOpen = false;
25+
private boolean leftOpen = false;
2326

2427
/**
2528
* Set the default command for a subsystem here.
@@ -123,6 +126,60 @@ public void lowerIntake() {
123126
rightVerticalSolenoid.set(rightSet);
124127
}
125128

129+
/**
130+
* Toggles the left intake between open and closed
131+
*/
132+
public void toggleLeftIntake() {
133+
// DoubleSolenoid.Value set;
134+
// if (Robot.getBool("Left Horizontal Solenoid Open", true)) {
135+
// set = Robot.getBool("Intake Left Horizontal Solenoid Inverted", false) ?
136+
// DoubleSolenoid.Value.kForward
137+
// : DoubleSolenoid.Value.kReverse;
138+
// } else {
139+
// set = Robot.getBool("Intake Left Horizontal Solenoid Inverted", false) ?
140+
// DoubleSolenoid.Value.kReverse
141+
// : DoubleSolenoid.Value.kForward;
142+
// }
143+
// leftHorizontalSolenoid.set(set);
144+
// SmartDashboard.putBoolean("Bool/Left Horizontal Solenoid Open",
145+
// !Robot.getBool("Left Horizontal Solenoid Open", true));
146+
if (leftOpen) {
147+
// set to closed
148+
leftHorizontalSolenoid.set(DoubleSolenoid.Value.kReverse);
149+
} else {
150+
// set to open
151+
leftHorizontalSolenoid.set(DoubleSolenoid.Value.kForward);
152+
}
153+
leftOpen = !leftOpen;
154+
}
155+
156+
/**
157+
* Toggles the right intake between open and closed
158+
*/
159+
public void toggleRightIntake() {
160+
// DoubleSolenoid.Value set;
161+
// if (Robot.getBool("Right Horizontal Solenoid Open", true)) {
162+
// set = Robot.getBool("Intake Right Horizontal Solenoid Inverted", false) ?
163+
// DoubleSolenoid.Value.kForward
164+
// : DoubleSolenoid.Value.kReverse;
165+
// } else {
166+
// set = Robot.getBool("Intake Right Horizontal Solenoid Inverted", false) ?
167+
// DoubleSolenoid.Value.kReverse
168+
// : DoubleSolenoid.Value.kForward;
169+
// }
170+
// rightHorizontalSolenoid.set(set);
171+
// SmartDashboard.putBoolean("Bool/Right Horizontal Solenoid Open",
172+
// !Robot.getBool("Right Horizontal Solenoid Open", true));
173+
if (rightOpen) {
174+
// set to closed
175+
rightHorizontalSolenoid.set(DoubleSolenoid.Value.kReverse);
176+
} else {
177+
// set to open
178+
rightHorizontalSolenoid.set(DoubleSolenoid.Value.kForward);
179+
}
180+
rightOpen = !rightOpen;
181+
}
182+
126183
/**
127184
* Closes the intake
128185
*/
@@ -133,8 +190,12 @@ public void closeIntake() {
133190
DoubleSolenoid.Value rightSet = Robot.getBool("Intake Right Horizontal Solenoid Inverted", false)
134191
? DoubleSolenoid.Value.kReverse
135192
: DoubleSolenoid.Value.kForward;
193+
SmartDashboard.putBoolean("Bool/Left Horizontal Solenoid Open", false);
194+
SmartDashboard.putBoolean("Bool/Right Horizontal Solenoid Open", false);
136195
leftHorizontalSolenoid.set(leftSet);
137196
rightHorizontalSolenoid.set(rightSet);
197+
leftOpen = false;
198+
rightOpen = false;
138199
}
139200

140201
/**
@@ -147,7 +208,11 @@ public void openIntake() {
147208
DoubleSolenoid.Value rightSet = Robot.getBool("Intake Right Horizontal Solenoid Inverted", false)
148209
? DoubleSolenoid.Value.kForward
149210
: DoubleSolenoid.Value.kReverse;
211+
SmartDashboard.putBoolean("Bool/Left Horizontal Solenoid Open", true);
212+
SmartDashboard.putBoolean("Bool/Right Horizontal Solenoid Open", true);
150213
leftHorizontalSolenoid.set(leftSet);
151214
rightHorizontalSolenoid.set(rightSet);
215+
leftOpen = false;
216+
rightOpen = false;
152217
}
153218
}

‎Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/IntakeEjectInterface.java‎

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -64,6 +64,16 @@ public interface IntakeEjectInterface {
6464
*/
6565
public void lowerIntake();
6666

67+
/**
68+
* Toggles the left intake between open and closed
69+
*/
70+
public void toggleLeftIntake();
71+
72+
/**
73+
* Toggles the right intake between open and closed
74+
*/
75+
public void toggleRightIntake();
76+
6777
/**
6878
* Closes the intake
6979
*/

0 commit comments

Comments
 (0)