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 = 1,
- LEFT_GEAR_PISTON_REVERSE = 0, RIGHT_GEAR_PISTON_FORWARD = 3,
- RIGHT_GEAR_PISTON_REVERSE = 2;
+
+ public static final int DRIVETRAIN_GEAR_FORWARD = 0,
+ DRIVETRAIN_GEAR_REVERSE = 1;
+
public static final Value FORWARD_PISTON_VALUE = DoubleSolenoid.Value.kForward;
public static final Value REVERSE_PISTON_VALUE = DoubleSolenoid.Value.kReverse;
public static class Intake {
public static final int INTAKE_ROLLER_PORT = 8;
+ }
+ public static class Climber {
+ public static final int WINCH_PORT = 0;
}
public static enum Direction {
}
}
- public static double limitValue(double val, double max, double min) {
+ public static double limitValue(double val, double min, double max) {
if (val > max) {
return max;
} else if (val < min) {
package org.usfirst.frc.team3501.robot;
-import org.usfirst.frc.team3501.robot.commands.driving.BrakeCANTalons;
-import org.usfirst.frc.team3501.robot.commands.driving.CoastCANTalons;
+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.driving.ToggleDriveGear;
import org.usfirst.frc.team3501.robot.commands.driving.ToggleGearManipulatorPiston;
import org.usfirst.frc.team3501.robot.commands.intake.ReverseIntakeContinuous;
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;
import org.usfirst.frc.team3501.robot.subsystems.Shooter;
private static Shooter shooter;
private static OI oi;
private static Intake intake;
+ private static Climber climber;
Command autonCommand;
SendableChooser autonChooser;
oi = OI.getOI();
shooter = Shooter.getShooter();
intake = Intake.getIntake();
+ climber = Climber.getClimber();
autonChooser = new SendableChooser();
autonChooser.addDefault("Middle Gear", new AutonMiddleGear());
CameraServer server = CameraServer.getInstance();
UsbCamera climberCam = server.startAutomaticCapture("climbercam", 0);
UsbCamera intakeCam = server.startAutomaticCapture("intakecam", 1);
-
- driveTrain.setCANTalonsBrakeMode(driveTrain.DRIVE_COAST_MODE);
}
public static DriveTrain getDriveTrain() {
return Intake.getIntake();
}
+ public static Climber getClimber() {
+ return Climber.getClimber();
+ }
+
@Override
public void autonomousInit() {
- // driveTrain.setLowGear();
- driveTrain.setCANTalonsBrakeMode(driveTrain.DRIVE_COAST_MODE);
+ driveTrain.setLowGear();
// autonCommand = (Command) autonChooser.getSelected();
- // autonCommand = new TimeDrive(1.5, 0.6);
- // Scheduler.getInstance().add(autonCommand);
+ autonCommand = new TimeDrive(2, 0.6);
+ Scheduler.getInstance().add(autonCommand);
}
@Override
@Override
public void teleopInit() {
- // driveTrain.setHighGear();
- driveTrain.setCANTalonsBrakeMode(driveTrain.DRIVE_COAST_MODE);
+ driveTrain.setHighGear();
}
@Override
--- /dev/null
+package org.usfirst.frc.team3501.robot.commands.climber;
+
+import org.usfirst.frc.team3501.robot.Robot;
+import org.usfirst.frc.team3501.robot.subsystems.Climber;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ *
+ */
+public class BrakeCANTalons extends Command {
+ Climber climber = Robot.getClimber();
+
+ public BrakeCANTalons() {
+ requires(climber);
+ }
+
+ @Override
+ protected void initialize() {
+ climber.setCANTalonsBrakeMode(climber.BRAKE_MODE);
+ }
+
+ @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.climber;
+
+import org.usfirst.frc.team3501.robot.Robot;
+import org.usfirst.frc.team3501.robot.subsystems.Climber;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ *
+ */
+public class CoastCANTalons extends Command {
+ Climber climber = Robot.getClimber();
+
+ public CoastCANTalons() {
+ requires(climber);
+ }
+
+ @Override
+ protected void initialize() {
+ climber.setCANTalonsBrakeMode(climber.COAST_MODE);
+ }
+
+ @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.climber;
-
-import org.usfirst.frc.team3501.robot.Robot;
-import org.usfirst.frc.team3501.robot.subsystems.DriveTrain;
-
-import edu.wpi.first.wpilibj.command.Command;
-
-/**
- * This command runs the winch at a specified speed when the robot has completed
- * the climb and when the button triggering it is pressed. This command also
- * makes the drive train motors run because the winch is controlled by the drive
- * train.
- *
- * pre-condition: This command is run by a button in OI. The robot must have
- * completed climbing.
- *
- * post-condition: Winch motor set to a specified speed.
- *
- * @param motorVal
- * value range is from -1 to 1
- *
- * @author shivanighanta
- *
- */
-public class MaintainClimbedPosition extends Command {
-
- /**
- * See JavaDoc comment in class for details
- *
- * @param time
- * time in seconds to run the winch
- */
- public MaintainClimbedPosition() {
- requires(Robot.getDriveTrain());
- }
-
- @Override
- protected void initialize() {
- }
-
- @Override
- protected void execute() {
- Robot.getDriveTrain().setMotorValues(DriveTrain.MAINTAIN_CLIMBED_POSITION,
- DriveTrain.MAINTAIN_CLIMBED_POSITION);
-
- }
-
- @Override
- protected boolean isFinished() {
- return false;
- }
-
- @Override
- protected void end() {
- Robot.getDriveTrain().stop();
- }
-
- @Override
- protected void interrupted() {
- end();
- }
-}
package org.usfirst.frc.team3501.robot.commands.climber;
import org.usfirst.frc.team3501.robot.Robot;
+import org.usfirst.frc.team3501.robot.subsystems.Climber;
import edu.wpi.first.wpilibj.command.Command;
*/
public class RunWinch extends Command {
+ Climber climber = Robot.getClimber();
+
private double time;
private double motorVal;
* value range is from -1 to 1
*/
public RunWinch(double time, double motorVal) {
- requires(Robot.getDriveTrain());
+ requires(climber);
this.time = time;
this.motorVal = motorVal;
}
@Override
protected void execute() {
- Robot.getDriveTrain().setMotorValues(motorVal, motorVal);
-
+ climber.setMotorValues(motorVal);
}
@Override
@Override
protected void end() {
- Robot.getDriveTrain().stop();
+ climber.stop();
}
@Override
package org.usfirst.frc.team3501.robot.commands.climber;
+import org.usfirst.frc.team3501.robot.OI;
import org.usfirst.frc.team3501.robot.Robot;
-import org.usfirst.frc.team3501.robot.subsystems.DriveTrain;
+import org.usfirst.frc.team3501.robot.subsystems.Climber;
import edu.wpi.first.wpilibj.command.Command;
*
*/
public class RunWinchContinuous extends Command {
- DriveTrain driveTrain = Robot.getDriveTrain();
+ Climber climber = Robot.getClimber();
private double climbingSpeed;
/**
* value range is from -1 to 1
*/
public RunWinchContinuous() {
- requires(driveTrain);
- climbingSpeed = driveTrain.CLIMBER_SPEED;
+ requires(climber);
+ climbingSpeed = climber.CLIMBER_SPEED;
}
@Override
@Override
protected void execute() {
- Robot.getDriveTrain().setMotorValues(climbingSpeed, climbingSpeed);
+ double thrust = OI.leftJoystick.getY();
+ climber.setMotorValues(-thrust);
}
@Override
@Override
protected void end() {
-
+ climber.stop();
}
@Override
package org.usfirst.frc.team3501.robot.commands.climber;
import org.usfirst.frc.team3501.robot.Robot;
+import org.usfirst.frc.team3501.robot.subsystems.Climber;
import edu.wpi.first.wpilibj.command.Command;
*
*/
public class StopWinch extends Command {
+ Climber climber = Robot.getClimber();
public StopWinch() {
- requires(Robot.getDriveTrain());
+ requires(climber);
}
@Override
@Override
protected void end() {
- Robot.getDriveTrain().stop();
-
+ climber.stop();
}
@Override
+++ /dev/null
-package org.usfirst.frc.team3501.robot.commands.climber;
-
-import org.usfirst.frc.team3501.robot.Robot;
-import org.usfirst.frc.team3501.robot.subsystems.DriveTrain;
-
-import edu.wpi.first.wpilibj.command.Command;
-
-/**
- *
- */
-public class ToggleWinch extends Command {
- DriveTrain driveTrain = Robot.getDriveTrain();
- private double climbingSpeed;
- private double maintainPositionSpeed;
-
- public ToggleWinch() {
- requires(driveTrain);
- climbingSpeed = driveTrain.CLIMBER_SPEED;
- maintainPositionSpeed = driveTrain.MAINTAIN_CLIMBED_POSITION;
- }
-
- @Override
- protected void initialize() {
- }
-
- @Override
- protected void execute() {
- if (driveTrain.shouldBeClimbing) {
- driveTrain.setMotorValues(climbingSpeed, climbingSpeed);
- } else {
- driveTrain.setMotorValues(maintainPositionSpeed, maintainPositionSpeed);
- }
- }
-
- @Override
- protected boolean isFinished() {
- return false;
- }
-
- @Override
- protected void end() {
- driveTrain.shouldBeClimbing = !driveTrain.shouldBeClimbing;
- }
-
- @Override
- protected void interrupted() {
- end();
- }
-}
+++ /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 BrakeCANTalons extends Command {
- private DriveTrain driveTrain = Robot.getDriveTrain();
-
- public BrakeCANTalons() {
- requires(driveTrain);
- }
-
- @Override
- protected void initialize() {
- driveTrain.setCANTalonsBrakeMode(driveTrain.DRIVE_BRAKE_MODE);
- }
-
- @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.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 CoastCANTalons extends Command {
- private DriveTrain driveTrain = Robot.getDriveTrain();
-
- public CoastCANTalons() {
- requires(driveTrain);
- }
-
- @Override
- protected void initialize() {
- driveTrain.setCANTalonsBrakeMode(driveTrain.DRIVE_COAST_MODE);
- }
-
- @Override
- protected void execute() {
- }
-
- @Override
- protected boolean isFinished() {
- return true;
- }
-
- @Override
- protected void end() {
- }
-
- @Override
- protected void interrupted() {
- }
-}
@Override
protected void execute() {
Value rightPistonValue = driveTrain.getRightDriveTrainPiston();
+ System.out.println("I think i'm at " + rightPistonValue);
if (rightPistonValue == Constants.DriveTrain.REVERSE_PISTON_VALUE) {
driveTrain.setHighGear();
} else {
driveTrain.setLowGear();
}
+ // boolean leftPistonValue = driveTrain.getLeftDriveTrainPiston();
+ // if (leftPistonValue == Constants.DriveTrain.RETRACT_VALUE) {
+ // driveTrain.setHighGear();
+ // } else {
+ // driveTrain.setLowGear();
+ // }
}
@Override
--- /dev/null
+package org.usfirst.frc.team3501.robot.subsystems;
+
+import org.usfirst.frc.team3501.robot.Constants;
+import org.usfirst.frc.team3501.robot.MathLib;
+import org.usfirst.frc.team3501.robot.commands.climber.RunWinchContinuous;
+
+import com.ctre.CANTalon;
+
+import edu.wpi.first.wpilibj.command.Subsystem;
+
+public class Climber extends Subsystem {
+ public static Climber climber;
+
+ public static final boolean BRAKE_MODE = true;
+ public static final boolean COAST_MODE = false;
+
+ public static final double CLIMBER_SPEED = 0;
+
+ private CANTalon winch;
+
+ public Climber() {
+ winch = new CANTalon(Constants.Climber.WINCH_PORT);
+ }
+
+ public static Climber getClimber() {
+ if (climber == null) {
+ climber = new Climber();
+ }
+ return climber;
+ }
+
+ public void setMotorValues(double climbingSpeed) {
+ winch.set(MathLib.limitValue(climbingSpeed, 0.0, 1.0));
+ }
+
+ public void stop() {
+ winch.set(0);
+ }
+
+ public void setCANTalonsBrakeMode(boolean mode) {
+ winch.enableBrakeMode(mode);
+ }
+
+ @Override
+ protected void initDefaultCommand() {
+ setDefaultCommand(new RunWinchContinuous());
+ }
+}
public static final double INCHES_PER_PULSE = WHEEL_DIAMETER * Math.PI
/ ENCODER_PULSES_PER_REVOLUTION;
- public static final double MAINTAIN_CLIMBED_POSITION = 0;
- public static final double TIME_TO_CLIMB_FOR = 0;
- public static final double CLIMBER_SPEED = 0;
-
- public static final boolean DRIVE_BRAKE_MODE = true;
- public static final boolean DRIVE_COAST_MODE = false;
-
private static DriveTrain driveTrain;
private final CANTalon frontLeft, frontRight, rearLeft, rearRight;
private final RobotDrive robotDrive;
private final Encoder leftEncoder, rightEncoder;
- private final DoubleSolenoid leftDriveTrainPiston, rightDriveTrainPiston;
+ private final DoubleSolenoid rightDriveTrainPiston;
+ // private final Solenoid leftDriveTrainPiston;
private final DoubleSolenoid gearManipulatorPiston;
private ADXRS450_Gyro imu;
- public boolean shouldBeClimbing = false;
-
private DriveTrain() {
// MOTOR CONTROLLERS
frontLeft = new CANTalon(Constants.DriveTrain.FRONT_LEFT);
this.imu = new ADXRS450_Gyro(Constants.DriveTrain.GYRO_PORT);
// TODO: Not sure if MODULE_NUMBER should be the same for both
- leftDriveTrainPiston = new DoubleSolenoid(
- Constants.DriveTrain.PISTON_MODULE,
- Constants.DriveTrain.LEFT_GEAR_PISTON_FORWARD,
- Constants.DriveTrain.LEFT_GEAR_PISTON_REVERSE);
+ // leftDriveTrainPiston = new Solenoid(Constants.DriveTrain.PISTON_MODULE,
+ // Constants.DriveTrain.LEFT_GEAR_PISTON_PORT);
rightDriveTrainPiston = new DoubleSolenoid(
Constants.DriveTrain.PISTON_MODULE,
- Constants.DriveTrain.RIGHT_GEAR_PISTON_FORWARD,
- Constants.DriveTrain.RIGHT_GEAR_PISTON_REVERSE);
+ Constants.DriveTrain.DRIVETRAIN_GEAR_FORWARD,
+ Constants.DriveTrain.DRIVETRAIN_GEAR_REVERSE);
gearManipulatorPiston = new DoubleSolenoid(
Constants.DriveTrain.PISTON_MODULE,
* @return a value that is the current setpoint for the piston kReverse or
* KForward
*/
- public Value getLeftDriveTrainPiston() {
- return leftDriveTrainPiston.get();
- }
+ // public boolean getLeftDriveTrainPiston() {
+ // return leftDriveTrainPiston.get();
+ // }
/*
* @return a value that is the current setpoint for the piston kReverse or
* Changes the gear to a DoubleSolenoid.Value
*/
private void changeGear(DoubleSolenoid.Value gear) {
- leftDriveTrainPiston.set(gear);
+ System.out.println("shifting to " + gear);
rightDriveTrainPiston.set(gear);
+ System.out.println("after: " + this.getRightDriveTrainPiston());
+
+ //
+ // if (gear == Constants.DriveTrain.FORWARD_PISTON_VALUE)
+ // leftDriveTrainPiston.set(Constants.DriveTrain.EXTEND_VALUE);
+ // else
+ // leftDriveTrainPiston.set(Constants.DriveTrain.RETRACT_VALUE);
}
public Value getGearManipulatorPistonValue() {
protected void initDefaultCommand() {
setDefaultCommand(new JoystickDrive());
}
-
- public void setCANTalonsBrakeMode(boolean mode) {
- frontLeft.enableBrakeMode(mode);
- rearLeft.enableBrakeMode(mode);
-
- frontRight.enableBrakeMode(mode);
- rearRight.enableBrakeMode(mode);
- }
}
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;
- private static final double DEFAULT_SHOOTING_SPEED = 2700; // rpm
- private static final double SHOOTING_SPEED_INCREMENT = 50;
+ private static final double DEFAULT_SHOOTING_SPEED = 2875; // rpm
+ private static final double SHOOTING_SPEED_INCREMENT = 25;
private double targetShootingSpeed = DEFAULT_SHOOTING_SPEED;
private double currentShooterMotorValue = 0;