team2059/ford-java
2
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

vision changes

Browse Source
This commit is contained in:
Adam Long 2016-10-08 22:51:11 +00:00
parent f0ea150fac
commit 7745c589ae
3 changed files with 37 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
src/org/usfirst/frc/team2059/robot/Robot.java
View File

@ -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 {

9
src/org/usfirst/frc/team2059/robot/subsystems/MainArm.java
View File

@ -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;
}
}

33
src/org/usfirst/frc/team2059/robot/subsystems/VisionHelper.java
View File

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