From: Your Name <you@example.com>
Date: Thu, 21 Jan 2016 03:48:32 +0000 (-0800)
Subject: Add Encoders to drivetrain and related methods for accessing encoder values.
X-Git-Url: http://challenge-bot.com/repos/?a=commitdiff_plain;h=111dc44471c81706296566c140900d990d20e240;hp=15a56fa4e4f00674d27bec4a9bcb1411376952c0;p=3501%2Fstronghold-2016

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 54b0ce01..138e1769 100644
--- 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);
+	}
 
 }