From: Eric Sandoval <harpnart@gmail.com>
Date: Mon, 20 Feb 2017 00:06:19 +0000 (-0800)
Subject: update code
X-Git-Url: http://challenge-bot.com/repos/?p=3501%2F2017steamworks;a=commitdiff_plain;h=576421d88b199d9ff05b677bcbbaed0130d91e76

update code
---

diff --git a/src/org/usfirst/frc/team3501/robot/Robot.java b/src/org/usfirst/frc/team3501/robot/Robot.java
index 16b2884..b1ce483 100644
--- a/src/org/usfirst/frc/team3501/robot/Robot.java
+++ b/src/org/usfirst/frc/team3501/robot/Robot.java
@@ -22,10 +22,10 @@ public class Robot extends IterativeRobot {
     shooter = Shooter.getShooter();
     intake = Intake.getIntake();
 
-    SmartDashboard.putNumber(driveTrain.DRIVE_P_Val, 0);
-    SmartDashboard.putNumber(driveTrain.DRIVE_I_Val, 0);
-    SmartDashboard.putNumber(driveTrain.DRIVE_D_Val, 0);
-    SmartDashboard.putNumber(driveTrain.DRIVE_GYRO_P_Val, 0);
+    SmartDashboard.putNumber(driveTrain.DRIVE_P_Val, 0.006);
+    SmartDashboard.putNumber(driveTrain.DRIVE_I_Val, 0.0011);
+    SmartDashboard.putNumber(driveTrain.DRIVE_D_Val, -0.002);
+    SmartDashboard.putNumber(driveTrain.DRIVE_GYRO_P_Val, 0.01);
 
     SmartDashboard.putNumber(driveTrain.DRIVE_TARGET_DIST, 50);
     SmartDashboard.putNumber(driveTrain.MAX_TIME_OUT, 10);
@@ -56,6 +56,7 @@ public class Robot extends IterativeRobot {
   // both set to high gear
   @Override
   public void autonomousInit() {
+    System.out.println("AUTON INIT");
     driveTrain.setHighGear();
 
     double driveP = SmartDashboard.getNumber(driveTrain.DRIVE_P_Val,
@@ -90,7 +91,7 @@ public class Robot extends IterativeRobot {
   @Override
   public void autonomousPeriodic() {
     Scheduler.getInstance().run();
-
+    SmartDashboard.putNumber("angle", driveTrain.getAngle());
   }
 
   @Override
diff --git a/src/org/usfirst/frc/team3501/robot/commands/driving/DriveDistance.java b/src/org/usfirst/frc/team3501/robot/commands/driving/DriveDistance.java
index 8a76d0d..24d6235 100755
--- a/src/org/usfirst/frc/team3501/robot/commands/driving/DriveDistance.java
+++ b/src/org/usfirst/frc/team3501/robot/commands/driving/DriveDistance.java
@@ -42,10 +42,12 @@ public class DriveDistance extends Command {
   protected void initialize() {
     this.driveTrain.resetEncoders();
     this.driveController.setSetPoint(this.target);
+    zeroAngle = driveTrain.getAngle();
   }
 
   @Override
   protected void execute() {
+    System.out.printf("angle: %.2f\t", driveTrain.getAngle() - zeroAngle);
     double xVal = driveStraightGyroP * (driveTrain.getAngle() - zeroAngle);
     double yVal = driveController.calcPID(driveTrain.getAvgEncoderDistance());
 
@@ -53,11 +55,17 @@ public class DriveDistance extends Command {
     double rightDrive = yVal + xVal;
     this.driveTrain.setMotorValues(leftDrive, rightDrive);
 
-    System.out.println(
-        "PID Vals: " + driveController.getP() + " " + driveController.getI()
-            + " " + driveController.getD());
+    // SmartDashboard.putNumber("x", xVal);
+    // SmartDashboard.putNumber("y", yVal);
+    //
+    // SmartDashboard.putNumber("left", driveTrain.getLeftEncoderDistance());
+    // SmartDashboard.putNumber("right", driveTrain.getRightEncoderDistance());
+    //
+    // System.out.printf("x: %.2f\t", xVal);
+    // System.out.printf("y: %.2f\t", yVal);
+    //
+    // System.out.printf("dist: %.2f\n", driveTrain.getAvgEncoderDistance());
 
-    System.out.println("Encoder " + driveTrain.getAvgEncoderDistance());
   }
 
   @Override
diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
index 1196d16..221bb22 100644
--- a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
+++ b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
@@ -212,6 +212,7 @@ public class DriveTrain extends Subsystem {
    * Changes the gear to a DoubleSolenoid.Value
    */
   private void changeGear(DoubleSolenoid.Value gear) {
+    System.out.println("shifting to " + gear);
     leftGearPiston.set(gear);
     rightGearPiston.set(gear);
   }