Add Encoders to drivetrain and related methods for accessing encoder values.

diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
index 54b0ce01131b2dd8a376e98447b30703c5bc7317..138e176956083416f27180650ee984d87ac1cdd4 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
+++ b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
@@ -1,21 +1,85 @@
 package org.usfirst.frc.team3501.robot.subsystems;
 
 import org.usfirst.frc.team3501.robot.Constants;
+
 import edu.wpi.first.wpilibj.CANTalon;
+import edu.wpi.first.wpilibj.CounterBase.EncodingType;
+import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj.command.Subsystem;
 
 public class DriveTrain extends Subsystem {
-  private CANTalon frontLeft, frontRight, rearLeft, rearRight;
-
-  public DriveTrain() {
-    frontLeft = new CANTalon(Constants.DriveTrain.FRONT_LEFT);
-    frontRight = new CANTalon(Constants.DriveTrain.FRONT_RIGHT);
-    rearLeft = new CANTalon(Constants.DriveTrain.REAR_LEFT);
-    rearRight = new CANTalon(Constants.DriveTrain.REAR_RIGHT);
-  }
-
-  @Override
-  protected void initDefaultCommand() {
-  }
+       private CANTalon frontLeft, frontRight, rearLeft, rearRight;
+       // operational constants
+
+       // inches/pulse
+       private final static double WHEEL_DIAMETER = 6.0; // in inches
+       private final static double PULSES_PER_ROTATION = 256;
+       private final static double OUTPUT_SPROCKET_DIAMETER = 2.0;
+       private final static double WHEEL_SPROCKET_DIAMETER = 3.5;
+
+       public final static double INCHES_PER_PULSE = (((Math.PI)
+                       * OUTPUT_SPROCKET_DIAMETER / PULSES_PER_ROTATION) / WHEEL_SPROCKET_DIAMETER)
+                       * WHEEL_DIAMETER;
+       private Encoder leftEncoder, rightEncoder;
+
+       public DriveTrain() {
+               frontLeft = new CANTalon(Constants.DriveTrain.FRONT_LEFT);
+               frontRight = new CANTalon(Constants.DriveTrain.FRONT_RIGHT);
+               rearLeft = new CANTalon(Constants.DriveTrain.REAR_LEFT);
+               rearRight = new CANTalon(Constants.DriveTrain.REAR_RIGHT);
+               leftEncoder = new Encoder(Constants.DriveTrain.ENCODER_LEFT_A,
+                               Constants.DriveTrain.ENCODER_LEFT_B, false, EncodingType.k4X);
+               rightEncoder = new Encoder(Constants.DriveTrain.ENCODER_RIGHT_A,
+                               Constants.DriveTrain.ENCODER_RIGHT_B, false, EncodingType.k4X);
+               leftEncoder.setDistancePerPulse(INCHES_PER_PULSE);
+               rightEncoder.setDistancePerPulse(INCHES_PER_PULSE);
+       }
+
+       @Override
+       protected void initDefaultCommand() {
+       }
+
+       public void resetEncoders() {
+               leftEncoder.reset();
+               rightEncoder.reset();
+       }
+
+       // Returns inches per second
+       public double getRightSpeed() {
+               return rightEncoder.getRate();
+       }
+
+       public double getLeftSpeed() {
+               return leftEncoder.getRate();
+       }
+
+       public double getSpeed() {
+               return (getLeftSpeed() + getRightSpeed()) / 2.0;
+       }
+
+       // Returns distance in in
+       public double getRightDistance() {
+               return rightEncoder.getDistance();
+       }
+
+       // Returns distance in in
+       public double getLeftDistance() {
+               return leftEncoder.getDistance();
+       }
+
+       public double getDistance() {
+               return (getRightDistance() + getLeftDistance()) / 2.0;
+       }
+
+       public void stop() {
+               setMotorSpeeds(0, 0);
+       }
+
+       public void setMotorSpeeds(double leftSpeed, double rightSpeed) {
+               this.frontLeft.set(leftSpeed);
+               this.frontRight.set(-rightSpeed);
+               this.rearLeft.set(leftSpeed);
+               this.rearRight.set(-rightSpeed);
+       }
 
 }