From c8e2c9697e40d6c020d0a7be10dd56c364d4c6ec Mon Sep 17 00:00:00 2001
From: Adam Long <macadam.long@gmail.com>
Date: Mon, 3 Oct 2016 00:23:40 +0000
Subject: [PATCH] attempt to work on vision more

---
 src/org/usfirst/frc/team2059/robot/Robot.java  |  3 +++
 .../commands/autonomous/RoutineShoot.java      | 18 ++++++------------
 .../autonomous/VisionAlignHorizontal.java      |  7 ++++++-
 .../team2059/robot/subsystems/DriveBase.java   |  2 +-
 .../robot/subsystems/VisionHelper.java         | 10 ++++++++--
 5 files changed, 24 insertions(+), 16 deletions(-)

diff --git a/src/org/usfirst/frc/team2059/robot/Robot.java b/src/org/usfirst/frc/team2059/robot/Robot.java
index b81fe82..8e8d72a 100644
--- a/src/org/usfirst/frc/team2059/robot/Robot.java
+++ b/src/org/usfirst/frc/team2059/robot/Robot.java
@@ -59,6 +59,9 @@ public class Robot extends IterativeRobot {
     SmartDashboard.putNumber("ArmAngleRaw", CommandBase.mainArm.getRaw());
     SmartDashboard.putNumber("ArmAngleDegrees", CommandBase.mainArm.getDegrees());
     SmartDashboard.putNumber("tmpRotations", CommandBase.driveBase.getLeftRotations());
+    SmartDashboard.putNumber("horizontalError",CommandBase.visionHelper.getHorizontalError());
+    SmartDashboard.putNumber("verticalError",CommandBase.visionHelper.getVerticalError());
+    SmartDashboard.putNumber("horizontalErrorCorrected",CommandBase.visionHelper.getHorizontalError()+5);
   }
   public void teleopInit() {
     if (autonomousCommand != null) {
diff --git a/src/org/usfirst/frc/team2059/robot/commands/autonomous/RoutineShoot.java b/src/org/usfirst/frc/team2059/robot/commands/autonomous/RoutineShoot.java
index 69ca31e..7101ace 100644
--- a/src/org/usfirst/frc/team2059/robot/commands/autonomous/RoutineShoot.java
+++ b/src/org/usfirst/frc/team2059/robot/commands/autonomous/RoutineShoot.java
@@ -4,17 +4,11 @@ import edu.wpi.first.wpilibj.command.CommandGroup;
 import org.usfirst.frc.team2059.robot.Robot;
 public class RoutineShoot extends CommandGroup {
   public RoutineShoot() {
-//    addSequential(new AutoSetArmStopState(true));
-//    addSequential(new AutoResetLower(-1));
-//    addSequential(new AutoDriveStraightTime(0.5, 1));
-//    addParallel(new AutoSetArmPosition(90));
-//    addSequential(new AutoRotate(45));
-//    addSequential(new AutoDriveStraightTime(0.4, 1.5));
-//    addSequential(new AutoSpinRollers(1, 2));
-//    addSequential(new AutoSetShooterState(true));
-//    addSequential(new AutoSetArmStopState(false));
-//    addParallel(new AutoSetArmPosition(10));
-//    addSequential(new AutoRotate(180));
-//    addSequential(new AutoSetShooterState(false));
+    addSequential(new AutoSetArmStopState(true));
+    addSequential(new AutoResetLower(-1));
+    addSequential(new AutoDriveStraightTime(0.5, 1));
+    addSequential(new AutoSetArmPosition(90));
+    //addSequential(new AutoRotate(45));
+    //addSequential(new AutoDriveStraightTime(0.4, 1.5));
     addSequential(new VisionAlignHorizontal()); }
 }
diff --git a/src/org/usfirst/frc/team2059/robot/commands/autonomous/VisionAlignHorizontal.java b/src/org/usfirst/frc/team2059/robot/commands/autonomous/VisionAlignHorizontal.java
index 8643bb5..2661667 100644
--- a/src/org/usfirst/frc/team2059/robot/commands/autonomous/VisionAlignHorizontal.java
+++ b/src/org/usfirst/frc/team2059/robot/commands/autonomous/VisionAlignHorizontal.java
@@ -12,7 +12,12 @@ public class VisionAlignHorizontal extends CommandBase {
     return false;
   }
   protected void execute() {
-    driveBase.rotateAngle(error);
+    if(visionHelper.getHorizontalError()==420){
+      driveBase.resetGyro();
+      driveBase.rotateAngle(5);
+    }else{
+//      driveBase.rotateAngle(visionHelper.getHorizontalError());
+    }
   }
   protected void end() {
     driveBase.getGyroController().disable();
diff --git a/src/org/usfirst/frc/team2059/robot/subsystems/DriveBase.java b/src/org/usfirst/frc/team2059/robot/subsystems/DriveBase.java
index 030bc14..2fef2b5 100644
--- a/src/org/usfirst/frc/team2059/robot/subsystems/DriveBase.java
+++ b/src/org/usfirst/frc/team2059/robot/subsystems/DriveBase.java
@@ -21,7 +21,7 @@ public class DriveBase extends Subsystem {
   PIDController leftEncoderController = new PIDController(0.02, 0.002, 0.017, leftEncoder, new MotorsPIDOutput());
   double encoderGyroCorrection = 1;
   public void initDefaultCommand() {
-    //setDefaultCommand(new Drive());
+    setDefaultCommand(new Drive());
   }
   public void stopDriving() {
     leftMotorOne.set(0);
diff --git a/src/org/usfirst/frc/team2059/robot/subsystems/VisionHelper.java b/src/org/usfirst/frc/team2059/robot/subsystems/VisionHelper.java
index 0417a14..6c4c9fb 100644
--- a/src/org/usfirst/frc/team2059/robot/subsystems/VisionHelper.java
+++ b/src/org/usfirst/frc/team2059/robot/subsystems/VisionHelper.java
@@ -18,22 +18,28 @@ public class VisionHelper {
       try{
          return contoursTable.getNumberArray("centerX",new double[0])[0];
       }catch(Exception e){
-         return 0;
+         return 420;
       }
    }
    public double getCenterY(){
       try{
          return contoursTable.getNumberArray("centerY",new double[0])[0];
       }catch(Exception e){
-         return 0;
+         return 420;
       }
    }
    public double getHorizontalError(){
+      if(getCenterX()==420){
+         return 420;
+      }
       double degreesPerPixel=hfieldOfView/imageWidth;
       double centerColumn=imageWidth/2;
       return (getCenterX()-centerColumn)*degreesPerPixel;
    }
    public double getVerticalError(){
+      if(getCenterY()==420){
+         return 420;
+      }
       double degreesPerPixel=vfieldOfView/imageHeight;
       double centerRow=imageHeight/2;
       return (getCenterY()-centerRow)*degreesPerPixel;