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   // Drivetrain related objects
  12   private Encoder leftEncoder, rightEncoder;
  13   private CANTalon frontLeft, frontRight, rearLeft, rearRight;
  14
  15   // Drivetrain specific constants that relate to the inches per pulse value for
  16   // the encoders
  17   private final static double WHEEL_DIAMETER = 6.0; // in inches
  18   private final static double PULSES_PER_ROTATION = 256;
  19   private final static double OUTPUT_SPROCKET_DIAMETER = 2.0;
  20   private final static double WHEEL_SPROCKET_DIAMETER = 3.5;
  21
  22   public final static double INCHES_PER_PULSE = (((Math.PI)
  23       * OUTPUT_SPROCKET_DIAMETER / PULSES_PER_ROTATION) / WHEEL_SPROCKET_DIAMETER)
  24       * WHEEL_DIAMETER;
  25
  26   public DriveTrain() {
  27     frontLeft = new CANTalon(Constants.DriveTrain.FRONT_LEFT);
  28     frontRight = new CANTalon(Constants.DriveTrain.FRONT_RIGHT);
  29     rearLeft = new CANTalon(Constants.DriveTrain.REAR_LEFT);
  30     rearRight = new CANTalon(Constants.DriveTrain.REAR_RIGHT);
  31
  32     leftEncoder = new Encoder(Constants.DriveTrain.ENCODER_LEFT_A,
  33         Constants.DriveTrain.ENCODER_LEFT_B, false, EncodingType.k4X);
  34     rightEncoder = new Encoder(Constants.DriveTrain.ENCODER_RIGHT_A,
  35         Constants.DriveTrain.ENCODER_RIGHT_B, false, EncodingType.k4X);
  36     leftEncoder.setDistancePerPulse(INCHES_PER_PULSE);
  37     rightEncoder.setDistancePerPulse(INCHES_PER_PULSE);
  38   }
  39
  40   @Override
  41   protected void initDefaultCommand() {
  42   }
  43
  44   public void resetEncoders() {
  45     leftEncoder.reset();
  46     rightEncoder.reset();
  47   }
  48
  49   // Returns inches per second
  50   public double getRightSpeed() {
  51     return rightEncoder.getRate();
  52   }
  53
  54   public double getLeftSpeed() {
  55     return leftEncoder.getRate();
  56   }
  57
  58   public double getSpeed() {
  59     return (getLeftSpeed() + getRightSpeed()) / 2.0;
  60   }
  61
  62   // Returns distance in in
  63   public double getRightDistance() {
  64     return rightEncoder.getDistance();
  65   }
  66
  67   // Returns distance in in
  68   public double getLeftDistance() {
  69     return leftEncoder.getDistance();
  70   }
  71
  72   public double getDistance() {
  73     return (getRightDistance() + getLeftDistance()) / 2.0;
  74   }
  75
  76   public void stop() {
  77     setMotorSpeeds(0, 0);
  78   }
  79
  80   public void setMotorSpeeds(double leftSpeed, double rightSpeed) {
  81     // speed passed to right motor is negative because right motor rotates in
  82     // opposite direction
  83     this.frontLeft.set(leftSpeed);
  84     this.frontRight.set(-rightSpeed);
  85     this.rearLeft.set(leftSpeed);
  86     this.rearRight.set(-rightSpeed);
  87   }
  88
  89 }