From e81578e3624e61c2a8eb5b5c6b1015a2999784e7 Mon Sep 17 00:00:00 2001 From: Logan Howard Date: Thu, 16 Apr 2015 22:56:08 -0700 Subject: [PATCH] add hella auton --- .../usfirst/frc/team3501/robot/Joystick.java | 4 ++++ src/org/usfirst/frc/team3501/robot/OI.java | 6 +++++ src/org/usfirst/frc/team3501/robot/Robot.java | 5 ++-- .../usfirst/frc/team3501/robot/RobotMap.java | 3 ++- .../robot/autons/ContainerOverStep.java | 23 +++++++++++++++++++ .../team3501/robot/autons/DriveOverStep.java | 8 ++++++- .../robot/autons/PickUpContainer.java | 20 ++++++++++++++++ .../frc/team3501/robot/commands/DriveFor.java | 22 ++++++++++++++++++ .../robot/commands/DriveWithJoysticks.java | 8 ++++++- .../frc/team3501/robot/commands/MoveArm.java | 2 +- .../team3501/robot/commands/MoveArmFor.java | 22 ++++++++++++++++++ .../team3501/robot/subsystems/Drivetrain.java | 4 ++++ 12 files changed, 121 insertions(+), 6 deletions(-) create mode 100644 src/org/usfirst/frc/team3501/robot/autons/ContainerOverStep.java create mode 100644 src/org/usfirst/frc/team3501/robot/autons/PickUpContainer.java create mode 100644 src/org/usfirst/frc/team3501/robot/commands/DriveFor.java create mode 100644 src/org/usfirst/frc/team3501/robot/commands/MoveArmFor.java diff --git a/src/org/usfirst/frc/team3501/robot/Joystick.java b/src/org/usfirst/frc/team3501/robot/Joystick.java index f78be05..fab6792 100644 --- a/src/org/usfirst/frc/team3501/robot/Joystick.java +++ b/src/org/usfirst/frc/team3501/robot/Joystick.java @@ -29,4 +29,8 @@ public class Joystick extends edu.wpi.first.wpilibj.Joystick { public void whileHeld(int button, Command c) { buttons.get(button).whileHeld(c); } + + public boolean get(int button) { + return getRawButton(button); + } } diff --git a/src/org/usfirst/frc/team3501/robot/OI.java b/src/org/usfirst/frc/team3501/robot/OI.java index f573281..82c4217 100644 --- a/src/org/usfirst/frc/team3501/robot/OI.java +++ b/src/org/usfirst/frc/team3501/robot/OI.java @@ -1,5 +1,7 @@ package org.usfirst.frc.team3501.robot; +import java.util.Arrays; + import org.usfirst.frc.team3501.robot.commands.*; public class OI { @@ -40,4 +42,8 @@ public class OI { public double getTwistR() { return right.getTwist(); } + + public boolean getRightPressed(int... buttons) { + return Arrays.stream(buttons).anyMatch(b -> right.get(b)); + } } diff --git a/src/org/usfirst/frc/team3501/robot/Robot.java b/src/org/usfirst/frc/team3501/robot/Robot.java index 23a43b7..633cde6 100644 --- a/src/org/usfirst/frc/team3501/robot/Robot.java +++ b/src/org/usfirst/frc/team3501/robot/Robot.java @@ -73,8 +73,9 @@ public class Robot extends IterativeRobot { 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); } diff --git a/src/org/usfirst/frc/team3501/robot/RobotMap.java b/src/org/usfirst/frc/team3501/robot/RobotMap.java index 8cfb7fb..5e73c65 100644 --- a/src/org/usfirst/frc/team3501/robot/RobotMap.java +++ b/src/org/usfirst/frc/team3501/robot/RobotMap.java @@ -27,5 +27,6 @@ public class RobotMap { // 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; } diff --git a/src/org/usfirst/frc/team3501/robot/autons/ContainerOverStep.java b/src/org/usfirst/frc/team3501/robot/autons/ContainerOverStep.java new file mode 100644 index 0000000..75e592b --- /dev/null +++ b/src/org/usfirst/frc/team3501/robot/autons/ContainerOverStep.java @@ -0,0 +1,23 @@ +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)); + } +} diff --git a/src/org/usfirst/frc/team3501/robot/autons/DriveOverStep.java b/src/org/usfirst/frc/team3501/robot/autons/DriveOverStep.java index 2298661..f70b3d2 100644 --- a/src/org/usfirst/frc/team3501/robot/autons/DriveOverStep.java +++ b/src/org/usfirst/frc/team3501/robot/autons/DriveOverStep.java @@ -12,7 +12,13 @@ public class DriveOverStep extends CommandBase { 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() { diff --git a/src/org/usfirst/frc/team3501/robot/autons/PickUpContainer.java b/src/org/usfirst/frc/team3501/robot/autons/PickUpContainer.java new file mode 100644 index 0000000..9b08d1f --- /dev/null +++ b/src/org/usfirst/frc/team3501/robot/autons/PickUpContainer.java @@ -0,0 +1,20 @@ +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)); + } +} diff --git a/src/org/usfirst/frc/team3501/robot/commands/DriveFor.java b/src/org/usfirst/frc/team3501/robot/commands/DriveFor.java new file mode 100644 index 0000000..c1ca507 --- /dev/null +++ b/src/org/usfirst/frc/team3501/robot/commands/DriveFor.java @@ -0,0 +1,22 @@ +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(); + } +} diff --git a/src/org/usfirst/frc/team3501/robot/commands/DriveWithJoysticks.java b/src/org/usfirst/frc/team3501/robot/commands/DriveWithJoysticks.java index cda6cdd..9802147 100644 --- a/src/org/usfirst/frc/team3501/robot/commands/DriveWithJoysticks.java +++ b/src/org/usfirst/frc/team3501/robot/commands/DriveWithJoysticks.java @@ -8,7 +8,13 @@ public class DriveWithJoysticks extends CommandBase { } 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() { diff --git a/src/org/usfirst/frc/team3501/robot/commands/MoveArm.java b/src/org/usfirst/frc/team3501/robot/commands/MoveArm.java index f0865af..2354b3d 100644 --- a/src/org/usfirst/frc/team3501/robot/commands/MoveArm.java +++ b/src/org/usfirst/frc/team3501/robot/commands/MoveArm.java @@ -8,7 +8,7 @@ public class MoveArm extends CommandBase { } protected void execute() { - arm.setFromJoystick(oi.getForwardL()); + arm.setFromJoystick(-oi.getForwardL()); } protected boolean isFinished() { diff --git a/src/org/usfirst/frc/team3501/robot/commands/MoveArmFor.java b/src/org/usfirst/frc/team3501/robot/commands/MoveArmFor.java new file mode 100644 index 0000000..7f475f1 --- /dev/null +++ b/src/org/usfirst/frc/team3501/robot/commands/MoveArmFor.java @@ -0,0 +1,22 @@ +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(); + } +} diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/Drivetrain.java b/src/org/usfirst/frc/team3501/robot/subsystems/Drivetrain.java index 72428e4..90e7847 100644 --- a/src/org/usfirst/frc/team3501/robot/subsystems/Drivetrain.java +++ b/src/org/usfirst/frc/team3501/robot/subsystems/Drivetrain.java @@ -34,6 +34,10 @@ public class Drivetrain extends Subsystem { false); } + public void driveRaw(double forward, double twist) { + robotDrive.arcadeDrive(forward, twist, false); + } + public void goForward(double speed) { robotDrive.arcadeDrive(speed, 0); } -- 2.30.2