more vision changes and things that probably wont work

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");
     }
@@ -77,11 +77,12 @@ public class Robot extends IterativeRobot {
     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("sonarDistance", CommandBase.mainArm.getSonarDistance());
+    SmartDashboard.putNumber("sonarDistanceRaw", CommandBase.mainArm.getSonarDistanceRaw());
+    SmartDashboard.putNumber("shooterAngleError", CommandBase.mainArm.getShooterAngleError());
     SmartDashboard.putNumber("goalDistance", 54 / Math.tan((Math.PI/180) * (60 - CommandBase.visionHelper.getVerticalError())));
     if (Robot.oi.getJoysticks()[1].getRawButton(3)) {
       CommandBase.pneumatics.setArmStopState(true);

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

@@ -36,6 +36,8 @@ public class RobotMap {
   public static double imageHeight = 480;
   public static double fWidth = 483.467261958;
   public static double fHeight = 362.600446468;
+  public static int sonar = 2;
+  public static int goalHeight = 72;
   //Misc
   public static int mainArmPresetCollect = -5;
   public static int mainArmPresetTraverse = 10;

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

@@ -16,6 +16,7 @@ public class DriveStraight extends CommandBase {
   }
   // Called repeatedly when this Command is scheduled to run
   protected void execute() {
+    driveBase.resetGyro();
     driveBase.driveStraight(speed, SmartDashboard.getNumber("GyroCorrection"));
   }
   // Make this return true when this Command no longer needs to run execute()

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

@@ -10,6 +10,7 @@ public class MainArm extends PIDSubsystem {
   CANTalon armMotorLeft = new CANTalon(RobotMap.armLeftMotor);
   CANTalon armMotorRight = new CANTalon(RobotMap.armRightMotor);
   AnalogInput pot = new AnalogInput(RobotMap.armPot);
+  AnalogInput sonar = new AnalogInput(RobotMap.sonar);
   DigitalInput limitSwitchBottom = new DigitalInput(RobotMap.armBottomLim);
   DigitalInput limitSwitchTop = new DigitalInput(RobotMap.armTopLim);
   private double min = RobotMap.zeroDegrees;
@@ -88,5 +89,15 @@ public class MainArm extends PIDSubsystem {
  //   }
     return false;
   }
+  public double getShooterAngleError(){
+    double correctedHeight = RobotMap.goalHeight - 44; //44 is the distance between the ground and the sonar
+    return (180/Math.PI) * Math.atan(correctedHeight/getSonarDistance());
+  }
+  public double getSonarDistanceRaw(){
+     return sonar.getVoltage();
+  }
+  public double getSonarDistance(){
+     return (getSonarDistanceRaw()/0.000976562f)/25.4f;
+  }
 }
 // vim: sw=2:ts=2:sts=2

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

@@ -21,6 +21,26 @@ public class VisionHelper {
         return 0;
      }
   }
+  public double getSlope(){
+    int highestLengthIndex = 0;
+    int index = 0;
+    double lengths[] = linesTable.getNumberArray("length", new double[0]);
+    try{
+    double x1s[] = linesTable.getNumberArray("x1", new double[0]);
+    double y1s[] = linesTable.getNumberArray("y1", new double[0]);
+    double x2s[] = linesTable.getNumberArray("x2", new double[0]);
+    double y2s[] = linesTable.getNumberArray("y2", new double[0]);
+    for(double length : lengths){
+       if(length >= lengths[highestLengthIndex]){
+          highestLengthIndex=index;
+       }
+       index++;
+    }
+    return ((y1s[highestLengthIndex]-y2s[highestLengthIndex])/(x1s[highestLengthIndex]-x2s[highestLengthIndex]));
+    }catch(Exception e){
+       return 0;
+    }
+  }
   public double getCenterX() {
     int highestLengthIndex = 0;
     int index = 0;
@@ -60,7 +80,7 @@ public class VisionHelper {
     }
   }
   public double getHorizontalError() {
-    return (180/Math.PI) * (Math.atan((getCenterContourX()+25 - (RobotMap.imageWidth / 2)) / RobotMap.fWidth));
+    return (180/Math.PI) * (Math.atan((getCenterContourX() - (RobotMap.imageWidth / 2)) / RobotMap.fWidth));
   }
   public double getVerticalError() {
     return (180/Math.PI) * (Math.atan((getCenterY() - (RobotMap.imageHeight / 2)) / RobotMap.fHeight));