From 3e4790a8551e2a7c20db705dea86cb59f3c84696 Mon Sep 17 00:00:00 2001 From: Logan Howard Date: Thu, 16 Apr 2015 19:45:25 -0700 Subject: [PATCH] continue to flesh out initial codebase --- src/org/usfirst/frc/team3501/robot/OI.java | 4 +- src/org/usfirst/frc/team3501/robot/Robot.java | 24 +++++++---- .../usfirst/frc/team3501/robot/RobotMap.java | 13 +++++- .../team3501/robot/autons/DriveOverStep.java | 29 +++++++++++++ .../team3501/robot/autons/DrivePastStep.java | 29 +++++++++++++ .../team3501/robot/commands/DriveForward.java | 26 ------------ .../robot/commands/DriveWithJoysticks.java | 2 +- .../frc/team3501/robot/subsystems/Arm.java | 41 +++++++++++++++---- .../frc/team3501/robot/subsystems/Claw.java | 10 ++--- .../team3501/robot/subsystems/Drivetrain.java | 7 +++- .../team3501/robot/subsystems/Pneumatics.java | 24 +++++++++++ 11 files changed, 154 insertions(+), 55 deletions(-) create mode 100644 src/org/usfirst/frc/team3501/robot/autons/DriveOverStep.java create mode 100644 src/org/usfirst/frc/team3501/robot/autons/DrivePastStep.java delete mode 100644 src/org/usfirst/frc/team3501/robot/commands/DriveForward.java create mode 100644 src/org/usfirst/frc/team3501/robot/subsystems/Pneumatics.java diff --git a/src/org/usfirst/frc/team3501/robot/OI.java b/src/org/usfirst/frc/team3501/robot/OI.java index d6344b7..0b8fa07 100644 --- a/src/org/usfirst/frc/team3501/robot/OI.java +++ b/src/org/usfirst/frc/team3501/robot/OI.java @@ -13,11 +13,11 @@ public class OI { right.whenReleased(1, new OpenClaw()); } - public double getForwardSpeed() { + public double getForward() { return right.getY(); } - public double getTwistSpeed() { + public double getTwist() { return right.getTwist(); } } diff --git a/src/org/usfirst/frc/team3501/robot/Robot.java b/src/org/usfirst/frc/team3501/robot/Robot.java index 68770bb..b5ac5b7 100644 --- a/src/org/usfirst/frc/team3501/robot/Robot.java +++ b/src/org/usfirst/frc/team3501/robot/Robot.java @@ -5,8 +5,10 @@ import edu.wpi.first.wpilibj.IterativeRobot; import edu.wpi.first.wpilibj.command.Command; import edu.wpi.first.wpilibj.command.Scheduler; import edu.wpi.first.wpilibj.livewindow.LiveWindow; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import org.usfirst.frc.team3501.robot.commands.*; +import org.usfirst.frc.team3501.robot.autons.*; import org.usfirst.frc.team3501.robot.subsystems.*; public class Robot extends IterativeRobot { @@ -17,14 +19,14 @@ public class Robot extends IterativeRobot { public static OI oi; - Command autonomousCommand; + private SendableChooser autoChooser; + + private Command autonomousCommand; public void robotInit() { oi = new OI(); - double time = 1.2; - double speed = 0.7; - autonomousCommand = new DriveForward(time, speed); + chooseAuto(); } public void disabledPeriodic() { @@ -32,6 +34,7 @@ public class Robot extends IterativeRobot { } public void autonomousInit() { + autonomousCommand = (Command) autoChooser.getSelected(); autonomousCommand.start(); } @@ -45,11 +48,18 @@ public class Robot extends IterativeRobot { public void teleopPeriodic() { Scheduler.getInstance().run(); - - } public void testPeriodic() { LiveWindow.run(); } + + private void chooseAuto() { + autoChooser = new SendableChooser(); + + autoChooser.addDefault("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 0525166..bfb2bea 100644 --- a/src/org/usfirst/frc/team3501/robot/RobotMap.java +++ b/src/org/usfirst/frc/team3501/robot/RobotMap.java @@ -3,16 +3,27 @@ package org.usfirst.frc.team3501.robot; import edu.wpi.first.wpilibj.DoubleSolenoid.Value; public class RobotMap { + // Driver Station public static final int LEFT_JOYSTICK_PORT = 0, RIGHT_JOYSTICK_PORT = 1; - public static final double MIN_DRIVE_JOYSTICK_INPUT = 0.1; + public static final double MIN_DRIVE_JOYSTICK_INPUT = 0.1, + MIN_ARM_JOYSTICK_INPUT = 0.1; + // Drivetrain public static final int FRONT_LEFT_ADDRESS = 4, FRONT_RIGHT_ADDRESS = 5, REAR_LEFT_ADDRESS = 3, REAR_RIGHT_ADDRESS = 6; public static final double MAX_DRIVE_SPEED = 0.7; + // Claw public static final int CLAW_FORWARD_CHANNEL = 0, CLAW_REVERSE_CHANNEL = 1; public static final Value OPEN = Value.kForward, CLOSED = Value.kReverse; + + // Arm + public static final int LEFT_WINCH_ADDRESS = 2, RIGHT_WINCH_ADDRESS = 7; + + // 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; } diff --git a/src/org/usfirst/frc/team3501/robot/autons/DriveOverStep.java b/src/org/usfirst/frc/team3501/robot/autons/DriveOverStep.java new file mode 100644 index 0000000..2298661 --- /dev/null +++ b/src/org/usfirst/frc/team3501/robot/autons/DriveOverStep.java @@ -0,0 +1,29 @@ +package org.usfirst.frc.team3501.robot.autons; + +import org.usfirst.frc.team3501.robot.RobotMap; +import org.usfirst.frc.team3501.robot.commands.CommandBase; + +public class DriveOverStep extends CommandBase { + + private double speed; + + public DriveOverStep() { + super("DriveOverStep"); + requires(drivetrain); + + setTimeout(RobotMap.OVER_STEP_TIME); + this.speed = RobotMap.OVER_STEP_SPEED; + } + + protected void execute() { + drivetrain.goForward(speed); + } + + protected boolean isFinished() { + return isTimedOut(); + } + + protected void end() { + drivetrain.stop(); + } +} diff --git a/src/org/usfirst/frc/team3501/robot/autons/DrivePastStep.java b/src/org/usfirst/frc/team3501/robot/autons/DrivePastStep.java new file mode 100644 index 0000000..d3105f8 --- /dev/null +++ b/src/org/usfirst/frc/team3501/robot/autons/DrivePastStep.java @@ -0,0 +1,29 @@ +package org.usfirst.frc.team3501.robot.autons; + +import org.usfirst.frc.team3501.robot.RobotMap; +import org.usfirst.frc.team3501.robot.commands.CommandBase; + +public class DrivePastStep extends CommandBase { + + private double speed; + + public DrivePastStep() { + super("DrivePastStep"); + requires(drivetrain); + + setTimeout(RobotMap.PAST_STEP_TIME); + this.speed = RobotMap.PAST_STEP_SPEED; + } + + protected void execute() { + drivetrain.goForward(speed); + } + + protected boolean isFinished() { + return isTimedOut(); + } + + protected void end() { + drivetrain.stop(); + } +} diff --git a/src/org/usfirst/frc/team3501/robot/commands/DriveForward.java b/src/org/usfirst/frc/team3501/robot/commands/DriveForward.java deleted file mode 100644 index 0590b6b..0000000 --- a/src/org/usfirst/frc/team3501/robot/commands/DriveForward.java +++ /dev/null @@ -1,26 +0,0 @@ -package org.usfirst.frc.team3501.robot.commands; - -public class DriveForward extends CommandBase { - - private double speed; - - public DriveForward(double time, double speed) { - super("DriveForward"); - requires(drivetrain); - - setTimeout(time); - this.speed = speed; - } - - protected void execute() { - drivetrain.goForward(speed); - } - - protected boolean isFinished() { - return isTimedOut(); - } - - protected void end() { - drivetrain.stop(); - } -} diff --git a/src/org/usfirst/frc/team3501/robot/commands/DriveWithJoysticks.java b/src/org/usfirst/frc/team3501/robot/commands/DriveWithJoysticks.java index d1f3e50..500bad9 100644 --- a/src/org/usfirst/frc/team3501/robot/commands/DriveWithJoysticks.java +++ b/src/org/usfirst/frc/team3501/robot/commands/DriveWithJoysticks.java @@ -8,7 +8,7 @@ public class DriveWithJoysticks extends CommandBase { } protected void execute() { - drivetrain.drive(oi.getForwardSpeed(), oi.getTwistSpeed()); + drivetrain.drive(oi.getForward(), oi.getTwist()); } protected boolean isFinished() { diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/Arm.java b/src/org/usfirst/frc/team3501/robot/subsystems/Arm.java index deffbdd..8cfcf9b 100644 --- a/src/org/usfirst/frc/team3501/robot/subsystems/Arm.java +++ b/src/org/usfirst/frc/team3501/robot/subsystems/Arm.java @@ -1,18 +1,41 @@ package org.usfirst.frc.team3501.robot.subsystems; +import org.usfirst.frc.team3501.robot.RobotMap; + +import edu.wpi.first.wpilibj.CANJaguar; import edu.wpi.first.wpilibj.command.Subsystem; -/** - * - */ public class Arm extends Subsystem { - - // Put methods for controlling this subsystem - // here. Call these from Commands. - public void initDefaultCommand() { - // Set the default command for a subsystem here. - //setDefaultCommand(new MySpecialCommand()); + private CANJaguar left, right; + + public Arm() { + left = new CANJaguar(RobotMap.LEFT_WINCH_ADDRESS); + right = new CANJaguar(RobotMap.RIGHT_WINCH_ADDRESS); + } + + public void set(double speed) { + left.set(-speed); + right.set(speed); + } + + public void moveLeft(double speed) { + left.set(speed); + right.set(0); } + + public void moveRight(double speed) { + right.set(speed); + left.set(0); + } + + public double getSpeedFromJoystick(double speed) { + if (Math.abs(speed) < RobotMap.MIN_ARM_JOYSTICK_INPUT) + speed = 0; + + return speed; + } + + public void initDefaultCommand() {} } diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/Claw.java b/src/org/usfirst/frc/team3501/robot/subsystems/Claw.java index aecab3e..5c9ae5b 100644 --- a/src/org/usfirst/frc/team3501/robot/subsystems/Claw.java +++ b/src/org/usfirst/frc/team3501/robot/subsystems/Claw.java @@ -1,24 +1,19 @@ package org.usfirst.frc.team3501.robot.subsystems; import org.usfirst.frc.team3501.robot.RobotMap; -import org.usfirst.frc.team3501.robot.commands.*; import edu.wpi.first.wpilibj.DoubleSolenoid; import edu.wpi.first.wpilibj.command.Subsystem; public class Claw extends Subsystem { - private final DoubleSolenoid piston; + private DoubleSolenoid piston; public Claw() { piston = new DoubleSolenoid( RobotMap.CLAW_FORWARD_CHANNEL, RobotMap.CLAW_REVERSE_CHANNEL); } - public void initDefaultCommand() { - setDefaultCommand(new CloseClaw()); - } - public void open() { piston.set(RobotMap.OPEN); } @@ -26,5 +21,6 @@ public class Claw extends Subsystem { public void close() { piston.set(RobotMap.CLOSED); } -} + public void initDefaultCommand() {} +} diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/Drivetrain.java b/src/org/usfirst/frc/team3501/robot/subsystems/Drivetrain.java index 5285956..5dd39ab 100644 --- a/src/org/usfirst/frc/team3501/robot/subsystems/Drivetrain.java +++ b/src/org/usfirst/frc/team3501/robot/subsystems/Drivetrain.java @@ -1,6 +1,7 @@ package org.usfirst.frc.team3501.robot.subsystems; import org.usfirst.frc.team3501.robot.RobotMap; +import org.usfirst.frc.team3501.robot.commands.DriveWithJoysticks; import edu.wpi.first.wpilibj.CANJaguar; import edu.wpi.first.wpilibj.RobotDrive; @@ -8,7 +9,7 @@ import edu.wpi.first.wpilibj.command.Subsystem; public class Drivetrain extends Subsystem { - RobotDrive robotDrive; + private RobotDrive robotDrive; public Drivetrain() { CANJaguar frontLeft = new CANJaguar(RobotMap.FRONT_LEFT_ADDRESS); @@ -46,6 +47,8 @@ public class Drivetrain extends Subsystem { return (x + Math.signum(x) * Math.sqrt(Math.abs(x))) / 2; } - public void initDefaultCommand() {} + public void initDefaultCommand() { + setDefaultCommand(new DriveWithJoysticks()); + } } diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/Pneumatics.java b/src/org/usfirst/frc/team3501/robot/subsystems/Pneumatics.java new file mode 100644 index 0000000..8a831d5 --- /dev/null +++ b/src/org/usfirst/frc/team3501/robot/subsystems/Pneumatics.java @@ -0,0 +1,24 @@ +package org.usfirst.frc.team3501.robot.subsystems; + +import edu.wpi.first.wpilibj.Compressor; +import edu.wpi.first.wpilibj.command.Subsystem; + +public class Pneumatics extends Subsystem { + + private Compressor compressor; + + public Pneumatics() { + compressor = new Compressor(); + } + + public void start() { + compressor.start(); + } + + public void stop() { + compressor.stop(); + } + + public void initDefaultCommand() {} +} + -- 2.30.2