import org.usfirst.frc3501.RiceCatRobot.Robot;
import org.usfirst.frc3501.RiceCatRobot.RobotMap.Direction;
+import org.usfirst.frc3501.RiceCatRobot.subsystems.DriveTrain;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.command.Command;
*
*/
public class DriveFor extends Command {
- private double seconds;
- private Timer timer;
- private 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 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 boolean isFinished() {
- return timer.get() > seconds;
- }
-
- @Override
- protected void end() {
- Robot.driveTrain.arcadeDrive(0, 0);
- }
-
- @Override
- protected void interrupted() {
- end();
+ private double seconds;
+ private double speed;
+ private Timer timer;
+ private Direction direction;
+
+ public DriveFor(double seconds, double speed, Direction direction) {
+ // limit speed to the range [0, MOTOR_MAX_VAL]
+ this.speed = Math.max(speed, -speed);
+ this.speed = Math.min(speed, DriveTrain.MOTOR_MAX_VAL);
+
+ this.seconds = seconds;
+ this.direction = direction;
+ }
+
+ @Override
+ protected void initialize() {
+ timer = new Timer();
+ timer.reset();
+ timer.start();
+ }
+
+ @Override
+ protected void execute() {
+ if (direction == Direction.FORWARD) {
+ Robot.driveTrain.setMotorSpeeds(-speed, -speed);
+ } else if (direction == Direction.BACKWARD) {
+ Robot.driveTrain.setMotorSpeeds(speed, speed);
}
-}
+ }
+
+ @Override
+ protected boolean isFinished() {
+ return timer.get() > seconds;
+ }
+
+ @Override
+ protected void end() {
+ Robot.driveTrain.setMotorSpeeds(0, 0);
+ }
+
+ @Override
+ protected void interrupted() {
+ end();
+ }
+}
\ No newline at end of file
import edu.wpi.first.wpilibj.command.Subsystem;
public class DriveTrain extends Subsystem {
- private CANJaguar frontLeft, frontRight, rearLeft, rearRight;
- private Encoder leftEncoder, rightEncoder;
+ public static final double MOTOR_MAX_VAL = 1;
+ public static final double MOTOR_MIN_VAL = -1;
- 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);
- }
+ private CANJaguar frontLeft, frontRight, rearLeft, rearRight;
+ private Encoder leftEncoder, rightEncoder;
- public void resetEncoders() {
- leftEncoder.reset();
- rightEncoder.reset();
- }
+ 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 double getRightSpeed() {
- // Returns in per second
- return rightEncoder.getRate();
- }
+ public void resetEncoders() {
+ leftEncoder.reset();
+ rightEncoder.reset();
+ }
- public double getLeftSpeed() {
- return leftEncoder.getRate();
- }
+ public double getRightSpeed() {
+ // Returns in per second
+ return rightEncoder.getRate();
+ }
- public double getRightDistance() {
- // Returns distance in in
- return rightEncoder.getDistance();
- }
+ public double getLeftSpeed() {
+ return leftEncoder.getRate();
+ }
- public double getLeftDistance() {
- // Returns distance in in
- return leftEncoder.getDistance();
- }
+ public double getRightDistance() {
+ // Returns distance in in
+ return rightEncoder.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 double getLeftDistance() {
+ // Returns distance in in
+ return leftEncoder.getDistance();
+ }
- 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;
- }
+ 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);
+ }
- 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() {
+ }
}