add getter/setter methods and constants for encoder

diff --git a/src/org/usfirst/frc/team3501/robot/Constants.java b/src/org/usfirst/frc/team3501/robot/Constants.java
index 021183a8d7339b382adccc0eae4e480c0202933b..163ba286e42d6da44e13883bb747b0444a692a19 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/Constants.java
+++ b/src/org/usfirst/frc/team3501/robot/Constants.java
@@ -7,19 +7,28 @@ package org.usfirst.frc.team3501.robot;
  */
 
 public class Constants {
-  public static class OI {
-    public final static int LEFT_STICK_PORT = 0;
-    public final static int RIGHT_STICK_PORT = 0;
-  }
+    public static class OI {
+        public final static int LEFT_STICK_PORT = 0;
+        public final static int RIGHT_STICK_PORT = 0;
+    }
 
-  public static class DriveTrain {
-    public static final int FRONT_LEFT = 0;
-    public static final int FRONT_RIGHT = 0;
-    public static final int REAR_LEFT = 0;
-    public static final int REAR_RIGHT = 0;
-  }
+    public static class DriveTrain {
+        // MOTOR CONTROLLERS
+        public static final int FRONT_LEFT = 0;
+        public static final int FRONT_RIGHT = 0;
+        public static final int REAR_LEFT = 0;
+        public static final int REAR_RIGHT = 0;
 
-  public static enum Direction {
-    LEFT, RIGHT, DOWN, UP, FORWARD, BACKWARD;
-  }
+        // ENCODERS
+        public static final int ENCODER_LEFT_A = 0;
+        public static final int ENCODER_LEFT_B = 0;
+        public static final int ENCODER_RIGHT_A = 0;
+        public static final int ENCODER_RIGHT_B = 0;
+
+        public static final int INCHES_PER_PULSE = 0;
+    }
+
+    public static enum Direction {
+        LEFT, RIGHT, DOWN, UP, FORWARD, BACKWARD;
+    }
 }

diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
index af22b6812d5d20125a1959d47b64365b6db490a7..426063628b9b5cd45a0e59f656f474aaa0c902c1 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
+++ b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
@@ -4,6 +4,7 @@ import org.usfirst.frc.team3501.robot.Constants;
 
 import com.ctre.CANTalon;
 
+import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj.RobotDrive;
 import edu.wpi.first.wpilibj.command.Subsystem;
 
@@ -11,13 +12,25 @@ public class DriveTrain extends Subsystem {
     private static DriveTrain driveTrain;
     private CANTalon frontLeft, frontRight, rearLeft, rearRight;
     private RobotDrive robotDrive;
+    private Encoder leftEncoder, rightEncoder;
 
     private DriveTrain() {
+        // MOTOR CONTROLLERS
         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);
 
+        // ENCODERS
+        leftEncoder = new Encoder(Constants.DriveTrain.ENCODER_LEFT_A,
+                Constants.DriveTrain.ENCODER_LEFT_B);
+        rightEncoder = new Encoder(Constants.DriveTrain.ENCODER_RIGHT_A,
+                Constants.DriveTrain.ENCODER_RIGHT_B);
+
+        leftEncoder.setDistancePerPulse(Constants.DriveTrain.INCHES_PER_PULSE);
+        rightEncoder.setDistancePerPulse(Constants.DriveTrain.INCHES_PER_PULSE);
+
+        // ROBOT DRIVE
         robotDrive = new RobotDrive(frontLeft, rearLeft, frontRight, rearRight);
     }
 
@@ -31,6 +44,37 @@ public class DriveTrain extends Subsystem {
         robotDrive.tankDrive(left, right);
     }
 
+    // ENCODER METHODS
+
+    public double getLeftEncoder() {
+        return leftEncoder.getDistance();
+    }
+
+    public double getRightEncoder() {
+        return rightEncoder.getDistance();
+    }
+
+    public double getAvgEncoderDistance() {
+        return (leftEncoder.getDistance() + rightEncoder.getDistance()) / 2;
+    }
+
+    public void resetEncoders() {
+        leftEncoder.reset();
+        rightEncoder.reset();
+    }
+
+    public double getLeftSpeed() {
+        return leftEncoder.getRate();
+    }
+
+    public double getRightSpeed() {
+        return rightEncoder.getRate();
+    }
+
+    public double getSpeed() {
+        return (getLeftSpeed() + getRightSpeed()) / 2.0;
+    }
+
     @Override
     protected void initDefaultCommand() {
     }