Work on encoders on drivetrain

#1
Team3309 · Feb 18, 2013 · a2ca2d8 · a2ca2d8
1 parent 1d8c36f
commit a2ca2d8
Show file tree

Hide file tree

Showing 3 changed files with 45 additions and 6 deletions.
diff --git a/src/org/team3309/frc2013/Drive.java b/src/org/team3309/frc2013/Drive.java
@@ -4,9 +4,13 @@
  */
 package org.team3309.frc2013;
 
+import edu.wpi.first.wpilibj.Counter;
+import edu.wpi.first.wpilibj.CounterBase;
 import edu.wpi.first.wpilibj.DoubleSolenoid;
-import edu.wpi.first.wpilibj.RobotDrive;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.Victor;
+import java.util.TimerTask;
 
 /**
  *
@@ -52,25 +56,43 @@ public Builder ptoShifter(int forward, int reverse) {
             return this;
         }
 
+        public Builder leftEncoder(int a, int b) {
+            drive.leftEncoder = new Encoder(a, b, false, Encoder.EncodingType.k4X);
+            drive.leftEncoder.setDistancePerPulse(1/360);
+            return this;
+        }
+
+        public Builder rightEncoder(int a, int b) {
+            drive.rightEncoder = new Encoder(a, b, false, Encoder.EncodingType.k4X);
+            drive.rightEncoder.setDistancePerPulse(1);
+            return this;
+        }
+
         public Drive build() {
+            drive.leftEncoder.start();
+            drive.rightEncoder.start();
             return drive;
         }
     }
     public static final int MODE_PERC = 0;
     public static final int MODE_RPM = 1;
     public static final int MODE_POS = 2;
 
-    private int mode = 0;
+    public static final int MAX_RPM = 300;
 
+    private int mode = 0;
     private Victor left1 = null;
     private Victor left2 = null;
     private Victor right1 = null;
     private Victor right2 = null;
-
     private DoubleSolenoid driveShifter = null;
     private DoubleSolenoid ptoShifter = null;
-
+    private Encoder leftEncoder = null;
+    private Encoder rightEncoder = null;
+    private Encoder ptoEncoder = null;
     private double gain = .5;
+    private boolean leftEncoderReversed = false;
+    private boolean rightEncoderReversed = false;
 
     double skim(double v) {
         // gain determines how much to skim off the top
@@ -81,18 +103,28 @@ public Drive build() {
         }
         return 0;
     }
+    private long lastPrinted = 0;
 
     public void drive(double throttle, double turn) {
         turn = -turn;
-        
+
         double t_left = throttle + turn;
         double t_right = throttle - turn;
 
         double left = t_left + skim(t_right);
         double right = t_right + skim(t_left);
 
         left = -left;
-        System.out.println("left: "+left+"\t right:"+right);
+        if (left < 0) {
+            leftEncoder.setReverseDirection(true);
+        } else if (left > 0) {
+            leftEncoder.setReverseDirection(false);
+        }
+        //if (System.currentTimeMillis() - lastPrinted > 500) {
+            System.out.println("right encoder: " + rightEncoder.getRate());
+            //lastPrinted = System.currentTimeMillis();
+        //}
+
 
         left1.set(left);
         left2.set(left);
@@ -119,4 +151,5 @@ public void engagePto() {
     public void disengagePto() {
         ptoShifter.set(DoubleSolenoid.Value.kReverse);
     }
+
 }
diff --git a/src/org/team3309/frc2013/Robot.java b/src/org/team3309/frc2013/Robot.java
@@ -34,6 +34,8 @@ public void robotInit() {
                 .right1(RobotMap.DRIVE_RIGHT_1).right2(RobotMap.DRIVE_RIGHT_2)
                 .driveShifter(RobotMap.DRIVE_SHIFTER_LEFT_FORWARD, RobotMap.DRIVE_SHIFTER_LEFT_REVERSE)
                 .ptoShifter(RobotMap.DRIVE_SHIFTER_PTO_FORWARD, RobotMap.DRIVE_SHIFTER_PTO_REVERSE)
+                .leftEncoder(RobotMap.DRIVE_ENCODER_LEFT_A, RobotMap.DRIVE_ENCODER_LEFT_B)
+                .rightEncoder(RobotMap.DRIVE_ENCODER_RIGHT_A, RobotMap.DRIVE_ENCODER_RIGHT_B)
                 .build();
 
         mShooter = new Shooter(RobotMap.SHOOTER_MOTOR, RobotMap.SHOOTER_PISTON_FORWARD, RobotMap.SHOOTER_PISTON_REVERSE, 0);

diff --git a/src/org/team3309/frc2013/RobotMap.java b/src/org/team3309/frc2013/RobotMap.java
@@ -17,6 +17,10 @@ public class RobotMap {
     public static final int DRIVE_LEFT_2    = 2;
     public static final int DRIVE_RIGHT_1   = 3;
     public static final int DRIVE_RIGHT_2   = 4;
+    public static final int DRIVE_ENCODER_LEFT_A = 1;
+    public static final int DRIVE_ENCODER_LEFT_B = 2;
+    public static final int DRIVE_ENCODER_RIGHT_A = 3;
+    public static final int DRIVE_ENCODER_RIGHT_B = 4;
 
     public static final int DRIVE_SHIFTER_LEFT_FORWARD  = 1;
     public static final int DRIVE_SHIFTER_LEFT_REVERSE  = 2;