public class Constants {
public static class OI {
public final static int XBOX_CONTROLLER_PORT = 0;
- // public final static int RIGHT_STICK_PORT = 1;
public static final int GAME_PAD_PORT = 2;
// Xbox Controller Ports
- // public final static int TOGGLE_GEAR_PORT = 5;
public final static int SHIFT_LOW_PORT = 9;
public final static int SHIFT_HIGH_PORT = 10;
public final static int RUN_INTAKE_PORT = 6;
public static final int BRAKE_CANTALONS_PORT = 1;
public static final int COAST_CANTALONS_PORT = 3;
public static final int CLIMB_PORT = 4;
+ public static final int STOP_CLIMB_PORT = 2;
// Game Pad Ports
- public final static int TOGGLE_FLYWHEEL_PORT = 1;
- public static final int REVERSE_FLYWHEEL_PORT = 3;
+ public final static int RUN_FLYWHEEL_PORT = 1;
+ public static final int STOP_FLYWHEEL_PORT = 3;
+ public static final int REVERSE_FLYWHEEL_PORT = 4;
public static final int INCREASE_SHOOTER_SPEED_PORT = 8;
public static final int DECREASE_SHOOTER_SPEED_PORT = 7;
- public static final int RESET_SHOOTER_SPEED_PORT = 5;
- public static final int TOGGLE_GEAR_MANIPULATOR_PORT = 2;
+
+ public static final int RESET_SHOOTER_SPEED_PORT = 2;
+ public static final int SHIFT_GEAR_MANIPULATOR_HIGH_PORT = 6;
+ public static final int SHIFT_GEAR_MANIPULATOR_LOW_PORT = 5;
+
+ // 5 is piston out (gear manipulator)
+ // 6 is piston in (gear manipulator)
}
import org.usfirst.frc.team3501.robot.commands.climber.BrakeCANTalons;
import org.usfirst.frc.team3501.robot.commands.climber.CoastCANTalons;
-import org.usfirst.frc.team3501.robot.commands.climber.ToggleWinch;
+import org.usfirst.frc.team3501.robot.commands.climber.RunWinch;
+import org.usfirst.frc.team3501.robot.commands.climber.StopWinch;
import org.usfirst.frc.team3501.robot.commands.driving.ShiftDriveHighGear;
import org.usfirst.frc.team3501.robot.commands.driving.ShiftDriveLowGear;
-import org.usfirst.frc.team3501.robot.commands.driving.ToggleGearManipulatorPiston;
+import org.usfirst.frc.team3501.robot.commands.driving.ShiftGearManipulatorPistonHigh;
+import org.usfirst.frc.team3501.robot.commands.driving.ShiftGearManipulatorPistonLow;
import org.usfirst.frc.team3501.robot.commands.intake.ReverseIntakeContinuous;
import org.usfirst.frc.team3501.robot.commands.intake.RunIntakeContinuous;
import org.usfirst.frc.team3501.robot.commands.shooter.DecreaseShootingSpeed;
import org.usfirst.frc.team3501.robot.commands.shooter.ReverseIndexWheelContinuous;
import org.usfirst.frc.team3501.robot.commands.shooter.RunFlyWheelContinuous;
import org.usfirst.frc.team3501.robot.commands.shooter.RunIndexWheelContinuous;
+import org.usfirst.frc.team3501.robot.commands.shooter.StopFlyWheel;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.buttons.Button;
public static Button runIndexWheel;
public static Button reverseIndexWheel;
- public static Button toggleFlyWheel;
+ public static Button runFlyWheel;
+ public static Button stopFlyWheel;
public static Button reverseFlyWheel;
// public static Button toggleGear;
public static Button shiftHigh;
public static Button shiftLow;
- public static Button toggleGearManipulatorPiston;
+ // public static Button toggleGearManipulatorPiston;
public static Button runIntake;
public static Button reverseIntake;
public static Button brakeCANTalons;
public static Button coastCANTalons;
public static Button climb;
+ public static Button stopclimb;
+
+ public static Button shiftGearManipulatorPistonHigh;
+ public static Button shiftGearManipulatorPistonLow;
public OI() {
xboxController = new Joystick(Constants.OI.XBOX_CONTROLLER_PORT);
Constants.OI.REVERSE_INDEXWHEEL_PORT);
reverseIndexWheel.whileHeld(new ReverseIndexWheelContinuous());
- toggleFlyWheel = new JoystickButton(gamePad,
- Constants.OI.TOGGLE_FLYWHEEL_PORT);
- toggleFlyWheel.toggleWhenPressed(new RunFlyWheelContinuous());
+ runFlyWheel = new JoystickButton(gamePad, Constants.OI.RUN_FLYWHEEL_PORT);
+ runFlyWheel.whenPressed(new RunFlyWheelContinuous());
+
+ stopFlyWheel = new JoystickButton(gamePad, Constants.OI.STOP_FLYWHEEL_PORT);
+ stopFlyWheel.whenPressed(new StopFlyWheel());
reverseFlyWheel = new JoystickButton(gamePad,
Constants.OI.REVERSE_FLYWHEEL_PORT);
reverseFlyWheel.whileHeld(new ReverseFlyWheelContinuous());
- // toggleGear = new JoystickButton(xboxController,
- // Constants.OI.TOGGLE_GEAR_PORT);
- // toggleGear.whenPressed(new ToggleDriveGear());
-
shiftHigh = new JoystickButton(xboxController,
Constants.OI.SHIFT_HIGH_PORT);
shiftHigh.whenPressed(new ShiftDriveHighGear());
shiftLow = new JoystickButton(xboxController, Constants.OI.SHIFT_LOW_PORT);
shiftLow.whenPressed(new ShiftDriveLowGear());
- toggleGearManipulatorPiston = new JoystickButton(gamePad,
- Constants.OI.TOGGLE_GEAR_MANIPULATOR_PORT);
- toggleGearManipulatorPiston.whenPressed(new ToggleGearManipulatorPiston());
+ /*
+ * toggleGearManipulatorPiston = new JoystickButton(gamePad,
+ * Constants.OI.TOGGLE_GEAR_MANIPULATOR_PORT);
+ * toggleGearManipulatorPiston.whenPressed(new
+ * ToggleGearManipulatorPiston());
+ */
+
+ shiftGearManipulatorPistonHigh = new JoystickButton(gamePad,
+ Constants.OI.SHIFT_GEAR_MANIPULATOR_HIGH_PORT);
+ shiftGearManipulatorPistonHigh
+ .whenPressed(new ShiftGearManipulatorPistonHigh());
+
+ shiftGearManipulatorPistonLow = new JoystickButton(gamePad,
+ Constants.OI.SHIFT_GEAR_MANIPULATOR_LOW_PORT);
+ shiftGearManipulatorPistonLow
+ .whenPressed(new ShiftGearManipulatorPistonLow());
runIntake = new JoystickButton(xboxController,
Constants.OI.RUN_INTAKE_PORT);
coastCANTalons.whenPressed(new CoastCANTalons());
climb = new JoystickButton(xboxController, Constants.OI.CLIMB_PORT);
- climb.whenPressed(new ToggleWinch());
+ climb.whenPressed(new RunWinch());
+
+ stopclimb = new JoystickButton(xboxController,
+ Constants.OI.STOP_CLIMB_PORT);
+ stopclimb.whenPressed(new StopWinch());
}
public static OI getOI() {
import org.usfirst.frc.team3501.robot.commandgroups.AutonMiddleGear;
import org.usfirst.frc.team3501.robot.commandgroups.AutonSideGear;
-import org.usfirst.frc.team3501.robot.commands.driving.TimeDrive;
import org.usfirst.frc.team3501.robot.subsystems.Climber;
import org.usfirst.frc.team3501.robot.subsystems.DriveTrain;
import org.usfirst.frc.team3501.robot.subsystems.Intake;
driveTrain.setLowGear();
// autonCommand = (Command) autonChooser.getSelected();
- autonCommand = new TimeDrive(2, 0.6);
+ autonCommand = new AutonMiddleGear();
Scheduler.getInstance().add(autonCommand);
}
package org.usfirst.frc.team3501.robot.commandgroups;
-import org.usfirst.frc.team3501.robot.commands.driving.DriveDistance;
+import org.usfirst.frc.team3501.robot.commands.driving.ShiftGearManipulatorPistonLow;
+import org.usfirst.frc.team3501.robot.commands.driving.TimeDrive;
import edu.wpi.first.wpilibj.command.CommandGroup;
*/
public class AutonMiddleGear extends CommandGroup {
private static final double DISTANCE_TO_PEG = 91.3 - 32;
- private static final double DISTANCE_TO_BACK_OUT = -29.75;
- private static final double THIRD_DISTANCE_TO_TRAVEL = 70;
- private static final double DISTANCE_TO_BASELINE = 50.5;
-
- private static final double ANGLE_TO_TURN = 90;
-
private static final double maxTimeOut = 7;
/***
* baseline. Only Direction.LEFT and Direction.RIGHT will be accepted
*/
public AutonMiddleGear() {
- addSequential(new DriveDistance(DISTANCE_TO_PEG, maxTimeOut));
- // addSequential(new WaitCommand(3));
- // addSequential(new DriveDistance(DISTANCE_TO_BACK_OUT, maxTimeOut));
- // addSequential(new TurnForAngle(ANGLE_TO_TURN, direction, maxTimeOut));
- // addSequential(new DriveDistance(THIRD_DISTANCE_TO_TRAVEL, maxTimeOut));
- // addSequential(
- // new TurnForAngle(ANGLE_TO_TURN, oppositeOf(direction), maxTimeOut));
- // addSequential(new DriveDistance(DISTANCE_TO_BASELINE, maxTimeOut));
+ // addSequential(new DriveDistance(DISTANCE_TO_PEG, maxTimeOut));
+
+ addSequential(new ShiftGearManipulatorPistonLow());
+ addSequential(new TimeDrive(2, 0.6));
}
}
* @param motorVal
* value range is from -1 to 1
*/
- public RunWinch(double time, double motorVal) {
+ public RunWinch() {
requires(climber);
this.time = time;
this.motorVal = motorVal;
@Override
protected void initialize() {
+ climber.setCANTalonsBrakeMode(climber.COAST_MODE);
}
@Override
protected void execute() {
- climber.setMotorValues(motorVal);
+ climber.setMotorValues(climber.CLIMBER_SPEED);
}
@Override
protected boolean isFinished() {
- return timeSinceInitialized() >= time;
+ // return timeSinceInitialized() >= time;
+ return false;
}
@Override
@Override
protected void initialize() {
+ climber.setCANTalonsBrakeMode(climber.COAST_MODE);
}
@Override
@Override
protected void end() {
climber.stop();
+ climber.setCANTalonsBrakeMode(climber.BRAKE_MODE);
}
@Override
@Override
protected void initialize() {
+ System.out.println("toggled");
}
@Override
if (climber.shouldBeClimbing) {
climber.setCANTalonsBrakeMode(climber.COAST_MODE);
climber.setMotorValues(climbingSpeed);
+ System.out.println("climbing");
} else {
climber.setCANTalonsBrakeMode(climber.BRAKE_MODE);
+ System.out.println("braked");
/* Not sure if should have */
climber.stop();
end();
@Override
protected void end() {
climber.shouldBeClimbing = !climber.shouldBeClimbing;
+ System.out.println("ended");
}
@Override
*/
public class JoystickDrive extends Command {
+ double previousThrust = 0;
+ double previousTwist = 0;
+
public JoystickDrive() {
requires(Robot.getDriveTrain());
}
@Override
protected void execute() {
- final double thrust = OI.xboxController.getY();
- final double twist = OI.xboxController.getAxis(AxisType.kZ);
+ double thrust = OI.xboxController.getY();
+ double twist = OI.xboxController.getAxis(AxisType.kZ);
- Robot.getDriveTrain().joystickDrive(-thrust, -twist);
+ thrust = (6 * previousThrust + thrust) / 7;
+ twist = (6 * previousTwist + twist) / 7;
- /*
- * double left = OI.leftJoystick.getY(); double right =
- * OI.rightJoystick.getY(); Robot.getDriveTrain().tankDrive(-left, -right);
- */
+ previousThrust = thrust;
+ previousTwist = twist;
+
+ Robot.getDriveTrain().joystickDrive(-thrust, -twist);
}
@Override
--- /dev/null
+package org.usfirst.frc.team3501.robot.commands.driving;
+
+import org.usfirst.frc.team3501.robot.Robot;
+import org.usfirst.frc.team3501.robot.subsystems.DriveTrain;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+public class ShiftGearManipulatorPistonHigh extends Command {
+ private DriveTrain driveTrain = Robot.getDriveTrain();
+
+ public ShiftGearManipulatorPistonHigh() {
+ }
+
+ @Override
+ protected void initialize() {
+
+ }
+
+ @Override
+ protected void execute() {
+ driveTrain.extendGearManipulatorPiston();
+ }
+
+ // 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 org.usfirst.frc.team3501.robot.subsystems.DriveTrain;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+public class ShiftGearManipulatorPistonLow extends Command {
+ private DriveTrain driveTrain = Robot.getDriveTrain();
+
+ public ShiftGearManipulatorPistonLow() {
+ }
+
+ @Override
+ protected void initialize() {
+
+ }
+
+ @Override
+ protected void execute() {
+ driveTrain.retractGearManipulatorPiston();
+ }
+
+ // 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() {
+ }
+}
package org.usfirst.frc.team3501.robot.commands.intake;
import org.usfirst.frc.team3501.robot.Robot;
+import org.usfirst.frc.team3501.robot.subsystems.Intake;
import edu.wpi.first.wpilibj.command.Command;
* button.whileHeld(...).
*/
public class ReverseIntakeContinuous extends Command {
+ private Intake intake = Robot.getIntake();
+
+ private double previousMotorValue = 0;
+ private double targetMotorValue = intake.REVERSE_SPEED;
public ReverseIntakeContinuous() {
- requires(Robot.getIntake());
+ requires(intake);
}
// Called just before this Command runs the first time
// Called repeatedly when this Command is scheduled to run
@Override
protected void execute() {
- Robot.getIntake().runReverseIntake();
+ double motorValue = (6 * previousMotorValue + targetMotorValue) / 7;
+ previousMotorValue = motorValue;
+ intake.setSpeed(motorValue);
}
// Make this return true when this Command no longer needs to run execute()
// Called once after isFinished returns true
@Override
protected void end() {
- Robot.getIntake().stopIntake();
+ intake.stopIntake();
}
// Called when another command which requires one or more of the same
package org.usfirst.frc.team3501.robot.commands.intake;
import org.usfirst.frc.team3501.robot.Robot;
+import org.usfirst.frc.team3501.robot.subsystems.Intake;
import edu.wpi.first.wpilibj.command.Command;
*
*/
public class RunIntakeContinuous extends Command {
- // create setter method for speed, use setSpeed method to do end() by setting
- // speed to 0
+ private Intake intake = Robot.getIntake();
+
+ private double previousMotorValue = 0;
+ private double targetMotorValue = intake.INTAKE_SPEED;
public RunIntakeContinuous() {
- requires(Robot.getIntake());
+ requires(intake);
}
@Override
// Called just before this Command runs the first time
@Override
protected void initialize() {
-
}
// Called repeatedly when this Command is scheduled to run
@Override
protected void execute() {
- Robot.getIntake().runIntake();
+ double motorValue = (6 * previousMotorValue + targetMotorValue) / 7;
+ previousMotorValue = motorValue;
+ intake.setSpeed(motorValue);
}
// Called once after isFinished returns true
public class ReverseIndexWheelContinuous extends Command {
private Shooter shooter = Robot.getShooter();
+ private double previousMotorValue = 0;
+ private double targetMotorValue = -shooter.DEFAULT_INDEXING_MOTOR_VALUE;
+
/**
* See JavaDoc comment in class for details
*
* value range from -1 to 1
*/
public ReverseIndexWheelContinuous() {
- requires(shooter);
}
// Called just before this Command runs the first time
// Called repeatedly when this Command is scheduled to run
@Override
protected void execute() {
- shooter.reverseIndexWheel();
+ double motorValue = (6 * previousMotorValue + targetMotorValue) / 7;
+ previousMotorValue = motorValue;
+ shooter.setIndexWheelMotorVal(motorValue);
}
// Called once after isFinished returns true
private PIDController wheelController;
public RunFlyWheelContinuous() {
+ requires(shooter);
}
@Override
public class RunIndexWheelContinuous extends Command {
private Shooter shooter = Robot.getShooter();
+ private double previousMotorValue = 0;
+ private double targetMotorValue = shooter.DEFAULT_INDEXING_MOTOR_VALUE;
+
/**
* See JavaDoc comment in class for details
*/
public RunIndexWheelContinuous() {
- requires(shooter);
}
@Override
double shooterSpeed = shooter.getShooterRPM();
double targetShooterSpeed = shooter.getTargetShootingSpeed();
double threshold = shooter.getRPMThreshold();
- if (Math.abs(shooterSpeed - targetShooterSpeed) <= threshold)
- shooter.runIndexWheel();
+ if (Math.abs(shooterSpeed - targetShooterSpeed) <= threshold) {
+ double motorValue = (6 * previousMotorValue + targetMotorValue) / 7;
+ previousMotorValue = motorValue;
+ shooter.setIndexWheelMotorVal(motorValue);
+ }
}
@Override
package org.usfirst.frc.team3501.robot.commands.shooter;
import org.usfirst.frc.team3501.robot.Robot;
+import org.usfirst.frc.team3501.robot.subsystems.Shooter;
import edu.wpi.first.wpilibj.command.Command;
* @author Shaina
*/
public class StopFlyWheel extends Command {
+ private Shooter shooter = Robot.getShooter();
+
/**
* This command stops the fly wheel.
*/
public StopFlyWheel() {
-
+ requires(shooter);
}
// Called just before this Command runs the first time
// subsystems is scheduled to run
@Override
protected void interrupted() {
+ end();
}
@Override
public static final double INCHES_PER_PULSE = WHEEL_DIAMETER * Math.PI
/ ENCODER_PULSES_PER_REVOLUTION;
+ public static final boolean BRAKE_MODE = true;
+ public static final boolean COAST_MODE = false;
+
private static DriveTrain driveTrain;
private final CANTalon frontLeft, frontRight, rearLeft, rearRight;
rearRight.set(-right);
}
+ public void setCANTalonsBrakeMode(boolean mode) {
+ frontLeft.enableBrakeMode(mode);
+ frontRight.enableBrakeMode(mode);
+ rearLeft.enableBrakeMode(mode);
+ rearRight.enableBrakeMode(mode);
+ }
+
public void joystickDrive(final double thrust, final double twist) {
if ((thrust < 0.1 && thrust > -0.1) && (twist < 0.1 && twist > -0.1))
robotDrive.arcadeDrive(0, 0, true);
else
robotDrive.arcadeDrive(thrust, twist, true);
+ System.out.println(thrust + " " + twist);
}
public void tankDrive(double left, double right) {
* @param speed
* from -1 to 1
*/
- private void setSpeed(double speed) {
+ public void setSpeed(double speed) {
speed = MathLib.restrictToRange(speed, -1.0, 1.0);
intakeWheel.set(speed);
}
private PIDController wheelController;
private static final double RPM_THRESHOLD = 10;
- private static final double DEFAULT_INDEXING_MOTOR_VALUE = 0.75;
- private static final double REVERSE_FLYWHEEL_MOTOR_VALUE = -0.75;
+ public static final double DEFAULT_INDEXING_MOTOR_VALUE = 0.75;
+ public static final double REVERSE_FLYWHEEL_MOTOR_VALUE = -0.75;
private static final double DEFAULT_SHOOTING_SPEED = 2875; // rpm
private static final double SHOOTING_SPEED_INCREMENT = 25;
projects / 3501 / 2017steamworks / commitdiff