First test of motors

[3501/stronghold-2016] / src / org / usfirst / frc / team3501 / robot / subsystems / DriveTrain.java

diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
index e19b46f02643a6c24af4d7d261ae9f13fb6c74a8..ac239361d2d44edc83358f8bb15dd54b1320cc56 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
+++ b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
@@ -44,8 +44,7 @@ public class DriveTrain extends PIDSubsystem {
 
     robotDrive = new RobotDrive(frontLeft, rearLeft, frontRight, rearRight);
 
-    lidar = new Lidar(I2C.Port.kOnboard);
-
+    lidar = new Lidar(I2C.Port.kMXP);
     leftEncoder = new Encoder(Constants.DriveTrain.ENCODER_LEFT_A,
         Constants.DriveTrain.ENCODER_LEFT_B, false, EncodingType.k4X);
     rightEncoder = new Encoder(Constants.DriveTrain.ENCODER_RIGHT_A,
@@ -63,9 +62,10 @@ public class DriveTrain extends PIDSubsystem {
     this.disable();
     gyro.start();
 
-    leftGearPiston = new DoubleSolenoid(Constants.DriveTrain.LEFT_FORWARD,
+    leftGearPiston = new DoubleSolenoid(10, Constants.DriveTrain.LEFT_FORWARD,
         Constants.DriveTrain.LEFT_REVERSE);
-    rightGearPiston = new DoubleSolenoid(Constants.DriveTrain.RIGHT_FORWARD,
+    rightGearPiston = new DoubleSolenoid(10,
+        Constants.DriveTrain.RIGHT_FORWARD,
         Constants.DriveTrain.RIGHT_REVERSE);
     Constants.DriveTrain.inverted = false;
   }
@@ -213,11 +213,12 @@ public class DriveTrain extends PIDSubsystem {
         output = Math.signum(output) * 0.3;
       left = output;
       right = output + drift * Constants.DriveTrain.kp / 10;
+      drive(left, right);
     } else if (DRIVE_MODE == Constants.DriveTrain.GYRO_MODE) {
       left = output;
       right = -output;
+      arcadeDrive(0, output);
     }
-    drive(left, right);
     pidOutput = output;
   }
 
@@ -227,16 +228,10 @@ public class DriveTrain extends PIDSubsystem {
   }
 
   /*
-   * Checks the drive mode <<<<<<< 9728080f491e9fb09795494349dba1297f447c0f
-   *
-   * @return the current state of the robot in each state Average distance from
-   * both sides of tank drive for Encoder Mode Angle from the gyro in GYRO_MODE
-   * =======
+   * Checks the drive mode
    *
    * @return the current state of the robot in each state Average distance from
    * both sides of tank drive for Encoder Mode Angle from the gyro in GYRO_MODE
-   * >>>>>>> Move all constants in DeadReckoning to Auton class because it makes
-   * more sense
    */
   private double sensorFeedback() {
     if (DRIVE_MODE == Constants.DriveTrain.ENCODER_MODE)
@@ -255,7 +250,7 @@ public class DriveTrain extends PIDSubsystem {
    * because RobotDrive tankdrive method drives inverted
    */
   public void drive(double left, double right) {
-    robotDrive.tankDrive(-left, -right);
+//    robotDrive.tankDrive(-left, -right);
     // dunno why but inverted drive (- values is forward)
     if (!Constants.DriveTrain.inverted)
       robotDrive.tankDrive(-left, -right);
@@ -263,6 +258,10 @@ public class DriveTrain extends PIDSubsystem {
       robotDrive.tankDrive(right, left);
   }
 
+  public void arcadeDrive(double y, double twist) {
+    robotDrive.arcadeDrive(y, twist);
+  }
+
   /*
    * constrains the distance to within -100 and 100 since we aren't going to
    * drive more than 100 inches
@@ -367,7 +366,4 @@ public class DriveTrain extends PIDSubsystem {
     rightGearPiston.set(gear);
   }
 
-  public void toggleTimeDeadReckoning() {
-    Constants.Auton.isUsingTime = !Constants.Auton.isUsingTime;
-  }
 }