import edu.wpi.first.wpilibj.buttons.JoystickButton;
public class OI {
- public static Joystick leftJoystick;
- public static Joystick rightJoystick;
- public static JoystickButton trigger;
- public static JoystickButton toggleCompressor;
- public static JoystickButton toggleClaw;
+ public static Joystick leftJoystick;
+ public static Joystick rightJoystick;
+ public static JoystickButton trigger;
+ public static JoystickButton toggleCompressor;
+ public static JoystickButton toggleClaw;
- public OI() {
- System.out.println("OI is open");
- leftJoystick = new Joystick(RobotMap.LEFT_STICK_PORT);
- rightJoystick = new Joystick(RobotMap.RIGHT_STICK_PORT);
+ public OI() {
+ System.out.println("OI is open");
+ leftJoystick = new Joystick(RobotMap.LEFT_STICK_PORT);
+ rightJoystick = new Joystick(RobotMap.RIGHT_STICK_PORT);
- trigger = new JoystickButton(rightJoystick, RobotMap.TRIGGER_PORT);
+ trigger = new JoystickButton(rightJoystick, RobotMap.TRIGGER_PORT);
- toggleClaw = new JoystickButton(rightJoystick, RobotMap.TOGGLE_PORT);
- toggleClaw.whenPressed(new ToggleClaw());
+ toggleClaw = new JoystickButton(rightJoystick, RobotMap.TOGGLE_PORT);
+ toggleClaw.whenPressed(new ToggleClaw());
- toggleCompressor = new JoystickButton(rightJoystick,
- RobotMap.TOGGLE_COMPRESSOR_PORT);
- toggleCompressor.whenPressed(new ToggleCompressor());
+ toggleCompressor = new JoystickButton(rightJoystick,
+ RobotMap.TOGGLE_COMPRESSOR_PORT);
+ toggleCompressor.whenPressed(new ToggleCompressor());
- }
+ }
}
import edu.wpi.first.wpilibj.command.Scheduler;
public class Robot extends IterativeRobot {
- static Timer timer;
- public static OI oi;
- public static DriveTrain driveTrain;
- public static Arm arm;
- public static Claw claw;
- public static Compressor compressor;
+ static Timer timer;
+ public static OI oi;
+ public static DriveTrain driveTrain;
+ public static Arm arm;
+ public static Claw claw;
+ public static Compressor compressor;
- public void robotInit() {
- RobotMap.init();
- driveTrain = new DriveTrain();
- arm = new Arm();
- claw = new Claw();
- oi = new OI();
- compressor = new Compressor(RobotMap.COMPRESSOR_PORT);
- }
+ public void robotInit() {
+ RobotMap.init();
+ driveTrain = new DriveTrain();
+ arm = new Arm();
+ claw = new Claw();
+ oi = new OI();
+ compressor = new Compressor(RobotMap.COMPRESSOR_PORT);
+ }
- public void autonomousInit() {
- }
+ public void autonomousInit() {
+ }
- public void autonomousPeriodic() {
- Scheduler.getInstance().run();
- }
+ public void autonomousPeriodic() {
+ Scheduler.getInstance().run();
+ }
- public void teleopInit() {
- System.out.println("running teleopInit");
- }
+ public void teleopInit() {
+ System.out.println("running teleopInit");
+ }
- public void teleopPeriodic() {
- Scheduler.getInstance().run();
+ public void teleopPeriodic() {
+ Scheduler.getInstance().run();
- }
+ }
- public void operate() {
- driveTrain.arcadeDrive(OI.rightJoystick.getY(),
- OI.rightJoystick.getTwist());
- claw.doTriggerAction();
- if (OI.leftJoystick.getRawButton(8)) {
- arm.setArmSpeeds(0.3);
- } else if (OI.leftJoystick.getRawButton(9)) {
- arm.setArmSpeeds(-0.3);
- } else if (OI.leftJoystick.getRawButton(6)) {
- arm.setLeft(0.3);
- } else if (OI.leftJoystick.getRawButton(7)) {
- arm.setLeft(-0.3);
- } else if (OI.leftJoystick.getRawButton(11)) {
- arm.setRight(-0.3);
- } else if (OI.leftJoystick.getRawButton(10)) {
- arm.setRight(0.3);
- }
- if (Math.abs(OI.leftJoystick.getY()) < 0.05) {
- arm.setArmSpeeds(0);
+ public void operate() {
+ driveTrain
+ .arcadeDrive(OI.rightJoystick.getY(), OI.rightJoystick.getTwist());
+ claw.doTriggerAction();
+ if (OI.leftJoystick.getRawButton(8)) {
+ arm.setArmSpeeds(0.3);
+ } else if (OI.leftJoystick.getRawButton(9)) {
+ arm.setArmSpeeds(-0.3);
+ } else if (OI.leftJoystick.getRawButton(6)) {
+ arm.setLeft(0.3);
+ } else if (OI.leftJoystick.getRawButton(7)) {
+ arm.setLeft(-0.3);
+ } else if (OI.leftJoystick.getRawButton(11)) {
+ arm.setRight(-0.3);
+ } else if (OI.leftJoystick.getRawButton(10)) {
+ arm.setRight(0.3);
+ }
+ if (Math.abs(OI.leftJoystick.getY()) < 0.05) {
+ arm.setArmSpeeds(0);
- } else {
- arm.fineTuneControl(OI.leftJoystick.getY());
- }
+ } else {
+ arm.fineTuneControl(OI.leftJoystick.getY());
}
+ }
}
* floating around.
*/
public class RobotMap {
- public final static int LEFT_STICK_PORT = 0, RIGHT_STICK_PORT = 1;
- public final static int TRIGGER_PORT = 1, TOGGLE_PORT = 2,
- TOGGLE_COMPRESSOR_PORT = 11;
- public static final int DRIVE_FRONT_LEFT = 4, DRIVE_FRONT_RIGHT = 5,
- DRIVE_REAR_LEFT = 3, DRIVE_REAR_RIGHT = 6;
- public static final int DRIVE_LEFT_A = 3, DRIVE_LEFT_B = 4,
- DRIVE_RIGHT_A = 2, DRIVE_RIGHT_B = 1;
+ public final static int LEFT_STICK_PORT = 0, RIGHT_STICK_PORT = 1;
+ public final static int TRIGGER_PORT = 1, TOGGLE_PORT = 2,
+ TOGGLE_COMPRESSOR_PORT = 11;
+ public static final int DRIVE_FRONT_LEFT = 4, DRIVE_FRONT_RIGHT = 5,
+ DRIVE_REAR_LEFT = 3, DRIVE_REAR_RIGHT = 6;
+ public static final int DRIVE_LEFT_A = 3, DRIVE_LEFT_B = 4,
+ DRIVE_RIGHT_A = 2, DRIVE_RIGHT_B = 1;
- public static final double DISTANCE_PER_PULSE = ((3.66 / 5.14) * 6 * Math.PI) / 256;
+ public static final double DISTANCE_PER_PULSE = ((3.66 / 5.14) * 6 * Math.PI) / 256;
- public static final int ARM_LEFT = 2, ARM_RIGHT = 7;
- public static final double ARM_HIGH_SPEED = 0.5, ARM_LOW_SPEED = 0.5;
+ public static final int ARM_LEFT = 2, ARM_RIGHT = 7;
+ public static final double ARM_HIGH_SPEED = 0.5, ARM_LOW_SPEED = 0.5;
- // Claw
- public static final int SOLENOID_FORWARD = 0, SOLENOID_REVERSE = 1,
- MODULE_NUMBER = 0;
- public final static Value open = DoubleSolenoid.Value.kForward,
- close = DoubleSolenoid.Value.kReverse;
- public static double DRIVE_DEAD_ZONE = 0.25;
- // Compressor
- public static final int COMPRESSOR_PORT = 0;
+ // Claw
+ public static final int SOLENOID_FORWARD = 0, SOLENOID_REVERSE = 1,
+ MODULE_NUMBER = 0;
+ public final static Value open = DoubleSolenoid.Value.kForward,
+ close = DoubleSolenoid.Value.kReverse;
+ public static double DRIVE_DEAD_ZONE = 0.25;
+ // Compressor
+ public static final int COMPRESSOR_PORT = 0;
- public static void init() {
- }
+ public static void init() {
+ }
- public static enum Direction {
- LEFT, RIGHT, DOWN, UP, FORWARD, BACKWARD;
- }
+ public static enum Direction {
+ LEFT, RIGHT, DOWN, UP, FORWARD, BACKWARD;
+ }
}
*/
public class CloseClaw extends Command {
- public CloseClaw() {
- requires(Robot.claw);
- }
+ public CloseClaw() {
+ requires(Robot.claw);
+ }
- protected void initialize() {
- System.out.println("IN INIT CLOSECLAW");
- }
+ protected void initialize() {
+ System.out.println("IN INIT CLOSECLAW");
+ }
- protected void execute() {
- Robot.claw.closeClaw();
- System.out.println("Closing claw");
- }
+ protected void execute() {
+ Robot.claw.closeClaw();
+ System.out.println("Closing claw");
+ }
- protected boolean isFinished() {
- System.out.println("Claw Closed");
- return !Robot.claw.isOpen();
- }
+ protected boolean isFinished() {
+ System.out.println("Claw Closed");
+ return !Robot.claw.isOpen();
+ }
- protected void end() {
+ protected void end() {
- }
+ }
- protected void interrupted() {
- }
+ protected void interrupted() {
+ }
}
*
*/
public class DriveFor extends Command {
- private double seconds;
- private Timer timer;
- private Direction direction;
+ private double seconds;
+ private Timer timer;
+ private Direction direction;
- public DriveFor(double seconds, Direction direction) {
- this.seconds = seconds;
- this.direction = direction;
+ public DriveFor(double seconds, Direction direction) {
+ this.seconds = seconds;
+ this.direction = direction;
- }
+ }
- @Override
- protected void initialize() {
- timer = new Timer();
- timer.reset();
- timer.start();
- }
+ @Override
+ protected void initialize() {
+ timer = new Timer();
+ timer.reset();
+ timer.start();
+ }
- @Override
- protected void execute() {
- System.out.println(timer.get());
- if (direction == Direction.FORWARD) {
- if (timer.get() < seconds * 0.2) { // for the first 20% of time, run
- // the robot at -.5 speed
- Robot.driveTrain.arcadeDrive(-0.3, 0);
- } else if (timer.get() >= seconds * 0.2
- && timer.get() <= seconds * 0.8) { // for the +20% - 75%
- // time, move the robot
- // at -.3 speed.
- Robot.driveTrain.arcadeDrive(-0.5, 0);
- } else if (timer.get() < seconds) {
- Robot.driveTrain.arcadeDrive(-0.25, 0);
- } else {
- Robot.driveTrain.arcadeDrive(0, 0);
- }
- } else if (direction == Direction.BACKWARD) {
- if (timer.get() < seconds * 0.2) {
- Robot.driveTrain.arcadeDrive(0.3, 0);
- } else if (timer.get() >= seconds * 0.2
- && timer.get() <= seconds * 0.8) {
- Robot.driveTrain.arcadeDrive(0.5, 0);
- } else if (timer.get() < seconds) {
- Robot.driveTrain.arcadeDrive(0.25, 0);
- } else {
- Robot.driveTrain.arcadeDrive(0, 0);
- }
- }
+ @Override
+ protected void execute() {
+ System.out.println(timer.get());
+ if (direction == Direction.FORWARD) {
+ if (timer.get() < seconds * 0.2) { // for the first 20% of time, run
+ // the robot at -.5 speed
+ Robot.driveTrain.arcadeDrive(-0.3, 0);
+ } else if (timer.get() >= seconds * 0.2 && timer.get() <= seconds * 0.8) {
+ Robot.driveTrain.arcadeDrive(-0.5, 0);
+ } else if (timer.get() < seconds) {
+ Robot.driveTrain.arcadeDrive(-0.25, 0);
+ } else {
+ Robot.driveTrain.arcadeDrive(0, 0);
+ }
+ } else if (direction == Direction.BACKWARD) {
+ if (timer.get() < seconds * 0.2) {
+ Robot.driveTrain.arcadeDrive(0.3, 0);
+ } else if (timer.get() >= seconds * 0.2 && timer.get() <= seconds * 0.8) {
+ Robot.driveTrain.arcadeDrive(0.5, 0);
+ } else if (timer.get() < seconds) {
+ Robot.driveTrain.arcadeDrive(0.25, 0);
+ } else {
+ Robot.driveTrain.arcadeDrive(0, 0);
+ }
}
+ }
- @Override
- protected boolean isFinished() {
- return timer.get() > seconds;
- }
+ @Override
+ protected boolean isFinished() {
+ return timer.get() > seconds;
+ }
- @Override
- protected void end() {
- Robot.driveTrain.arcadeDrive(0, 0);
- }
+ @Override
+ protected void end() {
+ Robot.driveTrain.arcadeDrive(0, 0);
+ }
- @Override
- protected void interrupted() {
- end();
- }
+ @Override
+ protected void interrupted() {
+ end();
+ }
}
*
*/
public class MoveArmFor extends Command {
- private double seconds;
- private Timer timer;
- private Direction direction;
-
- /*
- * @param Direction must be up or down
- */
- public MoveArmFor(double seconds, Direction direction) {
- this.seconds = seconds;
- this.direction = direction;
+ private double seconds;
+ private Timer timer;
+ private Direction direction;
+
+ /*
+ * @param Direction must be up or down
+ */
+ public MoveArmFor(double seconds, Direction direction) {
+ this.seconds = seconds;
+ this.direction = direction;
+ }
+
+ @Override
+ protected void initialize() {
+ timer = new Timer();
+ timer.start();
+ }
+
+ @Override
+ protected void execute() {
+ if (direction == Direction.UP) {
+ Robot.arm.setArmSpeeds(-RobotMap.ARM_LOW_SPEED);
+ } else if (direction == Direction.DOWN) {
+ Robot.arm.setArmSpeeds(RobotMap.ARM_LOW_SPEED);
}
+ }
- @Override
- protected void initialize() {
- timer = new Timer();
- timer.start();
- }
-
- @Override
- protected void execute() {
- if (direction == Direction.UP) {
- Robot.arm.setArmSpeeds(-RobotMap.ARM_LOW_SPEED);
- } else if (direction == Direction.DOWN) {
- Robot.arm.setArmSpeeds(RobotMap.ARM_LOW_SPEED);
- }
- }
-
- @Override
- protected boolean isFinished() {
- if (timer.get() > seconds) {
- Robot.arm.setArmSpeeds(0);
- }
- return timer.get() > seconds;
- }
-
- @Override
- protected void end() {
- Robot.arm.setArmSpeeds(0);
- }
-
- @Override
- protected void interrupted() {
- end();
+ @Override
+ protected boolean isFinished() {
+ if (timer.get() > seconds) {
+ Robot.arm.setArmSpeeds(0);
}
+ return timer.get() > seconds;
+ }
+
+ @Override
+ protected void end() {
+ Robot.arm.setArmSpeeds(0);
+ }
+
+ @Override
+ protected void interrupted() {
+ end();
+ }
}
*/
public class OpenClaw extends Command {
- public OpenClaw() {
- requires(Robot.claw);
- }
+ public OpenClaw() {
+ requires(Robot.claw);
+ }
- protected void initialize() {
- System.out.println("IN INIT OPENCLAW");
- }
+ protected void initialize() {
+ System.out.println("IN INIT OPENCLAW");
+ }
- protected void execute() {
- Robot.claw.openClaw();
- System.out.println("Opening Claw");
- }
+ protected void execute() {
+ Robot.claw.openClaw();
+ System.out.println("Opening Claw");
+ }
- protected boolean isFinished() {
- System.out.println("Claw Opened");
- return Robot.claw.isOpen();
- }
+ protected boolean isFinished() {
+ System.out.println("Claw Opened");
+ return Robot.claw.isOpen();
+ }
- protected void end() {
+ protected void end() {
- }
+ }
- protected void interrupted() {
- }
+ protected void interrupted() {
+ }
}
*/
public class ToggleClaw extends Command {
- public ToggleClaw() {
+ public ToggleClaw() {
- }
+ }
- protected void initialize() {
- Robot.claw.toggleOn = !Robot.claw.toggleOn;
- }
+ protected void initialize() {
+ Robot.claw.toggleOn = !Robot.claw.toggleOn;
+ }
- protected void execute() {
- if (Robot.claw.toggleOn && Robot.claw.isOpen()) {
- Robot.claw.closeClaw();
- }
+ protected void execute() {
+ if (Robot.claw.toggleOn && Robot.claw.isOpen()) {
+ Robot.claw.closeClaw();
}
+ }
- protected boolean isFinished() {
- return true;
- }
+ protected boolean isFinished() {
+ return true;
+ }
- protected void end() {
+ protected void end() {
- }
+ }
- protected void interrupted() {
- }
+ protected void interrupted() {
+ }
}
*/
public class ToggleCompressor extends Command {
- public ToggleCompressor() {
+ public ToggleCompressor() {
+ }
+
+ // Called just before this Command runs the first time
+ protected void initialize() {
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ protected void execute() {
+ if (Robot.compressor.enabled()) {
+ Robot.compressor.stop();
+ } else {
+ Robot.compressor.start();
}
- // Called just before this Command runs the first time
- protected void initialize() {
- }
-
- // Called repeatedly when this Command is scheduled to run
- protected void execute() {
- if (Robot.compressor.enabled()) {
- Robot.compressor.stop();
- } else {
- Robot.compressor.start();
- }
+ }
- }
+ // Make this return true when this Command no longer needs to run execute()
+ protected boolean isFinished() {
+ return true;
+ }
- // Make this return true when this Command no longer needs to run execute()
- protected boolean isFinished() {
- return true;
- }
+ // Called once after isFinished returns true
+ protected void end() {
- // Called once after isFinished returns true
- protected void end() {
+ }
- }
-
- // Called when another command which requires one or more of the same
- // subsystems is scheduled to run
- protected void interrupted() {
- }
+ // Called when another command which requires one or more of the same
+ // subsystems is scheduled to run
+ protected void interrupted() {
+ }
}
import edu.wpi.first.wpilibj.command.Command;
public class TurnFor extends Command {
- private double seconds;
- private Timer timer;
- private Direction direction;
-
- public TurnFor(double seconds, Direction direction) {
- this.seconds = seconds;
- this.direction = direction;
- }
-
- @Override
- protected void initialize() {
- timer = new Timer();
- timer.start();
- }
-
- @Override
- protected void execute() {
- if (direction == Direction.LEFT) {
- Robot.driveTrain.arcadeDrive(0, -0.5);
- } else if (direction == Direction.RIGHT) {
- Robot.driveTrain.arcadeDrive(0, 0.5);
- } else {
- Robot.driveTrain.arcadeDrive(0, 0);
- }
+ private double seconds;
+ private Timer timer;
+ private Direction direction;
+
+ public TurnFor(double seconds, Direction direction) {
+ this.seconds = seconds;
+ this.direction = direction;
+ }
+
+ @Override
+ protected void initialize() {
+ timer = new Timer();
+ timer.start();
+ }
+
+ @Override
+ protected void execute() {
+ if (direction == Direction.LEFT) {
+ Robot.driveTrain.arcadeDrive(0, -0.5);
+ } else if (direction == Direction.RIGHT) {
+ Robot.driveTrain.arcadeDrive(0, 0.5);
+ } else {
+ Robot.driveTrain.arcadeDrive(0, 0);
}
-
- @Override
- protected boolean isFinished() {
- System.out.println(timer.get());
- System.out.println(seconds);
- if (timer.get() > seconds) {
- Robot.driveTrain.arcadeDrive(0, 0);
- }
- return timer.get() > seconds;
+ }
+
+ @Override
+ protected boolean isFinished() {
+ System.out.println(timer.get());
+ System.out.println(seconds);
+ if (timer.get() > seconds) {
+ Robot.driveTrain.arcadeDrive(0, 0);
}
+ return timer.get() > seconds;
+ }
- @Override
- protected void end() {
- }
+ @Override
+ protected void end() {
+ }
- @Override
- protected void interrupted() {
- end();
- }
+ @Override
+ protected void interrupted() {
+ end();
+ }
}
import edu.wpi.first.wpilibj.command.Subsystem;
public class Arm extends Subsystem {
- private CANJaguar left, right;
-
- public Arm() {
- left = new CANJaguar(RobotMap.ARM_LEFT);
- right = new CANJaguar(RobotMap.ARM_RIGHT);
- }
-
- public void initDefaultCommand() {
- }
-
- public void fineTuneControl(double d) {
- if (Math.abs(d) < 0.05) {
- d = 0;
- } else if (d > 0) {
- d *= d;
- } else {
- d *= -d;
- }
- setArmSpeeds(d);
- }
-
- public void setLeft(double speed) {
- left.set(-speed);
- }
-
- public void setRight(double speed) {
- right.set(-speed);
- }
-
- public void setArmSpeeds(double speed) {
- setLeft(speed);
- setRight(speed);
- }
-
- public void stop() {
- left.set(0);
- right.set(0);
+ private CANJaguar left, right;
+
+ public Arm() {
+ left = new CANJaguar(RobotMap.ARM_LEFT);
+ right = new CANJaguar(RobotMap.ARM_RIGHT);
+ }
+
+ public void initDefaultCommand() {
+ }
+
+ public void fineTuneControl(double d) {
+ if (Math.abs(d) < 0.05) {
+ d = 0;
+ } else if (d > 0) {
+ d *= d;
+ } else {
+ d *= -d;
}
+ setArmSpeeds(d);
+ }
+
+ public void setLeft(double speed) {
+ left.set(-speed);
+ }
+
+ public void setRight(double speed) {
+ right.set(-speed);
+ }
+
+ public void setArmSpeeds(double speed) {
+ setLeft(speed);
+ setRight(speed);
+ }
+
+ public void stop() {
+ left.set(0);
+ right.set(0);
+ }
}
public class Claw extends Subsystem {
- private DoubleSolenoid solenoid;
- public boolean toggleOn = false;
-
- public Claw() {
- solenoid = new DoubleSolenoid(RobotMap.MODULE_NUMBER,
- RobotMap.SOLENOID_FORWARD, RobotMap.SOLENOID_REVERSE);
- }
-
- public void initDefaultCommand() {
- }
-
- public void closeClaw() {
- solenoid.set(RobotMap.close);
- }
-
- public void openClaw() {
- solenoid.set(RobotMap.open);
- }
-
- public boolean isOpen() {
- return solenoid.get() == RobotMap.open;
- }
-
- public void doTriggerAction() {
- if (!Robot.claw.toggleOn) {
- if (OI.rightJoystick.getRawButton(RobotMap.TRIGGER_PORT)) {
- if (Robot.claw.isOpen()) {
- new CloseClaw().start();
- }
- } else {
- if (!Robot.claw.isOpen()) {
- new OpenClaw().start();
- }
- }
+ private DoubleSolenoid solenoid;
+ public boolean toggleOn = false;
+
+ public Claw() {
+ solenoid = new DoubleSolenoid(RobotMap.MODULE_NUMBER,
+ RobotMap.SOLENOID_FORWARD, RobotMap.SOLENOID_REVERSE);
+ }
+
+ public void initDefaultCommand() {
+ }
+
+ public void closeClaw() {
+ solenoid.set(RobotMap.close);
+ }
+
+ public void openClaw() {
+ solenoid.set(RobotMap.open);
+ }
+
+ public boolean isOpen() {
+ return solenoid.get() == RobotMap.open;
+ }
+
+ public void doTriggerAction() {
+ if (!Robot.claw.toggleOn) {
+ if (OI.rightJoystick.getRawButton(RobotMap.TRIGGER_PORT)) {
+ if (Robot.claw.isOpen()) {
+ new CloseClaw().start();
+ }
+ } else {
+ if (!Robot.claw.isOpen()) {
+ new OpenClaw().start();
}
+ }
}
+ }
}
import edu.wpi.first.wpilibj.command.Subsystem;
public class DriveTrain extends Subsystem {
- private CANJaguar frontLeft, frontRight, rearLeft, rearRight;
- private Encoder leftEncoder, rightEncoder;
+ private CANJaguar frontLeft, frontRight, rearLeft, rearRight;
+ private Encoder leftEncoder, rightEncoder;
- public DriveTrain() {
- frontLeft = new CANJaguar(RobotMap.DRIVE_FRONT_LEFT);
- frontRight = new CANJaguar(RobotMap.DRIVE_FRONT_RIGHT);
- rearLeft = new CANJaguar(RobotMap.DRIVE_REAR_LEFT);
- rearRight = new CANJaguar(RobotMap.DRIVE_REAR_RIGHT);
- leftEncoder = new Encoder(RobotMap.DRIVE_LEFT_A, RobotMap.DRIVE_LEFT_B,
- false, EncodingType.k4X);
- rightEncoder = new Encoder(RobotMap.DRIVE_RIGHT_A,
- RobotMap.DRIVE_RIGHT_B, false, EncodingType.k4X);
- leftEncoder.setDistancePerPulse(RobotMap.DISTANCE_PER_PULSE);
- rightEncoder.setDistancePerPulse(RobotMap.DISTANCE_PER_PULSE);
- }
+ public DriveTrain() {
+ frontLeft = new CANJaguar(RobotMap.DRIVE_FRONT_LEFT);
+ frontRight = new CANJaguar(RobotMap.DRIVE_FRONT_RIGHT);
+ rearLeft = new CANJaguar(RobotMap.DRIVE_REAR_LEFT);
+ rearRight = new CANJaguar(RobotMap.DRIVE_REAR_RIGHT);
+ leftEncoder = new Encoder(RobotMap.DRIVE_LEFT_A, RobotMap.DRIVE_LEFT_B,
+ false, EncodingType.k4X);
+ rightEncoder = new Encoder(RobotMap.DRIVE_RIGHT_A, RobotMap.DRIVE_RIGHT_B,
+ false, EncodingType.k4X);
+ leftEncoder.setDistancePerPulse(RobotMap.DISTANCE_PER_PULSE);
+ rightEncoder.setDistancePerPulse(RobotMap.DISTANCE_PER_PULSE);
+ }
- public void resetEncoders() {
- leftEncoder.reset();
- rightEncoder.reset();
- }
+ public void resetEncoders() {
+ leftEncoder.reset();
+ rightEncoder.reset();
+ }
- public double getRightSpeed() {
- // Returns in per second
- return rightEncoder.getRate();
- }
+ public double getRightSpeed() {
+ // Returns in per second
+ return rightEncoder.getRate();
+ }
- public double getLeftSpeed() {
- return leftEncoder.getRate();
- }
+ public double getLeftSpeed() {
+ return leftEncoder.getRate();
+ }
- public double getRightDistance() {
- // Returns distance in in
- return rightEncoder.getDistance();
- }
+ public double getRightDistance() {
+ // Returns distance in in
+ return rightEncoder.getDistance();
+ }
- public double getLeftDistance() {
- // Returns distance in in
- return leftEncoder.getDistance();
- }
+ public double getLeftDistance() {
+ // Returns distance in in
+ return leftEncoder.getDistance();
+ }
- public void setMotorSpeeds(double leftSpeed, double rightSpeed) {
- if (Math.abs(leftSpeed) < RobotMap.DRIVE_DEAD_ZONE) {
- leftSpeed = 0;
- }
- if (Math.abs(rightSpeed) < RobotMap.DRIVE_DEAD_ZONE) {
- rightSpeed = 0;
- }
- this.frontLeft.set(leftSpeed);
- this.frontRight.set(-rightSpeed);
- this.rearLeft.set(leftSpeed);
- this.rearRight.set(-rightSpeed);
+ public void setMotorSpeeds(double leftSpeed, double rightSpeed) {
+ if (Math.abs(leftSpeed) < RobotMap.DRIVE_DEAD_ZONE) {
+ leftSpeed = 0;
}
+ if (Math.abs(rightSpeed) < RobotMap.DRIVE_DEAD_ZONE) {
+ rightSpeed = 0;
+ }
+ this.frontLeft.set(leftSpeed);
+ this.frontRight.set(-rightSpeed);
+ this.rearLeft.set(leftSpeed);
+ this.rearRight.set(-rightSpeed);
+ }
- public void arcadeDrive(double yVal, double twist) {
- if (yVal < 0 && Math.abs(yVal) < 0.1125) {
- yVal = 0;
- } else if (yVal > 0 && Math.abs(yVal) < 0.25) {
- yVal = 0;
- } else if (yVal > 0) {
- yVal -= 0.25;
- } else if (yVal < 0) {
- yVal += 0.15;
- }
- if (Math.abs(twist) < RobotMap.DRIVE_DEAD_ZONE) {
- twist = 0;
- }
-
- double leftMotorSpeed;
- double rightMotorSpeed;
- // adjust the sensitivity (yVal+rootof (yval)) / 2
- yVal = (yVal + Math.signum(yVal) * Math.sqrt(Math.abs(yVal))) / 2;
- // adjust the sensitivity (twist+rootof (twist)) / 2
- twist = (twist + Math.signum(twist) * Math.sqrt(Math.abs(twist))) / 2;
- if (yVal > 0) {
- if (twist > 0) {
- leftMotorSpeed = yVal - twist;
- rightMotorSpeed = Math.max(yVal, twist);
- } else {
- leftMotorSpeed = Math.max(yVal, -twist);
- rightMotorSpeed = yVal + twist;
- }
- } else {
- if (twist > 0.0) {
- leftMotorSpeed = -Math.max(-yVal, twist);
- rightMotorSpeed = yVal + twist;
- } else {
- leftMotorSpeed = yVal - twist;
- rightMotorSpeed = -Math.max(-yVal, -twist);
- }
- }
- setMotorSpeeds(leftMotorSpeed, rightMotorSpeed);
+ public void arcadeDrive(double yVal, double twist) {
+ if (yVal < 0 && Math.abs(yVal) < 0.1125) {
+ yVal = 0;
+ } else if (yVal > 0 && Math.abs(yVal) < 0.25) {
+ yVal = 0;
+ } else if (yVal > 0) {
+ yVal -= 0.25;
+ } else if (yVal < 0) {
+ yVal += 0.15;
+ }
+ if (Math.abs(twist) < RobotMap.DRIVE_DEAD_ZONE) {
+ twist = 0;
}
- @Override
- protected void initDefaultCommand() {
+ double leftMotorSpeed;
+ double rightMotorSpeed;
+ // adjust the sensitivity (yVal+rootof (yval)) / 2
+ yVal = (yVal + Math.signum(yVal) * Math.sqrt(Math.abs(yVal))) / 2;
+ // adjust the sensitivity (twist+rootof (twist)) / 2
+ twist = (twist + Math.signum(twist) * Math.sqrt(Math.abs(twist))) / 2;
+ if (yVal > 0) {
+ if (twist > 0) {
+ leftMotorSpeed = yVal - twist;
+ rightMotorSpeed = Math.max(yVal, twist);
+ } else {
+ leftMotorSpeed = Math.max(yVal, -twist);
+ rightMotorSpeed = yVal + twist;
+ }
+ } else {
+ if (twist > 0.0) {
+ leftMotorSpeed = -Math.max(-yVal, twist);
+ rightMotorSpeed = yVal + twist;
+ } else {
+ leftMotorSpeed = yVal - twist;
+ rightMotorSpeed = -Math.max(-yVal, -twist);
+ }
}
+ setMotorSpeeds(leftMotorSpeed, rightMotorSpeed);
+ }
+
+ @Override
+ protected void initDefaultCommand() {
+ }
}
projects / 3501 / 2015-FRC-Spark / commitdiff