more work on vision

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

@@ -13,6 +13,7 @@ import org.usfirst.frc.team2059.robot.commands.shooter.SetShooterState;
 import org.usfirst.frc.team2059.robot.commands.shooter.SetArmStopState;
 import org.usfirst.frc.team2059.robot.commands.shooter.ShootAtSpeed;
 import org.usfirst.frc.team2059.robot.commands.shooter.SpinRollers;
+import org.usfirst.frc.team2059.robot.commands.visionhelper.AlignHorizontal;
 import org.usfirst.frc.team2059.robot.RobotMap;
 /**
  * This class is the glue that binds the controls on the physical operator
@@ -34,7 +35,8 @@ public class OI {
     }
     // Print log when button 1 pressed
     //joystickButtons[0][0].whenPressed(new LogEncoder());
-    joystickButtons[0][0].whileHeld(new SetShooterState(true));
+//    joystickButtons[0][0].whileHeld(new SetShooterState(true));
+    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][6].whileHeld(new SpinRollers(1, true));

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

@@ -20,6 +20,11 @@ public class Robot extends IterativeRobot {
 //   cameraServer = CameraServer.getInstance();
 //    cameraServer.setQuality(50);
 //    cameraServer.startAutomaticCapture("cam0");
+    try{
+      Process grip = Runtime.getRuntime().exec(new String[]{"/usr/local/frc/JRE/bin/java", "-jar", "grip.jar", "project.grip"});
+    }catch(Exception e){
+      System.out.println("Error starting GRIP");
+    }
     chooser.addDefault("Nothing", new RoutineNothing());
     chooser.addObject("Time based low bar", new RoutineDriveTime());
     chooser.addObject("Time based straight low bar", new RoutineDriveStraightTime());
@@ -67,7 +72,10 @@ 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("centerX",CommandBase.visionHelper.getCenterX());
+    SmartDashboard.putNumber("horizontalError",CommandBase.visionHelper.getHorizontalError());
+    SmartDashboard.putNumber("verticalError",CommandBase.visionHelper.getVerticalError());
+    SmartDashboard.putNumber("horizontalErrorCorrected",CommandBase.visionHelper.getHorizontalError()+5);
+    SmartDashboard.putNumber("goalDistance",72/Math.tan(0.0175*(35+CommandBase.visionHelper.getVerticalError())));
     if (Robot.oi.getJoysticks()[1].getRawButton(3)) {
       CommandBase.pneumatics.setArmStopState(true);
     } else {

src/org/usfirst/frc/team2059/robot/commands/autonomous/RoutineShoot.java

@@ -4,17 +4,17 @@ 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 AutoSetArmStopState(true));
-    addSequential(new AutoResetLower(-1));
+//    addSequential(new AutoResetLower(-1));
-    addSequential(new AutoDriveStraightTime(0.5, 1));
+//    addSequential(new AutoDriveStraightTime(0.5, 1));
-    addParallel(new AutoSetArmPosition(90));
+//    addParallel(new AutoSetArmPosition(90));
-    addSequential(new AutoRotate(45));
+//    addSequential(new AutoRotate(45));
-    addSequential(new AutoDriveStraightTime(0.4, 1.5));
+//    addSequential(new AutoDriveStraightTime(0.4, 1.5));
-    addSequential(new AutoSpinRollers(1, 2));
+//    addSequential(new AutoSpinRollers(1, 2));
-    addSequential(new AutoSetShooterState(true));
+//    addSequential(new AutoSetShooterState(true));
-    addSequential(new AutoSetArmStopState(false));
+//    addSequential(new AutoSetArmStopState(false));
-    addParallel(new AutoSetArmPosition(10));
+//    addParallel(new AutoSetArmPosition(10));
-    addSequential(new AutoRotate(180));
+//    addSequential(new AutoRotate(180));
-    addSequential(new AutoSetShooterState(false));
+//    addSequential(new AutoSetShooterState(false));
-  }
+    addSequential(new VisionAlignHorizontal()); }
 }

src/org/usfirst/frc/team2059/robot/commands/autonomous/VisionAlignHorizontal.java

@@ -0,0 +1,24 @@
+package org.usfirst.frc.team2059.robot.commands.autonomous;
+import org.usfirst.frc.team2059.robot.commands.CommandBase;
+public class VisionAlignHorizontal extends CommandBase {
+  double error;
+  public VisionAlignHorizontal() {
+  }
+  protected void initialize() {
+    error = visionHelper.getHorizontalError();
+    driveBase.resetGyro();
+  }
+  protected boolean isFinished() {
+    return false;
+  }
+  protected void execute() {
+    driveBase.rotateAngle(error);
+  }
+  protected void end() {
+    driveBase.getGyroController().disable();
+  }
+  protected void interrupted() {
+    end();
+  }
+}
+// vim: sw=2:ts=2:sts=2

src/org/usfirst/frc/team2059/robot/commands/vision/AlignHorizontal.java

@@ -0,0 +1,24 @@
+package org.usfirst.frc.team2059.robot.commands.visionhelper;
+import org.usfirst.frc.team2059.robot.commands.CommandBase;
+public class AlignHorizontal extends CommandBase {
+  double error;
+  public AlignHorizontal() {
+  }
+  protected void initialize() {
+    error = visionHelper.getHorizontalError();
+    driveBase.resetGyro();
+  }
+  protected boolean isFinished() {
+    return false;
+  }
+  protected void execute() {
+    driveBase.rotateAngle(error);
+  }
+  protected void end() {
+    driveBase.getGyroController().disable();
+  }
+  protected void interrupted() {
+    end();
+  }
+}
+// vim: sw=2:ts=2:sts=2

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);

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

@@ -6,7 +6,11 @@ import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj.networktables.NetworkTable;
 public class VisionHelper {
-     NetworkTable contoursTable;
+   NetworkTable contoursTable;
+   double hfieldOfView=67;
+   double vfieldOfView=51;
+   double imageWidth=640;
+   double imageHeight=480; 
    public VisionHelper(){
      contoursTable=NetworkTable.getTable("GRIP/contours");
    }
@@ -16,6 +20,22 @@ public class VisionHelper {
       }catch(Exception e){
          return 0;
       }
-
+   }
+   public double getCenterY(){
+      try{
+         return contoursTable.getNumberArray("centerY",new double[0])[0];
+      }catch(Exception e){
+         return 0;
+      }
+   }
+   public double getHorizontalError(){
+      double degreesPerPixel=hfieldOfView/imageWidth;
+      double centerColumn=imageWidth/2;
+      return (getCenterX()-centerColumn)*degreesPerPixel;
+   }
+   public double getVerticalError(){
+      double degreesPerPixel=vfieldOfView/imageHeight;
+      double centerRow=imageHeight/2;
+      return (getCenterY()-centerRow)*degreesPerPixel;
    }
 }