vision almost works

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

@@ -15,6 +15,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.shooter.AlignVertical;
 import org.usfirst.frc.team2059.robot.commands.visionhelper.AlignHorizontal;
 import org.usfirst.frc.team2059.robot.RobotMap;
 /**
@@ -39,6 +40,7 @@ public class OI {
     //joystickButtons[0][0].whenPressed(new LogEncoder());
 //    joystickButtons[0][0].whileHeld(new SetShooterState(true));
     joystickButtons[0][0].whileHeld(new AlignHorizontal());
+    joystickButtons[0][1].whileHeld(new SetShooterState(true));
     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));
@@ -46,13 +48,14 @@ public class OI {
     joystickButtons[0][6].whileHeld(new SpinRollers(1, true));
     joystickButtons[0][9].whileHeld(new Drive());
 //    joystickButtons[0][2].whileHeld(new PIDDrive(400));
-    joystickButtons[1][0].whileHeld(new MoveArm(1));
-    joystickButtons[1][1].whileHeld(new MoveArm(-1));
     //joystickButtons[1][2].whileHeld(new SetArmPosition(RobotMap.mainArmPresetCollect));
+    joystickButtons[1][0].whileHeld(new AlignVertical());
     joystickButtons[1][2].whileHeld(new ResetLower(-1));
     joystickButtons[1][3].whileHeld(new SetArmPosition(RobotMap.mainArmPresetTraverse));
     joystickButtons[1][4].whileHeld(new SetArmPosition(RobotMap.mainArmPresetCloseShot));
     joystickButtons[1][5].whileHeld(new SetArmPosition(RobotMap.mainArmPresetLowShot));
+    joystickButtons[1][7].whileHeld(new MoveArm(1));
+    joystickButtons[1][8].whileHeld(new MoveArm(-1));
   }
   public Joystick[] getJoysticks() {
     return joysticks;

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

@@ -83,6 +83,7 @@ public class Robot extends IterativeRobot {
     SmartDashboard.putNumber("sonarDistance", CommandBase.mainArm.getSonarDistance());
     SmartDashboard.putNumber("sonarDistanceRaw", CommandBase.mainArm.getSonarDistanceRaw());
     SmartDashboard.putNumber("shooterAngleError", CommandBase.mainArm.getShooterAngleError());
+    SmartDashboard.putNumber("shooterAngleErrorFixed", CommandBase.mainArm.getShooterAngleError()+50);
     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

@@ -37,7 +37,8 @@ public class RobotMap {
   public static double fWidth = 483.467261958;
   public static double fHeight = 362.600446468;
   public static int sonar = 2;
-  public static int goalHeight = 72;
+  //public static int goalHeight = 102;
+  public static int goalHeight = 64;
   //Misc
   public static int mainArmPresetCollect = -5;
   public static int mainArmPresetTraverse = 10;

src/org/usfirst/frc/team2059/robot/commands/shooter/AlignVertical.java

@@ -0,0 +1,36 @@
+package org.usfirst.frc.team2059.robot.commands.shooter;
+import org.usfirst.frc.team2059.robot.commands.CommandBase;
+import org.usfirst.frc.team2059.robot.Robot;
+import org.usfirst.frc.team2059.robot.RobotMap;
+/**
+ *
+ */
+public class AlignVertical extends CommandBase {
+  public AlignVertical() {
+    requires(mainArm);
+  }
+  // Called just before this Command runs the first time
+  protected void initialize() {
+    mainArm.setSetpoint(mainArm.getShooterAngleError()+50);
+    mainArm.enable();
+  }
+  // Called repeatedly when this Command is scheduled to run
+  protected void execute() {
+    //Move the arm stop
+  }
+  // Make this return true when this Command no longer needs to run execute()
+  protected boolean isFinished() {
+    // Stop when either limit switch is hit
+    return false;
+  }
+  // Called once after isFinished returns true
+  protected void end() {
+    mainArm.disable();
+  }
+  // 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/commands/vision/AlignHorizontal.java

@@ -6,7 +6,10 @@ public class AlignHorizontal extends CommandBase {
   }
   protected void initialize() {
     error = visionHelper.getHorizontalError();
+    driveBase.setDriveEnabled(false);
     driveBase.resetGyro();
+    mainArm.setSetpoint(90);
+    mainArm.enable();
   }
   protected boolean isFinished() {
     return false;
@@ -16,6 +19,8 @@ public class AlignHorizontal extends CommandBase {
   }
   protected void end() {
     driveBase.getGyroController().disable();
+    driveBase.setDriveEnabled(true);
+    mainArm.disable();
   }
   protected void interrupted() {
     end();

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

@@ -20,8 +20,9 @@ public class DriveBase extends Subsystem {
   PIDController gyroController = new PIDController(0.07, 0.002, 0.08, new GyroPIDSource(), new MotorsPIDOutputReversed());
   PIDController leftEncoderController = new PIDController(0.02, 0.002, 0.017, leftEncoder, new MotorsPIDOutput());
   double encoderGyroCorrection = 1;
+  boolean driveEnabled = true;
   public void initDefaultCommand() {
-//    setDefaultCommand(new Drive());
+    setDefaultCommand(new Drive());
   }
   public void stopDriving() {
     leftMotorOne.set(0);
@@ -30,10 +31,12 @@ public class DriveBase extends Subsystem {
     rightMotorTwo.set(0);
   }
   public void driveArcade(double x, double y, double z, double sensitivity) {
-    leftMotorOne.set((-y + (x + z)) * sensitivity);
+    if(driveEnabled){
-    leftMotorTwo.set((-y + (x + z)) * sensitivity);
+      leftMotorOne.set((-y + (x + z)) * sensitivity);
-    rightMotorOne.set((y + (x + z)) * sensitivity);
+      leftMotorTwo.set((-y + (x + z)) * sensitivity);
-    rightMotorTwo.set((y + (x + z)) * sensitivity);
+      rightMotorOne.set((y + (x + z)) * sensitivity);
+      rightMotorTwo.set((y + (x + z)) * sensitivity);
+    }
   }
   public void driveStraight(double y, double correction) {
     SmartDashboard.putNumber("GyroAngle", gyro.getAngle());
@@ -53,6 +56,9 @@ public class DriveBase extends Subsystem {
     gyroController.enable();
     gyroController.setSetpoint(angle);
   }
+  public void setDriveEnabled(boolean enabled){
+    driveEnabled = enabled;
+  }
   public PIDController getLeftController() {
     return leftEncoderController;
   }

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

@@ -16,7 +16,7 @@ public class MainArm extends PIDSubsystem {
   private double min = RobotMap.zeroDegrees;
   private double max = RobotMap.ninetyDegrees;
   public MainArm() {
-    super("MainArm", 0.1, 0.0, 0.002);
+    super("MainArm", 0.08, 0.0, 0.002);
     getPIDController().setContinuous(false);
     setSetpoint(70);
     enable();
@@ -90,8 +90,8 @@ 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
+    double correctedHeight = (RobotMap.goalHeight + 26) - 44; //44 is the distance between the ground and the sonar
-    return (180/Math.PI) * Math.atan(correctedHeight/getSonarDistance());
+    return (180/Math.PI) * Math.atan(correctedHeight/(getSonarDistance()-5));
   }
   public double getSonarDistanceRaw(){
      return sonar.getVoltage();

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

@@ -15,8 +15,17 @@ public class VisionHelper {
     contoursTable = NetworkTable.getTable("GRIP/contours");
   }
   public double getCenterContourX(){
+    int highestAreaIndex = 0;
+    int index = 0;
+    double areas[] = linesTable.getNumberArray("area", new double[0]);
+    for(double area : areas){
+       if(area >= areas[highestAreaIndex]){
+          highestAreaIndex=index;
+       }
+       index++;
+    }
      try{
-        return contoursTable.getNumberArray("centerX",new double[0])[0];
+        return contoursTable.getNumberArray("centerX",new double[0])[highestAreaIndex];
      }catch(Exception e){
         return 0;
      }
@@ -53,7 +62,6 @@ public class VisionHelper {
        }
        index++;
     }
-    System.out.println("Highest Length Index:"+ highestLengthIndex);
     try {
       return (x1s[highestLengthIndex]+x2s[highestLengthIndex])/2;
     } catch (Exception e) {
@@ -72,7 +80,6 @@ public class VisionHelper {
        }
        index++;
     }
-    System.out.println("Highest Length Index:"+ highestLengthIndex);
     try {
       return (y1s[highestLengthIndex]+y2s[highestLengthIndex])/2;
     } catch (Exception e) {
@@ -80,7 +87,7 @@ public class VisionHelper {
     }
   }
   public double getHorizontalError() {
-    return (180/Math.PI) * (Math.atan((getCenterContourX() - (RobotMap.imageWidth / 2)) / RobotMap.fWidth));
+    return ((180/Math.PI) * (Math.atan((getCenterContourX() - (RobotMap.imageWidth / 2)) / RobotMap.fWidth)))+3.5;
   }
   public double getVerticalError() {
     return (180/Math.PI) * (Math.atan((getCenterY() - (RobotMap.imageHeight / 2)) / RobotMap.fHeight));