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;
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;
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 {
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;
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);
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());
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() {
--- /dev/null
+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));
+ }
+
+}
--- /dev/null
+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();
+ }
+}
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);
}
}
/***
- * 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;
}
/***
}
public void stop() {
- setArmSpeed(0);
+ setArmSpeeds(0);
}
public double getAngleForLevel(double targetLevel) {
projects / 3501 / stronghold-2016 / commitdiff