public final static int TOGGLE_GEAR_PORT = 5;
public final static int RUN_INTAKE_PORT = 1;
- public final static int REVERSE_INTAKE_PORT = 4;
+ public final static int REVERSE_INTAKE_PORT = 2;
public final static int RUN_INDEXWHEEL_PORT = 1;
public final static int REVERSE_INDEXWHEEL_PORT = 2;
public static final int BRAKE_CANTALONS_PORT = 5;
- public static final int COAST_CANTALONS_PORT = 3;
-
- public final static int TOGGLE_FLYWHEEL_PORT = 4;
- public static final int REVERSE_FLYWHEEL_PORT = 1;
- public static final int INCREASE_SHOOTER_SPEED_PORT = 6;
- public static final int DECREASE_SHOOTER_SPEED_PORT = 2;
- public static final int TOGGLE_GEAR_MANIPULATOR_PORT = 3;
+ public static final int COAST_CANTALONS_PORT = 6;
+
+ public final static int TOGGLE_FLYWHEEL_PORT = 1;
+ public static final int REVERSE_FLYWHEEL_PORT = 3;
+ 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 class Shooter {
public static final int INDEX_WHEEL = 7;
public final static int HALL_EFFECT_PORT = 9;
-
- public static final int MODULE_NUMBER = 10, PISTON_FORWARD = 4,
- PISTON_REVERSE = 5;
- public static final Value HIGH_GEAR = DoubleSolenoid.Value.kForward;
- public static final Value LOW_GEAR = DoubleSolenoid.Value.kReverse;
}
public static class DriveTrain {
public static final int PISTON_MODULE = 10;
public static final int GEAR_MANIPULATOR_PISTON_FORWARD = 4,
GEAR_MANIPULATOR_PISTON_REVERSE = 5;
- public static final int LEFT_GEAR_PISTON_FORWARD = 0,
- LEFT_GEAR_PISTON_REVERSE = 1, RIGHT_GEAR_PISTON_FORWARD = 3,
+ public static final int LEFT_GEAR_PISTON_FORWARD = 1,
+ LEFT_GEAR_PISTON_REVERSE = 0, RIGHT_GEAR_PISTON_FORWARD = 3,
RIGHT_GEAR_PISTON_REVERSE = 2;
public static final Value FORWARD_PISTON_VALUE = DoubleSolenoid.Value.kForward;
public static final Value REVERSE_PISTON_VALUE = DoubleSolenoid.Value.kReverse;
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.IncreaseShootingSpeed;
+import org.usfirst.frc.team3501.robot.commands.shooter.ResetShootingSpeed;
import org.usfirst.frc.team3501.robot.commands.shooter.ReverseFlyWheelContinuous;
import org.usfirst.frc.team3501.robot.commands.shooter.ReverseIndexWheelContinuous;
import org.usfirst.frc.team3501.robot.commands.shooter.RunFlyWheelContinuous;
public static Button increaseShooterSpeed;
public static Button decreaseShooterSpeed;
+ public static Button resetShooterSpeed;
public static Button brakeCANTalons;
public static Button coastCANTalons;
Constants.OI.DECREASE_SHOOTER_SPEED_PORT);
decreaseShooterSpeed.whenPressed(new DecreaseShootingSpeed());
+ resetShooterSpeed = new JoystickButton(gamePad,
+ Constants.OI.RESET_SHOOTER_SPEED_PORT);
+ resetShooterSpeed.whenPressed(new ResetShootingSpeed());
+
brakeCANTalons = new JoystickButton(rightJoystick,
Constants.OI.BRAKE_CANTALONS_PORT);
brakeCANTalons.whenPressed(new BrakeCANTalons());
package org.usfirst.frc.team3501.robot;
+import org.usfirst.frc.team3501.robot.commandgroups.AutonMiddleGear;
+import org.usfirst.frc.team3501.robot.commandgroups.AutonSideGear;
import org.usfirst.frc.team3501.robot.subsystems.DriveTrain;
import org.usfirst.frc.team3501.robot.subsystems.Intake;
import org.usfirst.frc.team3501.robot.subsystems.Shooter;
import edu.wpi.first.wpilibj.CameraServer;
import edu.wpi.first.wpilibj.DriverStation;
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.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
public class Robot extends IterativeRobot {
private static OI oi;
private static Intake intake;
+ Command autonCommand;
+ SendableChooser autonChooser;
+
@Override
public void robotInit() {
driveTrain = DriveTrain.getDriveTrain();
shooter = Shooter.getShooter();
intake = Intake.getIntake();
+ autonChooser = new SendableChooser();
+ autonChooser.addDefault("Middle Gear", new AutonMiddleGear());
+ autonChooser.addObject("Red Boiler Gear",
+ new AutonSideGear("RED", "BOILER"));
+ autonChooser.addObject("Red Retrieval Gear",
+ new AutonSideGear("RED", "RETRIEVAL"));
+ autonChooser.addObject("Blue Boiler Gear",
+ new AutonSideGear("BLUE", "BOILER"));
+ autonChooser.addObject("Blue Retrieval Gear",
+ new AutonSideGear("BLUE", "RETRIEVAL"));
+ SmartDashboard.putData("Autonomous Chooser", autonChooser);
+
CameraServer server = CameraServer.getInstance();
UsbCamera climberCam = server.startAutomaticCapture("climbercam", 0);
UsbCamera intakeCam = server.startAutomaticCapture("intakecam", 1);
return Intake.getIntake();
}
- // If the gear values do not match in the left and right piston, then they are
- // both set to high gear
@Override
public void autonomousInit() {
- driveTrain.setHighGear();
+ // driveTrain.setLowGear();
driveTrain.setCANTalonsBrakeMode(driveTrain.DRIVE_COAST_MODE);
+
+ // autonCommand = (Command) autonChooser.getSelected();
+ // autonCommand = new TimeDrive(1.5, 0.6);
+ // Scheduler.getInstance().add(autonCommand);
}
@Override
@Override
public void teleopInit() {
+ // driveTrain.setHighGear();
driveTrain.setCANTalonsBrakeMode(driveTrain.DRIVE_COAST_MODE);
}
@Override
public void teleopPeriodic() {
- driveTrain.printEncoderOutput();
Scheduler.getInstance().run();
updateSmartDashboard();
}
- @Override
- public void disabledInit() {
- driveTrain.setCANTalonsBrakeMode(driveTrain.DRIVE_BRAKE_MODE);
- }
- //
- // @Override
- // public void disabledPeriodic() {
- // Scheduler.getInstance().add(new RunFlyWheel(2));
- // }
-
public void updateSmartDashboard() {
SmartDashboard.putNumber("left encode ",
driveTrain.getLeftEncoderDistance());
package org.usfirst.frc.team3501.robot.commandgroups;
-import org.usfirst.frc.team3501.robot.Constants.Direction;
import org.usfirst.frc.team3501.robot.commands.driving.DriveDistance;
-import org.usfirst.frc.team3501.robot.commands.driving.TurnForAngle;
import edu.wpi.first.wpilibj.command.CommandGroup;
-import edu.wpi.first.wpilibj.command.WaitCommand;
/**
*
* direction to turn after placing gear on peg in order to cross the
* baseline. Only Direction.LEFT and Direction.RIGHT will be accepted
*/
- public AutonMiddleGear(Direction direction) {
+ 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));
- }
-
- private Direction oppositeOf(Direction direction) {
- if (direction == Direction.LEFT)
- return Direction.RIGHT;
- return Direction.LEFT;
+ // 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));
}
}
* inches and 205.7286 inches from the right side of the arena this program
* chooses which peg to go for based on the starting point
*/
- public AutonSideGear(String side) {
+ public AutonSideGear(String team, String side) {
requires(Robot.getDriveTrain());
if (side.equals("BOILER")) {
131.6 - (94.88 - ROBOT_WIDTH / 2 - distanceFromCorner) / Math.sqrt(3)
- ROBOT_LENGTH / 2,
5));
- addSequential(new TurnForAngle(60, Direction.RIGHT, 5));
+
+ if (team.equals("RED"))
+ addSequential(new TurnForAngle(60, Direction.LEFT, 5));
+ else if (team.equals("BLUE"))
+ addSequential(new TurnForAngle(60, Direction.RIGHT, 5));
+
addSequential(new DriveDistance(
2 * (94.88 - ROBOT_WIDTH / 2 - distanceFromCorner) / Math.sqrt(3)
- ROBOT_LENGTH / 2 + 7,
131.6 - (93.13 - ROBOT_WIDTH / 2 - distanceFromCorner) / Math.sqrt(3)
- ROBOT_LENGTH / 2,
5));
- addSequential(new TurnForAngle(60, Direction.LEFT, 5));
+
+ if (team.equals("RED"))
+ addSequential(new TurnForAngle(60, Direction.RIGHT, 5));
+ else if (team.equals("BLUE"))
+ addSequential(new TurnForAngle(60, Direction.LEFT, 5));
+
addSequential(new DriveDistance(
2 * (93.13 - ROBOT_WIDTH / 2 - distanceFromCorner) / Math.sqrt(3)
- ROBOT_LENGTH / 2,
@Override
protected void execute() {
double xVal = gyroP * (driveTrain.getAngle() - zeroAngle);
- double yVal = driveController.calcPID(driveTrain.getAvgEncoderDistance());
+ double yVal = driveController.calcPID(driveTrain.getRightEncoderDistance());
double leftDrive = yVal - xVal;
double rightDrive = yVal + xVal;
@Override
protected void execute() {
- Value leftPistonValue = driveTrain.getLeftDriveTrainPiston();
Value rightPistonValue = driveTrain.getRightDriveTrainPiston();
- if (leftPistonValue == Constants.DriveTrain.REVERSE_PISTON_VALUE) {
+ if (rightPistonValue == Constants.DriveTrain.REVERSE_PISTON_VALUE) {
driveTrain.setHighGear();
} else {
driveTrain.setLowGear();
import edu.wpi.first.wpilibj.command.Subsystem;
public class DriveTrain extends Subsystem {
- public static double driveP = 0.012, driveI = 0.0011, driveD = -0.002;
+ public static double driveP = 0.01, driveI = 0.00115, driveD = -0.002;
public static double smallTurnP = 0.004, smallTurnI = 0.0013,
smallTurnD = 0.005;
public static double largeTurnP = .003, largeTurnI = .0012, largeTurnD = .006;
}
public void joystickDrive(final double thrust, final double twist) {
- robotDrive.arcadeDrive(thrust, twist, true);
+ if ((thrust < 0.1 && thrust > -0.1) && (twist < 0.1 && twist > -0.1))
+ robotDrive.arcadeDrive(0, 0, true);
+ else
+ robotDrive.arcadeDrive(thrust, twist, true);
}
public void stop() {
import com.ctre.CANTalon;
-import edu.wpi.first.wpilibj.DoubleSolenoid;
-import edu.wpi.first.wpilibj.DoubleSolenoid.Value;
import edu.wpi.first.wpilibj.command.Subsystem;
public class Shooter extends Subsystem {
private double targetShootingSpeed = DEFAULT_SHOOTING_SPEED;
private double currentShooterMotorValue = 0;
- private final DoubleSolenoid piston;
-
private Shooter() {
flyWheel1 = new CANTalon(Constants.Shooter.FLY_WHEEL1);
flyWheel2 = new CANTalon(Constants.Shooter.FLY_WHEEL2);
indexWheel = new CANTalon(Constants.Shooter.INDEX_WHEEL);
hallEffect = new HallEffectSensor(Constants.Shooter.HALL_EFFECT_PORT, 1);
-
- piston = new DoubleSolenoid(Constants.Shooter.MODULE_NUMBER,
- Constants.Shooter.PISTON_FORWARD, Constants.Shooter.PISTON_REVERSE);
}
/**
* motor value from -1 to 1(fastest forward)
*/
public void setFlyWheelMotorVal(double val) {
- val = MathLib.restrictToRange(val, 0.0, 1.0);
+ val = MathLib.restrictToRange(val, -1.0, 1.0);
flyWheel1.set(val);
flyWheel2.set(val);
}
this.currentShooterMotorValue = 0;
}
- public Value getPistonValue() {
- return piston.get();
- }
-
- public void setHighGear() {
- changeGear(Constants.Shooter.HIGH_GEAR);
- }
-
- public void setLowGear() {
- changeGear(Constants.Shooter.LOW_GEAR);
- }
-
- private void changeGear(DoubleSolenoid.Value gear) {
- piston.set(gear);
- }
-
public void reverseFlyWheel() {
this.setFlyWheelMotorVal(shooter.REVERSE_FLYWHEEL_MOTOR_VALUE);
}