diff --git a/src/org/usfirst/frc/team2059/robot/Robot.java b/src/org/usfirst/frc/team2059/robot/Robot.java index 7c0b4e4..efb9311 100644 --- a/src/org/usfirst/frc/team2059/robot/Robot.java +++ b/src/org/usfirst/frc/team2059/robot/Robot.java @@ -21,7 +21,7 @@ public class Robot extends IterativeRobot { // 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"}); + new ProcessBuilder("/home/lvuser/grip").inheritIO().start(); } catch (Exception e) { System.out.println("Error starting GRIP"); } @@ -75,10 +75,11 @@ 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", 71 / Math.tan((52.7 - CommandBase.visionHelper.getVerticalError()))); + SmartDashboard.putNumber("goalDistance", 55 / Math.tan((Math.PI/180) * (64 - CommandBase.visionHelper.getVerticalError()))); if (Robot.oi.getJoysticks()[1].getRawButton(3)) { CommandBase.pneumatics.setArmStopState(true); } else { diff --git a/src/org/usfirst/frc/team2059/robot/subsystems/MainArm.java b/src/org/usfirst/frc/team2059/robot/subsystems/MainArm.java index 7bcb348..2296375 100644 --- a/src/org/usfirst/frc/team2059/robot/subsystems/MainArm.java +++ b/src/org/usfirst/frc/team2059/robot/subsystems/MainArm.java @@ -80,11 +80,12 @@ public class MainArm extends PIDSubsystem { System.out.println("Calibrating bottom to: " + getRaw()); min = getRaw(); return true; - } else if (getTopPressed()) { - System.out.println("Calibrating top to: " + getRaw()); - max = getRaw(); - return true; } + // } else if (getTopPressed()) { + // System.out.println("Calibrating top to: " + getRaw()); + // max = getRaw(); + // return true; + // } return false; } } diff --git a/src/org/usfirst/frc/team2059/robot/subsystems/VisionHelper.java b/src/org/usfirst/frc/team2059/robot/subsystems/VisionHelper.java index 2442e2f..887db79 100644 --- a/src/org/usfirst/frc/team2059/robot/subsystems/VisionHelper.java +++ b/src/org/usfirst/frc/team2059/robot/subsystems/VisionHelper.java @@ -7,28 +7,53 @@ 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 linesTable, contoursTable; double imageWidth = 640; double imageHeight = 480; public VisionHelper() { + linesTable = NetworkTable.getTable("GRIP/lines"); contoursTable = NetworkTable.getTable("GRIP/contours"); } public double getCenterX() { + int highestLengthIndex = 0; + int index = 0; + double lengths[] = linesTable.getNumberArray("length", new double[0]); + double x1s[] = linesTable.getNumberArray("x1", new double[0]); + double x2s[] = linesTable.getNumberArray("x2", new double[0]); + for(double length : lengths){ + if(length >= lengths[highestLengthIndex]){ + highestLengthIndex=index; + } + index++; + } + System.out.println("Highest Length Index:"+ highestLengthIndex); try { - return contoursTable.getNumberArray("centerX", new double[0])[0]; + return (x1s[highestLengthIndex]+x2s[highestLengthIndex])/2; } catch (Exception e) { return 0; } } public double getCenterY() { + int highestLengthIndex = 0; + int index = 0; + double lengths[] = linesTable.getNumberArray("length", new double[0]); + double y1s[] = linesTable.getNumberArray("y1", new double[0]); + double y2s[] = linesTable.getNumberArray("y2", new double[0]); + for(double length : lengths){ + if(length >= lengths[highestLengthIndex]){ + highestLengthIndex=index; + } + index++; + } + System.out.println("Highest Length Index:"+ highestLengthIndex); try { - return contoursTable.getNumberArray("centerY", new double[0])[0]; + return (y1s[highestLengthIndex]+y2s[highestLengthIndex])/2; } catch (Exception e) { return 0; } } public double getHorizontalError() { - return (180/Math.PI) * (Math.atan((getCenterX() - (RobotMap.imageWidth / 2)) / RobotMap.fWidth)); + return (180/Math.PI) * (Math.atan((getCenterX()-20 - (RobotMap.imageWidth / 2)) / RobotMap.fWidth)); } public double getVerticalError() { return (180/Math.PI) * (Math.atan((getCenterY() - (RobotMap.imageHeight / 2)) / RobotMap.fHeight));