From 0a179caa3cb1017af9fc7dab08c7123c3407e84f Mon Sep 17 00:00:00 2001 From: Kevin Zhang Date: Sat, 20 Feb 2016 15:56:14 -0800 Subject: [PATCH] Add buttons to move intake arm. --- .../usfirst/frc/team3501/robot/Constants.java | 15 ++- src/org/usfirst/frc/team3501/robot/OI.java | 97 ++++++++++++++----- .../intakearm/MoveIntakeArmToAngle.java | 4 +- .../intakearm/RunBothIntakeMotors.java | 26 +++++ .../commands/intakearm/RunIntakeMotor.java | 57 +++++++++++ .../team3501/robot/subsystems/IntakeArm.java | 60 ++++++++---- 6 files changed, 214 insertions(+), 45 deletions(-) create mode 100644 src/org/usfirst/frc/team3501/robot/commands/intakearm/RunBothIntakeMotors.java create mode 100644 src/org/usfirst/frc/team3501/robot/commands/intakearm/RunIntakeMotor.java diff --git a/src/org/usfirst/frc/team3501/robot/Constants.java b/src/org/usfirst/frc/team3501/robot/Constants.java index 520959a9..262d7846 100644 --- a/src/org/usfirst/frc/team3501/robot/Constants.java +++ b/src/org/usfirst/frc/team3501/robot/Constants.java @@ -28,11 +28,18 @@ public class Constants { public final static int ARCADE_INTAKEARM_LEVEL_FOUR_PORT = 0; public final static int LEFT_JOYSTICK_TRIGGER_PORT = 1; - public final static int SPIN1_PORT = 4; - public final static int SPIN2_PORT = 5; + public final static int LEFT_JOYSTICK_TOP_LEFT_BUTTON = 4; + public final static int LEFT_JOYSTICK_TOP_RIGHT_BUTTON = 5; public final static int LEFT_JOYSTICK_TOP_CENTER_PORT = 3; public final static int LEFT_JOYSTICK_TOP_LOW_PORT = 2; + public final static int LEFT_JOYSTICK_BOTTOM_BACK_LEFT_BUTTON = 8; + public final static int LEFT_JOYSTICK_BOTTOM_BACK_RIGHT_BUTTON = 9; + public final static int LEFT_JOYSTICK_BOTTOM_LEFT_FORWARD_BUTTON = 6; + public final static int LEFT_JOYSTICK_BOTTOM_LEFT_BACK_BUTTON = 7; + public final static int LEFT_JOYSTICK_BOTTOM_RIGHT_FORWARD_BUTTON = 11; + public final static int LEFT_JOYSTICK_BOTTOM_RIGHT_BACK_BUTTON = 10; + public final static int RIGHT_JOYSTICK_TRIGGER_PORT = 1; public final static int RIGHT_JOYSTICK_THUMB_PORT = 2; @@ -131,7 +138,8 @@ public class Constants { public static class IntakeArm { public static final int ROLLER_PORT = 0; - public static final int ARM_PORT = 1; + public static final int LEFT_ARM_PORT = 1; + public static final int RIGHT_ARM_PORT = 1; public static final int POT_CHANNEL = 0; public static final double INTAKE_SPEED = 0.5; public static final double OUTPUT_SPEED = -0.5; @@ -139,6 +147,7 @@ public class Constants { public final static double OFFSET = -135.0; // in degrees public static final double ZERO_ANGLE = 0; public static final double DEFAULT_INTAKE_ARM_SPEED = 0.3; + public static final double STOP_INTAKE_ARM_SPEED = 0.0; } public static class DefenseArm { diff --git a/src/org/usfirst/frc/team3501/robot/OI.java b/src/org/usfirst/frc/team3501/robot/OI.java index 07c47e84..75d6beff 100644 --- a/src/org/usfirst/frc/team3501/robot/OI.java +++ b/src/org/usfirst/frc/team3501/robot/OI.java @@ -2,6 +2,8 @@ package org.usfirst.frc.team3501.robot; import org.usfirst.frc.team3501.robot.commands.driving.ChangeGear; import org.usfirst.frc.team3501.robot.commands.shooter.Shoot; +import org.usfirst.frc.team3501.robot.commands.intakearm.RunBothIntakeMotors; +import org.usfirst.frc.team3501.robot.commands.intakearm.RunIntakeMotor; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.buttons.Button; @@ -11,33 +13,40 @@ public class OI { public static Joystick leftJoystick; public static Joystick rightJoystick; - // first column of arcade buttons - getting past defenses - public static DigitalButton passPortcullis; - public static DigitalButton passChevalDeFrise; - public static DigitalButton passDrawbridge; - public static DigitalButton passSallyPort; - - // second column of arcade buttons - different angles for intake arm - // TO DO: change position numbers to angle values (?) - public static DigitalButton lowerChevalDeFrise; - public static DigitalButton moveToIntakeBoulder; - public static DigitalButton poiseAboveChevalDeFrise; - public static DigitalButton moveIntakeArmInsideRobot; - - // left joystick buttons - public static Button toggleShooter; - public static Button SpinRobot180_1; // both do the same thing, just two - public static Button SpinRobot180_2; // different buttons - public static Button compactRobot_1; - public static Button compactRobot_2; + public static Button leftIntakeArmMotorUp; + public static Button leftIntakeArmMotorDown; + public static Button rightIntakeArmMotorUp; + public static Button rightIntakeArmMotorDown; + public static Button bothIntakeArmMotorUp; + public static Button bothIntakeArmMotorDown; + // // first column of arcade buttons - getting past defenses + // public static DigitalButton passPortcullis; + // public static DigitalButton passChevalDeFrise; + // public static DigitalButton passDrawbridge; + // public static DigitalButton passSallyPort; + // + // // second column of arcade buttons - different angles for intake arm + // // TO DO: change position numbers to angle values (?) + // public static DigitalButton lowerChevalDeFrise; + // public static DigitalButton moveToIntakeBoulder; + // public static DigitalButton poiseAboveChevalDeFrise; + // public static DigitalButton moveIntakeArmInsideRobot; + // + // // left joystick buttons + // public static Button toggleShooter; + // public static Button SpinRobot180_1; // both do the same thing, just two + // public static Button SpinRobot180_2; // different buttons + // public static Button compactRobot_1; + // public static Button compactRobot_2; + // // right joystick buttons public static Button intakeBoulder; public static Button shootBoulder; public static Button toggleGear; - - // button to change robot to the scaling mode - public static DigitalButton toggleScaling; + // + // // button to change robot to the scaling mode + // public static DigitalButton toggleScaling; public OI() { leftJoystick = new Joystick(Constants.OI.LEFT_STICK_PORT); @@ -51,6 +60,50 @@ public class OI { Constants.OI.LEFT_JOYSTICK_TRIGGER_PORT); shootBoulder.whenPressed(new Shoot()); + recordPotAngle1 = new JoystickButton(leftJoystick, + Constants.OI.LEFT_JOYSTICK_TOP_LEFT_BUTTON); + recordPotAngle1.whenPressed(new RecordPotAngle(0)); + recordPotAngle2 = new JoystickButton(leftJoystick, + Constants.OI.LEFT_JOYSTICK_TOP_CENTER_BUTTON); + recordPotAngle2.whenPressed(new RecordPotAngle(1)); + recordPotAngle3 = new JoystickButton(leftJoystick, + Constants.OI.LEFT_JOYSTICK_TOP_RIGHT_BUTTON); + recordPotAngle3.whenPressed(new RecordPotAngle(2)); + recordPotAngle4 = new JoystickButton(leftJoystick, + Constants.OI.LEFT_JOYSTICK_TOP_LOW_BUTTON); + recordPotAngle4.whenPressed(new RecordPotAngle(3)); + + leftIntakeArmMotorUp = new JoystickButton(leftJoystick, + Constants.OI.LEFT_JOYSTICK_BOTTOM_LEFT_FORWARD_BUTTON); + leftIntakeArmMotorUp.whenPressed(new RunIntakeMotor( + Robot.intakeArm.getLeftIntakeArmMotor(), + Constants.IntakeArm.DEFAULT_INTAKE_ARM_SPEED)); + leftIntakeArmMotorDown = new JoystickButton(leftJoystick, + Constants.OI.LEFT_JOYSTICK_BOTTOM_LEFT_BACK_BUTTON); + leftIntakeArmMotorDown.whenPressed(new RunIntakeMotor( + Robot.intakeArm.getLeftIntakeArmMotor(), + -Constants.IntakeArm.DEFAULT_INTAKE_ARM_SPEED)); + + rightIntakeArmMotorUp = new JoystickButton(leftJoystick, + Constants.OI.LEFT_JOYSTICK_BOTTOM_RIGHT_FORWARD_BUTTON); + rightIntakeArmMotorUp.whenPressed(new RunIntakeMotor( + Robot.intakeArm.getRightIntakeArmMotor(), + Constants.IntakeArm.DEFAULT_INTAKE_ARM_SPEED)); + rightIntakeArmMotorDown = new JoystickButton(leftJoystick, + Constants.OI.LEFT_JOYSTICK_BOTTOM_RIGHT_BACK_BUTTON); + rightIntakeArmMotorDown.whenPressed(new RunIntakeMotor( + Robot.intakeArm.getRightIntakeArmMotor(), + -Constants.IntakeArm.DEFAULT_INTAKE_ARM_SPEED)); + + bothIntakeArmMotorUp = new JoystickButton(leftJoystick, + Constants.OI.LEFT_JOYSTICK_BOTTOM_BACK_LEFT_BUTTON); + bothIntakeArmMotorUp.whenPressed(new RunBothIntakeMotors( + Constants.IntakeArm.DEFAULT_INTAKE_ARM_SPEED)); + bothIntakeArmMotorDown = new JoystickButton(leftJoystick, + Constants.OI.LEFT_JOYSTICK_BOTTOM_BACK_RIGHT_BUTTON); + bothIntakeArmMotorDown.whenPressed(new RunBothIntakeMotors( + -Constants.IntakeArm.DEFAULT_INTAKE_ARM_SPEED)); + // passPortcullis = new DigitalButton( // new DigitalInput(Constants.OI.PASS_PORTCULLIS_PORT)); // passPortcullis.whenPressed(new PassPortcullis()); diff --git a/src/org/usfirst/frc/team3501/robot/commands/intakearm/MoveIntakeArmToAngle.java b/src/org/usfirst/frc/team3501/robot/commands/intakearm/MoveIntakeArmToAngle.java index 510146cc..25a109bb 100644 --- a/src/org/usfirst/frc/team3501/robot/commands/intakearm/MoveIntakeArmToAngle.java +++ b/src/org/usfirst/frc/team3501/robot/commands/intakearm/MoveIntakeArmToAngle.java @@ -27,13 +27,13 @@ public class MoveIntakeArmToAngle extends Command { if (speed < MIN_SPEED && speed > 0) speed = Math.signum(getCalculatedSpeed()) * MIN_SPEED; - Robot.intakeArm.setArmSpeed(speed); + Robot.intakeArm.setArmSpeeds(speed); } @Override protected void execute() { // set the arm speed to the calculated angle - Robot.intakeArm.setArmSpeed(calculatedSpeed); + Robot.intakeArm.setArmSpeeds(calculatedSpeed); } private double getError() { diff --git a/src/org/usfirst/frc/team3501/robot/commands/intakearm/RunBothIntakeMotors.java b/src/org/usfirst/frc/team3501/robot/commands/intakearm/RunBothIntakeMotors.java new file mode 100644 index 00000000..a2d4f4c3 --- /dev/null +++ b/src/org/usfirst/frc/team3501/robot/commands/intakearm/RunBothIntakeMotors.java @@ -0,0 +1,26 @@ +package org.usfirst.frc.team3501.robot.commands.intakearm; + +import org.usfirst.frc.team3501.robot.Robot; + +import edu.wpi.first.wpilibj.command.CommandGroup; + +/*** + * This command runs both intake arm motors at a given speed momentarily. + * + * @author harel, garima + * + */ +public class RunBothIntakeMotors extends CommandGroup { + + /*** + * @param motorSpeed + * Speed at which to run intake arm motors. Range is [-1,1] + */ + public RunBothIntakeMotors(double motorSpeed) { + addParallel(new RunIntakeMotor(Robot.intakeArm.getLeftIntakeArmMotor(), + motorSpeed)); + addParallel(new RunIntakeMotor(Robot.intakeArm.getRightIntakeArmMotor(), + motorSpeed)); + } + +} diff --git a/src/org/usfirst/frc/team3501/robot/commands/intakearm/RunIntakeMotor.java b/src/org/usfirst/frc/team3501/robot/commands/intakearm/RunIntakeMotor.java new file mode 100644 index 00000000..6d95b994 --- /dev/null +++ b/src/org/usfirst/frc/team3501/robot/commands/intakearm/RunIntakeMotor.java @@ -0,0 +1,57 @@ +package org.usfirst.frc.team3501.robot.commands.intakearm; + +import org.usfirst.frc.team3501.robot.Constants; +import org.usfirst.frc.team3501.robot.Robot; + +import edu.wpi.first.wpilibj.CANTalon; +import edu.wpi.first.wpilibj.command.Command; + +/*** + * This command runs the given motor (intended to be an intake arm motor) at the + * given speed momentarily. + * + * @author harel, garima + * + */ + +public class RunIntakeMotor extends Command { + CANTalon intakeMotor; + double motorSpeed; + + /*** + * @param intakeMotor + * Motor to run. + * @param motorSpeed + * Speed at which to run motor. Range is [-1,1] + */ + public RunIntakeMotor(CANTalon intakeMotor, double motorSpeed) { + requires(Robot.intakeArm); + this.intakeMotor = intakeMotor; + this.motorSpeed = motorSpeed; + } + + @Override + protected void initialize() { + intakeMotor.set(motorSpeed); + } + + @Override + protected void execute() { + + } + + @Override + protected boolean isFinished() { + return true; + } + + @Override + protected void end() { + intakeMotor.set(Constants.IntakeArm.STOP_INTAKE_ARM_SPEED); + } + + @Override + protected void interrupted() { + end(); + } +} diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/IntakeArm.java b/src/org/usfirst/frc/team3501/robot/subsystems/IntakeArm.java index 5e00d05f..8c87ed1c 100755 --- a/src/org/usfirst/frc/team3501/robot/subsystems/IntakeArm.java +++ b/src/org/usfirst/frc/team3501/robot/subsystems/IntakeArm.java @@ -21,14 +21,16 @@ import edu.wpi.first.wpilibj.command.Subsystem; public class IntakeArm extends Subsystem { private CANTalon intakeRoller; - private CANTalon intakeArm; + private CANTalon leftIntakeArm; + private CANTalon rightIntakeArm; private AnalogPotentiometer intakePot; public static double[] potAngles = { 0, 30, 45, 90 }; // TODO: correct angles public static double moveIntakeArmSpeed = 0; public IntakeArm() { intakeRoller = new CANTalon(Constants.IntakeArm.ROLLER_PORT); - intakeArm = new CANTalon(Constants.IntakeArm.ARM_PORT); + leftIntakeArm = new CANTalon(Constants.IntakeArm.LEFT_ARM_PORT); + rightIntakeArm = new CANTalon(Constants.IntakeArm.RIGHT_ARM_PORT); intakePot = new AnalogPotentiometer(Constants.IntakeArm.POT_CHANNEL, Constants.IntakeArm.FULL_RANGE, Constants.IntakeArm.OFFSET); } @@ -68,36 +70,58 @@ public class IntakeArm extends Subsystem { } /*** - * This method sets the voltage of the arm motor. The range is from [-1,1]. A - * negative voltage makes the direction of the motor go backwards. + * This method sets the voltages of the arm motors. The range is from [-1,1]. + * A negative voltage makes the direction of the motor go backwards. * * @param voltage - * The voltage that you set the motor at. The range of the voltage of - * the arm motor is from [-1,1]. A negative voltage makes the - * direction of the motor go backwards. + * The voltage that you set the motors at. The range of the voltage + * of the arm motors is from [-1,1]. A negative voltage makes the + * direction of the motors go backwards. */ - public void setArmSpeed(double voltage) { + public void setArmSpeeds(double voltage) { if (voltage > 1) voltage = 1; else if (voltage < -1) voltage = -1; - intakeArm.set(voltage); + leftIntakeArm.set(voltage); + rightIntakeArm.set(voltage); } /*** - * This method gets you the current voltage of the motor that controls the - * intake arm. The range of voltage is from [-1,1]. A negative voltage makes - * the motor run backwards. + * This method gets you the current voltage of the left motor that controls + * the intake arm. The range of voltage is from [-1,1]. A negative voltage + * makes the motor run backwards. + * + * @return Returns the voltage of the left motor that controls the arm. The + * range of the voltage goes from [-1,1]. A negative voltage indicates + * that the motor is running backwards. + */ + + public double getLeftArmSpeed() { + return leftIntakeArm.get(); + } + + /*** + * This method gets you the current voltage of the right motor that controls + * the intake arm. The range of voltage is from [-1,1]. A negative voltage + * makes the motor run backwards. * - * @return Returns the voltage of the motor that controls the arm. The range - * of the voltage goes from [-1,1]. A negative voltage indicates that - * the motor is running backwards. + * @return Returns the voltage of the right motor that controls the arm. The + * range of the voltage goes from [-1,1]. A negative voltage indicates + * that the motor is running backwards. */ + public double getRightArmSpeed() { + return rightIntakeArm.get(); + } + + public CANTalon getLeftIntakeArmMotor() { + return leftIntakeArm; + } - public double getArmSpeed() { - return intakeArm.get(); + public CANTalon getRightIntakeArmMotor() { + return rightIntakeArm; } /*** @@ -137,7 +161,7 @@ public class IntakeArm extends Subsystem { } public void stop() { - setArmSpeed(0); + setArmSpeeds(0); } public double getAngleForLevel(double targetLevel) { -- 2.30.2