public void whileHeld(int button, Command c) {
buttons.get(button).whileHeld(c);
}
+
+ public boolean get(int button) {
+ return getRawButton(button);
+ }
}
package org.usfirst.frc.team3501.robot;
+import java.util.Arrays;
+
import org.usfirst.frc.team3501.robot.commands.*;
public class OI {
public double getTwistR() {
return right.getTwist();
}
+
+ public boolean getRightPressed(int... buttons) {
+ return Arrays.stream(buttons).anyMatch(b -> right.get(b));
+ }
}
private void chooseAuto() {
autoChooser = new SendableChooser();
- autoChooser.addDefault("Drive over step", new DriveOverStep());
- autoChooser.addObject("Drive past step", new DrivePastStep());
+ autoChooser.addDefault("Pick up container", new ContainerOverStep());
+ autoChooser.addObject("Drive over step", new DriveOverStep());
+ autoChooser.addObject("Drive past step", new DrivePastStep());
SmartDashboard.putData("Auto Mode", autoChooser);
}
// auton
public static final double OVER_STEP_TIME = 1.2, OVER_STEP_SPEED = 0.7,
- PAST_STEP_TIME = 1.5, PAST_STEP_SPEED = 0.5;
+ PAST_STEP_TIME = 1.5, PAST_STEP_SPEED = 0.5,
+ PICKUP_TIME = 1.4, PICKUP_SPEED = 0.5;
}
--- /dev/null
+package org.usfirst.frc.team3501.robot.autons;
+
+import org.usfirst.frc.team3501.robot.Robot;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+public class ContainerOverStep extends CommandGroup {
+
+ public ContainerOverStep() {
+ super("ContainerOverStep");
+
+ requires(Robot.drivetrain);
+ requires(Robot.arm);
+ requires(Robot.claw);
+
+ queueCommands();
+ }
+
+ private void queueCommands() {
+ addSequential(new PickUpContainer());
+ addSequential(new DriveOverStep(-1));
+ }
+}
requires(drivetrain);
setTimeout(RobotMap.OVER_STEP_TIME);
- this.speed = RobotMap.OVER_STEP_SPEED;
+ speed = RobotMap.OVER_STEP_SPEED;
+ }
+
+ // TODO: this is an ugly "solution"
+ public DriveOverStep(int coef) {
+ this();
+ this.speed *= coef;
}
protected void execute() {
--- /dev/null
+package org.usfirst.frc.team3501.robot.autons;
+
+import org.usfirst.frc.team3501.robot.RobotMap;
+import org.usfirst.frc.team3501.robot.commands.*;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+public class PickUpContainer extends CommandGroup {
+
+ public PickUpContainer() {
+ super("PickUpContainer");
+
+ queueCommands();
+ }
+
+ private void queueCommands() {
+ addSequential(new CloseClaw());
+ addSequential(new MoveArmFor(RobotMap.PICKUP_TIME, RobotMap.PICKUP_SPEED));
+ }
+}
--- /dev/null
+package org.usfirst.frc.team3501.robot.commands;
+
+public class DriveFor extends CommandBase {
+
+ private double speed;
+
+ public DriveFor(double secs, double speed) {
+ super("DriveFor");
+ requires(drivetrain);
+
+ setTimeout(secs);
+ this.speed = speed;
+ }
+
+ protected void execute() {
+ drivetrain.driveRaw(speed, 0);
+ }
+
+ protected boolean isFinished() {
+ return isTimedOut();
+ }
+}
}
protected void execute() {
- drivetrain.drive(oi.getForwardR(), oi.getTwistR());
+ double forward = oi.getForwardR();
+ double twist = oi.getTwistR();
+
+ if (oi.getRightPressed(3, 4, 5, 6))
+ twist = 0;
+
+ drivetrain.drive(forward, twist);
}
protected boolean isFinished() {
}
protected void execute() {
- arm.setFromJoystick(oi.getForwardL());
+ arm.setFromJoystick(-oi.getForwardL());
}
protected boolean isFinished() {
--- /dev/null
+package org.usfirst.frc.team3501.robot.commands;
+
+public class MoveArmFor extends CommandBase {
+
+ private double speed;
+
+ public MoveArmFor(double secs, double speed) {
+ super("MoveArmFor");
+ requires(arm);
+
+ setTimeout(secs);
+ this.speed = speed;
+ }
+
+ protected void execute() {
+ arm.set(speed);
+ }
+
+ protected boolean isFinished() {
+ return isTimedOut();
+ }
+}
false);
}
+ public void driveRaw(double forward, double twist) {
+ robotDrive.arcadeDrive(forward, twist, false);
+ }
+
public void goForward(double speed) {
robotDrive.arcadeDrive(speed, 0);
}