package org.usfirst.frc3501.RiceCatRobot.subsystems;
-import org.usfirst.frc3501.RiceCatRobot.RobotMap;
+import org.usfirst.frc3501.RiceCatRobot.robot.RobotMap;
import edu.wpi.first.wpilibj.CANJaguar;
import edu.wpi.first.wpilibj.Encoder;
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 inches per second
- return rightEncoder.getRate();
- }
+ public double getRightSpeed() {
+ // Returns inches per second
+ return rightEncoder.getRate();
+ }
- public double getLeftSpeed() {
- return leftEncoder.getRate();
- }
-
- public double getAverageSpeed() {
- return (getRightSpeed() + getLeftSpeed())/2;
- }
+ public double getLeftSpeed() {
+ return leftEncoder.getRate();
+ }
- public double getRightDistance() {
- // Returns distance in inches
- return rightEncoder.getDistance();
- }
+ public double getAverageSpeed() {
+ return (getRightSpeed() + getLeftSpeed()) / 2;
+ }
- public double getLeftDistance() {
- // Returns distance in inches
- return leftEncoder.getDistance();
- }
+ public double getRightDistance() {
+ // Returns distance in inches
+ return rightEncoder.getDistance();
+ }
+
+ public double getLeftDistance() {
+ // Returns distance in inches
+ return leftEncoder.getDistance();
+ }
+
+ public void stop() {
+ setMotorSpeeds(0, 0);
+ }
- public void stop() {
- setMotorSpeeds(0, 0);
+ public void setMotorSpeeds(double leftSpeed, double rightSpeed) {
+ if (Math.abs(leftSpeed) < RobotMap.DRIVE_DEAD_ZONE) {
+ leftSpeed = 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);
+ 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() {
+ }
}