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 CANTalon frontLeft, frontRight, rearLeft, rearRight;
  13   private Encoder leftEncoder, rightEncoder;
  14
  15   // Drivetrain Specific Constants that relate to the Inches per Pulse value of
  16   // the encoders
  17   private final static double WHEEL_DIAMETER = 6.0; // in inches
  18   private final static double PULSES_PER_ROTATION = 256; // in pulses
  19   private final static double OUTPUT_SPROCKET_DIAMETER = 2.0; // in inches
  20   private final static double WHEEL_SPROCKET_DIAMETER = 3.5; // in inches
  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   public double getRightSpeed() {
  50     return rightEncoder.getRate(); // in inches per second
  51   }
  52
  53   public double getLeftSpeed() {
  54     return leftEncoder.getRate(); // in inches per second
  55   }
  56
  57   public double getSpeed() {
  58     return (getLeftSpeed() + getRightSpeed()) / 2.0; // in inches per second
  59   }
  60
  61   public double getRightDistance() {
  62     return rightEncoder.getDistance(); // in inches
  63   }
  64
  65   public double getLeftDistance() {
  66     return leftEncoder.getDistance(); // in inches
  67   }
  68
  69   public double getDistance() {
  70     return (getRightDistance() + getLeftDistance()) / 2.0; // in inches
  71   }
  72
  73   public void stop() {
  74     setMotorSpeeds(0, 0);
  75   }
  76
  77   public void setMotorSpeeds(double leftSpeed, double rightSpeed) {
  78     // Right motors receive negative values because they turn in the opposite
  79     // direction
  80     this.frontLeft.set(leftSpeed);
  81     this.frontRight.set(-rightSpeed);
  82     this.rearLeft.set(leftSpeed);
  83     this.rearRight.set(-rightSpeed);
  84   }
  85
  86 }