From d0c46f6e125e9f327646864e411844d4d3362689 Mon Sep 17 00:00:00 2001 From: Adam Long Date: Wed, 12 Oct 2016 00:49:54 +0000 Subject: [PATCH] more vision changes and things that probably wont work --- src/org/usfirst/frc/team2059/robot/Robot.java | 7 +++--- .../usfirst/frc/team2059/robot/RobotMap.java | 2 ++ .../commands/drivetrain/DriveStraight.java | 1 + .../team2059/robot/subsystems/MainArm.java | 11 ++++++++++ .../robot/subsystems/VisionHelper.java | 22 ++++++++++++++++++- 5 files changed, 39 insertions(+), 4 deletions(-) diff --git a/src/org/usfirst/frc/team2059/robot/Robot.java b/src/org/usfirst/frc/team2059/robot/Robot.java index e327780..b7470ed 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 { -// 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); diff --git a/src/org/usfirst/frc/team2059/robot/RobotMap.java b/src/org/usfirst/frc/team2059/robot/RobotMap.java index fec4f28..2a9f0f0 100644 --- a/src/org/usfirst/frc/team2059/robot/RobotMap.java +++ b/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; diff --git a/src/org/usfirst/frc/team2059/robot/commands/drivetrain/DriveStraight.java b/src/org/usfirst/frc/team2059/robot/commands/drivetrain/DriveStraight.java index 3bf6554..cfa8c11 100644 --- a/src/org/usfirst/frc/team2059/robot/commands/drivetrain/DriveStraight.java +++ b/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() diff --git a/src/org/usfirst/frc/team2059/robot/subsystems/MainArm.java b/src/org/usfirst/frc/team2059/robot/subsystems/MainArm.java index 2296375..4cc3c0d 100644 --- a/src/org/usfirst/frc/team2059/robot/subsystems/MainArm.java +++ b/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 diff --git a/src/org/usfirst/frc/team2059/robot/subsystems/VisionHelper.java b/src/org/usfirst/frc/team2059/robot/subsystems/VisionHelper.java index efc84f5..2e5c0c5 100644 --- a/src/org/usfirst/frc/team2059/robot/subsystems/VisionHelper.java +++ b/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));