challenge-bot.com Git - 3501/stronghold-2016/blob - src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java

   1 package org.usfirst.frc.team3501.robot.subsystems;
   2
   3 import org.usfirst.frc.team3501.robot.Constants;
   4
   5 import edu.wpi.first.wpilibj.CANTalon;
   6 import edu.wpi.first.wpilibj.CounterBase.EncodingType;
   7 import edu.wpi.first.wpilibj.Encoder;
   8 import edu.wpi.first.wpilibj.command.Subsystem;
   9
  10 public class DriveTrain extends Subsystem {
  11         private CANTalon frontLeft, frontRight, rearLeft, rearRight;
  12         // operational constants
  13
  14         // inches/pulse
  15         private final static double WHEEL_DIAMETER = 6.0; // in inches
  16         private final static double PULSES_PER_ROTATION = 256;
  17         private final static double OUTPUT_SPROCKET_DIAMETER = 2.0;
  18         private final static double WHEEL_SPROCKET_DIAMETER = 3.5;
  19
  20         public final static double INCHES_PER_PULSE = (((Math.PI)
  21                         * OUTPUT_SPROCKET_DIAMETER / PULSES_PER_ROTATION) / WHEEL_SPROCKET_DIAMETER)
  22                         * WHEEL_DIAMETER;
  23         private Encoder leftEncoder, rightEncoder;
  24
  25         public DriveTrain() {
  26                 frontLeft = new CANTalon(Constants.DriveTrain.FRONT_LEFT);
  27                 frontRight = new CANTalon(Constants.DriveTrain.FRONT_RIGHT);
  28                 rearLeft = new CANTalon(Constants.DriveTrain.REAR_LEFT);
  29                 rearRight = new CANTalon(Constants.DriveTrain.REAR_RIGHT);
  30                 leftEncoder = new Encoder(Constants.DriveTrain.ENCODER_LEFT_A,
  31                                 Constants.DriveTrain.ENCODER_LEFT_B, false, EncodingType.k4X);
  32                 rightEncoder = new Encoder(Constants.DriveTrain.ENCODER_RIGHT_A,
  33                                 Constants.DriveTrain.ENCODER_RIGHT_B, false, EncodingType.k4X);
  34                 leftEncoder.setDistancePerPulse(INCHES_PER_PULSE);
  35                 rightEncoder.setDistancePerPulse(INCHES_PER_PULSE);
  36         }
  37
  38         @Override
  39         protected void initDefaultCommand() {
  40         }
  41
  42         public void resetEncoders() {
  43                 leftEncoder.reset();
  44                 rightEncoder.reset();
  45         }
  46
  47         // Returns inches per second
  48         public double getRightSpeed() {
  49                 return rightEncoder.getRate();
  50         }
  51
  52         public double getLeftSpeed() {
  53                 return leftEncoder.getRate();
  54         }
  55
  56         public double getSpeed() {
  57                 return (getLeftSpeed() + getRightSpeed()) / 2.0;
  58         }
  59
  60         // Returns distance in in
  61         public double getRightDistance() {
  62                 return rightEncoder.getDistance();
  63         }
  64
  65         // Returns distance in in
  66         public double getLeftDistance() {
  67                 return leftEncoder.getDistance();
  68         }
  69
  70         public double getDistance() {
  71                 return (getRightDistance() + getLeftDistance()) / 2.0;
  72         }
  73
  74         public void stop() {
  75                 setMotorSpeeds(0, 0);
  76         }
  77
  78         public void setMotorSpeeds(double leftSpeed, double rightSpeed) {
  79                 this.frontLeft.set(leftSpeed);
  80                 this.frontRight.set(-rightSpeed);
  81                 this.rearLeft.set(leftSpeed);
  82                 this.rearRight.set(-rightSpeed);
  83         }
  84
  85 }