public final static int LEFT_STICK_PORT = 0;
public final static int RIGHT_STICK_PORT = 1;
- public final static int PASS_PORTCULLIS_PORT = 0;
- public final static int PASS_CHEVAL_DE_FRISE_PORT = 0;
- public final static int PASS_DRAWBRIDGE_PORT = 0;
- public final static int PASS_SALLYPORT_PORT = 0;
-
- public final static int ARCADE_INTAKEARM_LEVEL_ONE_PORT = 0;
- public final static int ARCADE_INTAKEARM_LEVEL_TWO_PORT = 0;
- public final static int ARCADE_INTAKEARM_LEVEL_THREE_PORT = 0;
- public final static int ARCADE_INTAKEARM_LEVEL_FOUR_PORT = 0;
-
- public final static int LEFT_JOYSTICK_TRIGGER_PORT = 0;
- public final static int SPIN1_PORT = 4;
- public final static int SPIN2_PORT = 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 RIGHT_JOYSTICK_TRIGGER_PORT = 0;
- public final static int RIGHT_JOYSTICK_THUMB_PORT = 2;
+ public final static int LEFT_JOYSTICK_TRIGGER_PORT = 1;
- public final static int TOGGLE_SCALING_PORT = 0;
+ public final static int RIGHT_JOYSTICK_TRIGGER_PORT = 1;
+ public final static int RIGHT_JOYSTICK_THUMB_PORT = 2;
+ public final static int RIGHT_JOYSTICK_SHOOT_PORT = 3;
}
// Encoder related ports
public final static int ENCODER_LEFT_A = 0;
public final static int ENCODER_LEFT_B = 1;
- public final static int ENCODER_RIGHT_A = 9;
- public final static int ENCODER_RIGHT_B = 8;
+ public final static int ENCODER_RIGHT_A = 3;
+ public final static int ENCODER_RIGHT_B = 4;
+
+ public static final int LEFT_MODULE = PCM_MODULE_B;
+ public static final int LEFT_FORWARD = 6, LEFT_REVERSE = 5;
+ public static final int RIGHT_MODULE = PCM_MODULE_B;
+ public static final int RIGHT_FORWARD = 0, RIGHT_REVERSE = 1;
- public static final double INCHES_PER_PULSE = ((3.66 / 5.14) * 6 * Math.PI)
- / 256;
+ public static final double INCHES_PER_PULSE = ((3.66 / 5.14) * 6 * Math.PI) / 256;
public static double kp = 0.013, ki = 0.000015, kd = -0.002;
public static double gp = 0.018, gi = 0.000015, gd = 0;
public static double encoderTolerance = 8.0, gyroTolerance = 5.0;
public static final int MANUAL_MODE = 1, ENCODER_MODE = 2, GYRO_MODE = 3;
- public static final int LEFT_FORWARD = 6, LEFT_REVERSE = 5,
- RIGHT_FORWARD = 0, RIGHT_REVERSE = 1;
public static double time = 0;
// Gearing constants
}
public static class Shooter {
- public static final int CATAPULT1_MODULE = PCM_MODULE_A;
+ public static final int CATAPULT1_MODULE = PCM_MODULE_B;
public static final int CATAPULT1_FORWARD = 0;
- public static final int CATAPULT1_REVERSE = 1;
+ public static final int CATAPULT1_REVERSE = 6;
public static final int CATAPULT2_MODULE = PCM_MODULE_B;
- public static final int CATAPULT2_FORWARD = 2;
- public static final int CATAPULT2_REVERSE = 3;// TODO Determine actual
+ public static final int CATAPULT2_FORWARD = 1;
+ public static final int CATAPULT2_REVERSE = 7;// TODO Determine actual
// shooter ports
public static final Value shoot = Value.kForward;
}
public static class IntakeArm {
- public static final int ROLLER_PORT = 0;
+ public static final int ROLLER_PORT = 3;
// for pistons
public static final int LEFT_FORWARD = 0;
package org.usfirst.frc.team3501.robot;
-import org.usfirst.frc.team3501.robot.commands.auton.CompactRobot;
-import org.usfirst.frc.team3501.robot.commands.auton.PassChevalDeFrise;
-import org.usfirst.frc.team3501.robot.commands.auton.PassDrawBridge;
-import org.usfirst.frc.team3501.robot.commands.auton.PassPortcullis;
-import org.usfirst.frc.team3501.robot.commands.auton.PassSallyPort;
-import org.usfirst.frc.team3501.robot.commands.driving.Turn180;
-import org.usfirst.frc.team3501.robot.commands.intakearm.IntakeBall;
-import org.usfirst.frc.team3501.robot.commands.scaler.ExtendLift;
-import org.usfirst.frc.team3501.robot.commands.scaler.RetractLift;
-import org.usfirst.frc.team3501.robot.commands.scaler.RunWinchContinuous;
-import org.usfirst.frc.team3501.robot.commands.scaler.StopWinch;
-import org.usfirst.frc.team3501.robot.commands.scaler.ToggleScaling;
+import org.usfirst.frc.team3501.robot.commands.driving.SetHighGear;
+import org.usfirst.frc.team3501.robot.commands.driving.SetLowGear;
+import org.usfirst.frc.team3501.robot.commands.driving.ToggleFront;
+import org.usfirst.frc.team3501.robot.commands.intakearm.RunIntake;
+import org.usfirst.frc.team3501.robot.commands.intakearm.StopIntake;
import org.usfirst.frc.team3501.robot.commands.shooter.Shoot;
+import org.usfirst.frc.team3501.robot.subsystems.IntakeArm;
+
-import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.buttons.Button;
import edu.wpi.first.wpilibj.buttons.JoystickButton;
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 gearTrigger;
// right joystick buttons
- public static Button intakeBoulder;
+ public static Button intakeTrigger;
public static Button shootBoulder;
-
- // button to change robot to the scaling mode
- public static DigitalButton toggleScaling;
+ public static Button toggleFront;
public OI() {
leftJoystick = new Joystick(Constants.OI.LEFT_STICK_PORT);
rightJoystick = new Joystick(Constants.OI.RIGHT_STICK_PORT);
- passPortcullis = new DigitalButton(
- new DigitalInput(Constants.OI.PASS_PORTCULLIS_PORT));
- passPortcullis.whenPressed(new PassPortcullis());
-
- passChevalDeFrise = new DigitalButton(
- new DigitalInput(Constants.OI.PASS_CHEVAL_DE_FRISE_PORT));
- passChevalDeFrise.whenPressed(new PassChevalDeFrise());
-
- passDrawbridge = new DigitalButton(
- new DigitalInput(Constants.OI.PASS_DRAWBRIDGE_PORT));
- passDrawbridge.whenPressed(new PassDrawBridge());
-
- passSallyPort = new DigitalButton(
- new DigitalInput(Constants.OI.PASS_SALLYPORT_PORT));
- passSallyPort.whenPressed(new PassSallyPort());
-
- moveToIntakeBoulder = new DigitalButton(
- new DigitalInput(Constants.OI.ARCADE_INTAKEARM_LEVEL_TWO_PORT));
-
- toggleShooter = new JoystickButton(leftJoystick,
+ // Left joystick
+ gearTrigger = new JoystickButton(leftJoystick,
Constants.OI.LEFT_JOYSTICK_TRIGGER_PORT);
- SpinRobot180_1 = new JoystickButton(leftJoystick, Constants.OI.SPIN1_PORT);
- SpinRobot180_1.whenPressed(new Turn180());
+ gearTrigger.whenPressed(new SetHighGear());
+ gearTrigger.whenReleased(new SetLowGear());
- SpinRobot180_2 = new JoystickButton(leftJoystick, Constants.OI.SPIN2_PORT);
- SpinRobot180_2.whenPressed(new Turn180());
-
- compactRobot_1 = new JoystickButton(leftJoystick,
- Constants.OI.LEFT_JOYSTICK_TOP_CENTER_PORT);
- compactRobot_2 = new JoystickButton(leftJoystick,
- Constants.OI.LEFT_JOYSTICK_TOP_LOW_PORT);
-
- intakeBoulder = new JoystickButton(rightJoystick,
+ // Right joystick
+ intakeTrigger = new JoystickButton(rightJoystick,
Constants.OI.RIGHT_JOYSTICK_TRIGGER_PORT);
- shootBoulder = new JoystickButton(rightJoystick,
- Constants.OI.RIGHT_JOYSTICK_THUMB_PORT);
-
- toggleScaling = new DigitalButton(
- new DigitalInput(Constants.OI.TOGGLE_SCALING_PORT));
- toggleScaling.whenPressed(new ToggleScaling());
+ intakeTrigger.whenPressed(new RunIntake());
+ intakeTrigger.whenReleased(new StopIntake());
- if (!Constants.Scaler.SCALING) {
- compactRobot_1.whenPressed(new CompactRobot());
- compactRobot_2.whenPressed(new CompactRobot());
-
- intakeBoulder.whenPressed(new IntakeBall());
- shootBoulder.whenPressed(new Shoot());
-
- } else {
- // toggleShooter becomes winch
- // compact robot button 1 and 2 retracts the lift
- // intake button stops the winch
- // shoot button extends the lift
- toggleShooter.whenPressed(new RunWinchContinuous(
- Constants.Scaler.SCALE_SPEED, Constants.Scaler.SECONDS_TO_SCALE));
- compactRobot_1.whenPressed(new RetractLift());
- compactRobot_2.whenPressed(new RetractLift());
+ shootBoulder = new JoystickButton(rightJoystick,
+ Constants.OI.RIGHT_JOYSTICK_SHOOT_PORT);
+ shootBoulder.whenPressed(new Shoot());
- intakeBoulder.whenReleased(new StopWinch());
- shootBoulder.whenPressed(new ExtendLift());
- }
+ toggleFront = new JoystickButton(rightJoystick,
+ Constants.OI.RIGHT_JOYSTICK_THUMB_PORT);
+ toggleFront.whenPressed(new ToggleFront());
}
}
package org.usfirst.frc.team3501.robot;
import org.usfirst.frc.team3501.robot.Constants.Defense;
+import org.usfirst.frc.team3501.robot.commands.driving.SetLowGear;
import org.usfirst.frc.team3501.robot.commands.shooter.ResetCatapult;
import org.usfirst.frc.team3501.robot.subsystems.DefenseArm;
import org.usfirst.frc.team3501.robot.subsystems.DriveTrain;
@Override
public void teleopInit() {
+ Scheduler.getInstance().add(new SetLowGear()); // Start each match in low
+ // gear
Scheduler.getInstance().add(new ResetCatapult()); // Reset catapult at start
// of each match.
}
/**
* Runs throughout teleop and listens for joystick inputs and drives the
- * driveTrain
- * Never finishes until teleop ends
+ * driveTrain Never finishes until teleop ends
*/
public class JoystickDrive extends Command {
@Override
protected void execute() {
+ double k = (Robot.driveTrain.isFlipped() ? -1 : 1);
// IDK why but the joystick gives positive values for pulling backwards
double left = -Robot.oi.leftJoystick.getY();
double right = -Robot.oi.rightJoystick.getY();
- Robot.driveTrain.drive(left, right);
+ Robot.driveTrain.drive(left * k, right * k);
}
@Override
--- /dev/null
+package org.usfirst.frc.team3501.robot.commands.driving;
+
+import org.usfirst.frc.team3501.robot.Robot;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ *
+ */
+public class SetHighGear extends Command {
+
+ public SetHighGear() {
+ requires(Robot.driveTrain);
+ }
+
+ // Called just before this Command runs the first time
+ @Override
+ protected void initialize() {
+ Robot.driveTrain.setHighGear();
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ @Override
+ protected void execute() {
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ @Override
+ protected boolean isFinished() {
+ return true;
+ }
+
+ // Called once after isFinished returns true
+ @Override
+ protected void end() {
+ }
+
+ // Called when another command which requires one or more of the same
+ // subsystems is scheduled to run
+ @Override
+ protected void interrupted() {
+ }
+}
--- /dev/null
+package org.usfirst.frc.team3501.robot.commands.driving;
+
+import org.usfirst.frc.team3501.robot.Robot;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ *
+ */
+public class SetLowGear extends Command {
+
+ public SetLowGear() {
+ requires(Robot.driveTrain);
+ }
+
+ // Called just before this Command runs the first time
+ @Override
+ protected void initialize() {
+ Robot.driveTrain.setLowGear();
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ @Override
+ protected void execute() {
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ @Override
+ protected boolean isFinished() {
+ return true;
+ }
+
+ // Called once after isFinished returns true
+ @Override
+ protected void end() {
+ }
+
+ // Called when another command which requires one or more of the same
+ // subsystems is scheduled to run
+ @Override
+ protected void interrupted() {
+ }
+}
--- /dev/null
+package org.usfirst.frc.team3501.robot.commands.driving;
+
+import org.usfirst.frc.team3501.robot.Robot;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ *
+ */
+public class ToggleFront extends Command {
+
+ public ToggleFront() {
+ requires(Robot.driveTrain);
+ }
+
+ // Called just before this Command runs the first time
+ @Override
+ protected void initialize() {
+ Robot.driveTrain.switchGear();
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ @Override
+ protected void execute() {
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ @Override
+ protected boolean isFinished() {
+ return true;
+ }
+
+ // Called once after isFinished returns true
+ @Override
+ protected void end() {
+ }
+
+ // Called when another command which requires one or more of the same
+ // subsystems is scheduled to run
+ @Override
+ protected void interrupted() {
+ }
+}
--- /dev/null
+package org.usfirst.frc.team3501.robot.commands.driving;
+
+import org.usfirst.frc.team3501.robot.Robot;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ *
+ */
+public class ToggleGear extends Command {
+
+ public ToggleGear() {
+ requires(Robot.driveTrain);
+ }
+
+ // Called just before this Command runs the first time
+ @Override
+ protected void initialize() {
+ Robot.driveTrain.toggleFlipped();
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ @Override
+ protected void execute() {
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ @Override
+ protected boolean isFinished() {
+ return true;
+ }
+
+ // Called once after isFinished returns true
+ @Override
+ protected void end() {
+ }
+
+ // Called when another command which requires one or more of the same
+ // subsystems is scheduled to run
+ @Override
+ protected void interrupted() {
+ }
+}
--- /dev/null
+package org.usfirst.frc.team3501.robot.commands.intakearm;
+
+import org.usfirst.frc.team3501.robot.Robot;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+public class RunIntake extends Command {
+
+ public RunIntake() {
+ requires(Robot.intakeArm);
+ }
+
+ @Override
+ protected void initialize() {
+ Robot.intakeArm.intakeBall();
+ }
+
+ @Override
+ protected void execute() {
+ }
+
+ @Override
+ protected boolean isFinished() {
+ return true;
+ }
+
+ @Override
+ protected void end() {
+ }
+
+ @Override
+ protected void interrupted() {
+ }
+
+}
--- /dev/null
+package org.usfirst.frc.team3501.robot.commands.intakearm;
+
+import org.usfirst.frc.team3501.robot.Robot;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/***
+ * This command will take a boulder into the robot if there is not a boulder
+ * present inside already.
+ *
+ * pre-condition: Intake arm must be at correct height and a boulder is not
+ * present inside the robot.
+ *
+ * post-condition: A boulder is taken in from the field outside of the robot
+ * into the robot.
+ *
+ * @author Lauren and Niyati
+ *
+ */
+
+public class StopIntake extends Command {
+
+ public StopIntake() {
+ requires(Robot.intakeArm);
+ }
+
+ @Override
+ protected void initialize() {
+ Robot.intakeArm.stopRollers();
+ }
+
+ @Override
+ protected void execute() {
+ }
+
+ @Override
+ protected boolean isFinished() {
+ return true;
+ }
+
+ @Override
+ protected void end() {
+ }
+
+ @Override
+ protected void interrupted() {
+ }
+
+}
public class DriveTrain extends PIDSubsystem {
// Current Drive Mode Default Drive Mode is Manual
private int DRIVE_MODE = 1;
+ private boolean outputFlipped = false;
private static double pidOutput = 0;
private Encoder leftEncoder, rightEncoder;
this.disable();
gyro.start();
- leftGearPiston = new DoubleSolenoid(10, Constants.DriveTrain.LEFT_FORWARD,
- Constants.DriveTrain.LEFT_REVERSE);
- rightGearPiston = new DoubleSolenoid(10,
- Constants.DriveTrain.RIGHT_FORWARD,
- Constants.DriveTrain.RIGHT_REVERSE);
+ leftGearPiston = new DoubleSolenoid(Constants.DriveTrain.LEFT_MODULE,
+ Constants.DriveTrain.LEFT_FORWARD, Constants.DriveTrain.LEFT_REVERSE);
+ rightGearPiston = new DoubleSolenoid(Constants.DriveTrain.RIGHT_MODULE,
+ Constants.DriveTrain.RIGHT_FORWARD, Constants.DriveTrain.RIGHT_REVERSE);
Constants.DriveTrain.inverted = false;
}
/*
* Method is a required method that the PID Subsystem uses to return the
* calculated PID value to the driver
- *
+ *
* @param Gives the user the output from the PID algorithm that is calculated
* internally
- *
+ *
* Body: Uses the output, does some filtering and drives the robot
*/
@Override
/*
* Checks the drive mode
- *
+ *
* @return the current state of the robot in each state Average distance from
* both sides of tank drive for Encoder Mode Angle from the gyro in GYRO_MODE
*/
/*
* constrains the distance to within -100 and 100 since we aren't going to
* drive more than 100 inches
- *
+ *
* Configure Encoder PID
- *
+ *
* Sets the setpoint to the PID subsystem
*/
public void driveDistance(double dist, double maxTimeOut) {
/*
* Turning method that should be used repeatedly in a command
- *
+ *
* First constrains the angle to within -360 and 360 since that is as much as
* we need to turn
- *
+ *
* Configures Gyro PID and sets the setpoint as an angle
*/
public void turnAngle(double angle) {
rearRight.set(right);
}
- /*
- * @return a value that is the current setpoint for the piston kReverse or
- * kForward
- */
- public Value getLeftGearPistonValue() {
- return leftGearPiston.get();
- }
-
- /*
- * @return a value that is the current setpoint for the piston kReverse or
- * kForward
+ /**
+ * @return a value that is the current setpoint for the piston (kReverse or
+ * kForward)
*/
- public Value getRightGearPistonValue() {
- return rightGearPiston.get();
+ public Value getGearPistonValue() {
+ return leftGearPiston.get(); // Pistons should always be in the same state
}
- /*
+ /**
* Changes the ball shift gear assembly to high
*/
public void setHighGear() {
changeGear(Constants.DriveTrain.HIGH_GEAR);
}
- /*
+ /**
* Changes the ball shift gear assembly to low
*/
public void setLowGear() {
changeGear(Constants.DriveTrain.LOW_GEAR);
}
- /*
- * changes the gear to a DoubleSolenoid.Value
+ /**
+ * Changes the gear to a DoubleSolenoid.Value
*/
public void changeGear(DoubleSolenoid.Value gear) {
leftGearPiston.set(gear);
rightGearPiston.set(gear);
}
+ /**
+ * Switches drivetrain gears from high to low or low to high
+ */
+ public void switchGear() {
+ Value currentValue = getGearPistonValue();
+ Value setValue = (currentValue == Constants.DriveTrain.HIGH_GEAR) ? Constants.DriveTrain.LOW_GEAR
+ : Constants.DriveTrain.HIGH_GEAR;
+ changeGear(setValue);
+ }
+
+ /**
+ * Toggle whether the motor outputs are flipped, effectively switching which
+ * side of the robot is the front.
+ */
+ public void toggleFlipped() {
+ outputFlipped = !outputFlipped;
+ }
+
+ public boolean isFlipped() {
+ return outputFlipped;
+ }
+
}
projects / 3501 / stronghold-2016 / commitdiff