gave up on vision based distance

src/org/usfirst/frc/team2059/robot/OI.java

@@ -6,6 +6,7 @@ import edu.wpi.first.wpilibj.buttons.JoystickButton;
 import org.usfirst.frc.team2059.robot.commands.drivetrain.LogEncoder;
 import org.usfirst.frc.team2059.robot.commands.drivetrain.PIDDrive;
 import org.usfirst.frc.team2059.robot.commands.drivetrain.Drive;
+import org.usfirst.frc.team2059.robot.commands.drivetrain.DriveStraight;
 import org.usfirst.frc.team2059.robot.commands.shooter.MoveArm;
 import org.usfirst.frc.team2059.robot.commands.shooter.ResetLower;
 import org.usfirst.frc.team2059.robot.commands.shooter.ResetUpper;
@@ -40,6 +41,8 @@ public class OI {
     joystickButtons[0][0].whileHeld(new AlignHorizontal());
     joystickButtons[0][2].whileHeld(new SpinRollers(-0.5, false));
     joystickButtons[0][3].whileHeld(new SpinRollers(1, false));
+    joystickButtons[0][4].whileHeld(new DriveStraight(0.25));
+    joystickButtons[0][5].whileHeld(new DriveStraight(-0.25));
     joystickButtons[0][6].whileHeld(new SpinRollers(1, true));
     joystickButtons[0][9].whileHeld(new Drive());
 //    joystickButtons[0][2].whileHeld(new PIDDrive(400));

src/org/usfirst/frc/team2059/robot/Robot.java

@@ -21,7 +21,7 @@ public class Robot extends IterativeRobot {
 //    cameraServer.setQuality(50);
 //    cameraServer.startAutomaticCapture("cam0");
     try {
-      new ProcessBuilder("/home/lvuser/grip").inheritIO().start();
+//      new ProcessBuilder("/home/lvuser/grip").inheritIO().start();
     } catch (Exception e) {
       System.out.println("Error starting GRIP");
     }
@@ -72,14 +72,17 @@ public class Robot extends IterativeRobot {
   }
   public void teleopPeriodic() {
     Scheduler.getInstance().run();
+    SmartDashboard.putNumber("GyroAngle", CommandBase.driveBase.getGyro().getAngle());
     SmartDashboard.putNumber("ArmAngleRaw", CommandBase.mainArm.getRaw());
     SmartDashboard.putNumber("ArmAngleDegrees", CommandBase.mainArm.getDegrees());
     SmartDashboard.putNumber("tmpRotations", CommandBase.driveBase.getLeftRotations());
     SmartDashboard.putNumber("centerX", CommandBase.visionHelper.getCenterX());
+//    SmartDashboard.putNumber("centerY", CommandBase.visionHelper.getCenterY());
+    SmartDashboard.putNumber("centerContoursX", CommandBase.visionHelper.getCenterContourX());
     SmartDashboard.putNumber("horizontalError", CommandBase.visionHelper.getHorizontalError());
     SmartDashboard.putNumber("verticalError", CommandBase.visionHelper.getVerticalError());
     SmartDashboard.putNumber("horizontalErrorCorrected", CommandBase.visionHelper.getHorizontalError() + 5);
-    SmartDashboard.putNumber("goalDistance", 55 / Math.tan((Math.PI/180) * (64 - CommandBase.visionHelper.getVerticalError())));
+    SmartDashboard.putNumber("goalDistance", 54 / Math.tan((Math.PI/180) * (60 - CommandBase.visionHelper.getVerticalError())));
     if (Robot.oi.getJoysticks()[1].getRawButton(3)) {
       CommandBase.pneumatics.setArmStopState(true);
     } else {

src/org/usfirst/frc/team2059/robot/commands/drivetrain/DriveStraight.java

@@ -0,0 +1,35 @@
+package org.usfirst.frc.team2059.robot.commands.drivetrain;
+import org.usfirst.frc.team2059.robot.commands.CommandBase;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import org.usfirst.frc.team2059.robot.Robot;
+/**
+ *
+ */
+public class DriveStraight extends CommandBase {
+  double speed;
+  public DriveStraight(double speed) {
+    requires(driveBase);
+    this.speed = speed;
+  }
+  // Called just before this Command runs the first time
+  protected void initialize() {
+  }
+  // Called repeatedly when this Command is scheduled to run
+  protected void execute() {
+    driveBase.driveStraight(speed, SmartDashboard.getNumber("GyroCorrection"));
+  }
+  // Make this return true when this Command no longer needs to run execute()
+  protected boolean isFinished() {
+    return false;
+  }
+  // Called once after isFinished returns true
+  protected void end() {
+    driveBase.driveStraight(0, SmartDashboard.getNumber("GyroCorrection"));
+  }
+  // Called when another command which requires one or more of the same
+  // subsystems is scheduled to run
+  protected void interrupted() {
+    end();
+  }
+}
+// vim: sw=2:ts=2:sts=2

src/org/usfirst/frc/team2059/robot/subsystems/VisionHelper.java

@@ -14,6 +14,13 @@ public class VisionHelper {
     linesTable = NetworkTable.getTable("GRIP/lines");
     contoursTable = NetworkTable.getTable("GRIP/contours");
   }
+  public double getCenterContourX(){
+     try{
+        return contoursTable.getNumberArray("centerX",new double[0])[0];
+     }catch(Exception e){
+        return 0;
+     }
+  }
   public double getCenterX() {
     int highestLengthIndex = 0;
     int index = 0;
@@ -53,7 +60,7 @@ public class VisionHelper {
     }
   }
   public double getHorizontalError() {
-    return (180/Math.PI) * (Math.atan((getCenterX()-20 - (RobotMap.imageWidth / 2)) / RobotMap.fWidth));
+    return (180/Math.PI) * (Math.atan((getCenterContourX()+25 - (RobotMap.imageWidth / 2)) / RobotMap.fWidth));
   }
   public double getVerticalError() {
     return (180/Math.PI) * (Math.atan((getCenterY() - (RobotMap.imageHeight / 2)) / RobotMap.fHeight));