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.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,
  22                 RobotMap.DRIVE_RIGHT_B, 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 getRightDistance() {
  42         // Returns distance in inches
  43         return rightEncoder.getDistance();
  44     }
  45
  46     public double getLeftDistance() {
  47         // Returns distance in inches
  48         return leftEncoder.getDistance();
  49     }
  50
  51     public void setMotorSpeeds(double leftSpeed, double rightSpeed) {
  52         if (Math.abs(leftSpeed) < RobotMap.DRIVE_DEAD_ZONE) {
  53             leftSpeed = 0;
  54         }
  55         if (Math.abs(rightSpeed) < RobotMap.DRIVE_DEAD_ZONE) {
  56             rightSpeed = 0;
  57         }
  58         this.frontLeft.set(leftSpeed);
  59         this.frontRight.set(-rightSpeed);
  60         this.rearLeft.set(leftSpeed);
  61         this.rearRight.set(-rightSpeed);
  62     }
  63
  64     public void arcadeDrive(double yVal, double twist) {
  65         if (yVal < 0 && Math.abs(yVal) < 0.1125) {
  66             yVal = 0;
  67         } else if (yVal > 0 && Math.abs(yVal) < 0.25) {
  68             yVal = 0;
  69         } else if (yVal > 0) {
  70             yVal -= 0.25;
  71         } else if (yVal < 0) {
  72             yVal += 0.15;
  73         }
  74         if (Math.abs(twist) < RobotMap.DRIVE_DEAD_ZONE) {
  75             twist = 0;
  76         }
  77
  78         double leftMotorSpeed;
  79         double rightMotorSpeed;
  80         // adjust the sensitivity (yVal+rootof (yval)) / 2
  81         yVal = (yVal + Math.signum(yVal) * Math.sqrt(Math.abs(yVal))) / 2;
  82         // adjust the sensitivity (twist+rootof (twist)) / 2
  83         twist = (twist + Math.signum(twist) * Math.sqrt(Math.abs(twist))) / 2;
  84         if (yVal > 0) {
  85             if (twist > 0) {
  86                 leftMotorSpeed = yVal - twist;
  87                 rightMotorSpeed = Math.max(yVal, twist);
  88             } else {
  89                 leftMotorSpeed = Math.max(yVal, -twist);
  90                 rightMotorSpeed = yVal + twist;
  91             }
  92         } else {
  93             if (twist > 0.0) {
  94                 leftMotorSpeed = -Math.max(-yVal, twist);
  95                 rightMotorSpeed = yVal + twist;
  96             } else {
  97                 leftMotorSpeed = yVal - twist;
  98                 rightMotorSpeed = -Math.max(-yVal, -twist);
  99             }
 100         }
 101         setMotorSpeeds(leftMotorSpeed, rightMotorSpeed);
 102     }
 103
 104     @Override
 105     protected void initDefaultCommand() {
 106     }
 107 }