challenge-bot.com Git - 3501/2015-FRC-Spark/blob - src/org/usfirst/frc3501/RiceCatRobot/subsystems/DriveTrain.java

   1 package org.usfirst.frc3501.RiceCatRobot.subsystems;
   2
   3 import org.usfirst.frc3501.RiceCatRobot.robot.RobotMap;
   4
   5 import edu.wpi.first.wpilibj.CANJaguar;
   6 import edu.wpi.first.wpilibj.Encoder;
   7 import edu.wpi.first.wpilibj.CounterBase.EncodingType;
   8 import edu.wpi.first.wpilibj.command.Subsystem;
   9
  10 public class DriveTrain extends Subsystem {
  11   private CANJaguar frontLeft, frontRight, rearLeft, rearRight;
  12   private Encoder leftEncoder, rightEncoder;
  13
  14   public DriveTrain() {
  15     frontLeft = new CANJaguar(RobotMap.DRIVE_FRONT_LEFT);
  16     frontRight = new CANJaguar(RobotMap.DRIVE_FRONT_RIGHT);
  17     rearLeft = new CANJaguar(RobotMap.DRIVE_REAR_LEFT);
  18     rearRight = new CANJaguar(RobotMap.DRIVE_REAR_RIGHT);
  19     leftEncoder = new Encoder(RobotMap.DRIVE_LEFT_A, RobotMap.DRIVE_LEFT_B,
  20         false, EncodingType.k4X);
  21     rightEncoder = new Encoder(RobotMap.DRIVE_RIGHT_A, RobotMap.DRIVE_RIGHT_B,
  22         false, EncodingType.k4X);
  23     leftEncoder.setDistancePerPulse(RobotMap.DISTANCE_PER_PULSE);
  24     rightEncoder.setDistancePerPulse(RobotMap.DISTANCE_PER_PULSE);
  25   }
  26
  27   public void resetEncoders() {
  28     leftEncoder.reset();
  29     rightEncoder.reset();
  30   }
  31
  32   public double getRightSpeed() {
  33     // Returns inches per second
  34     return rightEncoder.getRate();
  35   }
  36
  37   public double getLeftSpeed() {
  38     return leftEncoder.getRate();
  39   }
  40
  41   public double getAverageSpeed() {
  42     return (getRightSpeed() + getLeftSpeed()) / 2;
  43   }
  44
  45   public double getRightDistance() {
  46     // Returns distance in inches
  47     return rightEncoder.getDistance();
  48   }
  49
  50   public double getLeftDistance() {
  51     // Returns distance in inches
  52     return leftEncoder.getDistance();
  53   }
  54
  55   public void stop() {
  56     setMotorSpeeds(0, 0);
  57   }
  58
  59   public void setMotorSpeeds(double leftSpeed, double rightSpeed) {
  60     if (Math.abs(leftSpeed) < RobotMap.DRIVE_DEAD_ZONE) {
  61       leftSpeed = 0;
  62     }
  63     if (Math.abs(rightSpeed) < RobotMap.DRIVE_DEAD_ZONE) {
  64       rightSpeed = 0;
  65     }
  66     this.frontLeft.set(leftSpeed);
  67     this.frontRight.set(-rightSpeed);
  68     this.rearLeft.set(leftSpeed);
  69     this.rearRight.set(-rightSpeed);
  70   }
  71
  72   public void arcadeDrive(double yVal, double twist) {
  73     if (yVal < 0 && Math.abs(yVal) < 0.1125) {
  74       yVal = 0;
  75     } else if (yVal > 0 && Math.abs(yVal) < 0.25) {
  76       yVal = 0;
  77     } else if (yVal > 0) {
  78       yVal -= 0.25;
  79     } else if (yVal < 0) {
  80       yVal += 0.15;
  81     }
  82     if (Math.abs(twist) < RobotMap.DRIVE_DEAD_ZONE) {
  83       twist = 0;
  84     }
  85
  86     double leftMotorSpeed;
  87     double rightMotorSpeed;
  88     // adjust the sensitivity (yVal+rootof (yval)) / 2
  89     yVal = (yVal + Math.signum(yVal) * Math.sqrt(Math.abs(yVal))) / 2;
  90     // adjust the sensitivity (twist+rootof (twist)) / 2
  91     twist = (twist + Math.signum(twist) * Math.sqrt(Math.abs(twist))) / 2;
  92     if (yVal > 0) {
  93       if (twist > 0) {
  94         leftMotorSpeed = yVal - twist;
  95         rightMotorSpeed = Math.max(yVal, twist);
  96       } else {
  97         leftMotorSpeed = Math.max(yVal, -twist);
  98         rightMotorSpeed = yVal + twist;
  99       }
 100     } else {
 101       if (twist > 0.0) {
 102         leftMotorSpeed = -Math.max(-yVal, twist);
 103         rightMotorSpeed = yVal + twist;
 104       } else {
 105         leftMotorSpeed = yVal - twist;
 106         rightMotorSpeed = -Math.max(-yVal, -twist);
 107       }
 108     }
 109     setMotorSpeeds(leftMotorSpeed, rightMotorSpeed);
 110   }
 111
 112   @Override
 113   protected void initDefaultCommand() {
 114   }
 115 }