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

Added formulas for vision tracking

Browse Source
This commit is contained in:
Austen Adler 2016-10-05 20:09:09 -04:00
parent c8e2c9697e
commit 836a4317e5
No known key found for this signature in database
GPG Key ID: 7ECEE590CCDFE3F1
2 changed files with 14 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -27,6 +27,15 @@ public class RobotMap {
public static int portcullisSolenoidTwo = 3; public static int portcullisSolenoidTwo = 3;
public static int armStopSolenoidOne = 4; public static int armStopSolenoidOne = 4;
public static int armStopSolenoidTwo = 5; public static int armStopSolenoidTwo = 5;
//Camera
//public static double fWidth = 2.8;
// Camera: Axis M1013
// FOV = 67deg
// f = pixelSize/(2*tan(FOV/2))
public static double imageWidth = 640;
public static double imageHeight = 480;
public static double fWidth = 483.467261958;
public static double fHeight = 362.600446468;
//Misc //Misc
public static int mainArmPresetCollect = -5; public static int mainArmPresetCollect = -5;
public static int mainArmPresetTraverse = 10; public static int mainArmPresetTraverse = 10;

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

@ -1,4 +1,5 @@
package org.usfirst.frc.team2059.robot.subsystems; package org.usfirst.frc.team2059.robot.subsystems;
import org.usfirst.frc.team2059.robot.RobotMap;
import edu.wpi.first.wpilibj.command.PIDSubsystem; import edu.wpi.first.wpilibj.command.PIDSubsystem;
import edu.wpi.first.wpilibj.CANTalon; import edu.wpi.first.wpilibj.CANTalon;
import edu.wpi.first.wpilibj.AnalogInput; import edu.wpi.first.wpilibj.AnalogInput;
@ -18,30 +19,20 @@ public class VisionHelper {
try{ try{
return contoursTable.getNumberArray("centerX",new double[0])[0]; return contoursTable.getNumberArray("centerX",new double[0])[0];
}catch(Exception e){ }catch(Exception e){
return 420; return 0;
} }
} }
public double getCenterY(){ public double getCenterY(){
try{ try{
return contoursTable.getNumberArray("centerY",new double[0])[0]; return contoursTable.getNumberArray("centerY",new double[0])[0];
}catch(Exception e){ }catch(Exception e){
return 420; return 0;
} }
} }
public double getHorizontalError(){ public double getHorizontalError(){
if(getCenterX()==420){ return Math.atan2(getCenterX()-RobotMap.imageWidth/2, RobotMap.fWidth);
return 420;
}
double degreesPerPixel=hfieldOfView/imageWidth;
double centerColumn=imageWidth/2;
return (getCenterX()-centerColumn)*degreesPerPixel;
} }
public double getVerticalError(){ public double getVerticalError(){
if(getCenterY()==420){ return Math.atan2(getCenterY()-RobotMap.imageHeight/2, RobotMap.fHeight);
return 420;
}
double degreesPerPixel=vfieldOfView/imageHeight;
double centerRow=imageHeight/2;
return (getCenterY()-centerRow)*degreesPerPixel;
} }
} }