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   // TODO: More descriptive comments
  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     // TODO: Same thing add newlines between different groups of declarations
  31     // and add comments
  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     this.frontLeft.set(leftSpeed);
  82     this.frontRight.set(-rightSpeed);
  83     this.rearLeft.set(leftSpeed);
  84     this.rearRight.set(-rightSpeed);
  85     // TODO: add comment here to explain - in speed
  86   }
  87
  88 }