So now we are having problems with our distance and maybe angle (although it did seem to work…). Here is all code that has to do with the camera and our tracking/aiming systems: (in ProcessImage there is a findDistance method)
ProcessImage Command:
/*
* To change this template, choose Tools | Templates
* and open the template in the editor.
*/
package com.slidellrobotics.reboundrumble.commands;
import com.slidellrobotics.reboundrumble.RobotMap;
import com.slidellrobotics.reboundrumble.subsystems.TrackingCamera;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.image.NIVisionException;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
/**
*
* @author Allister Wright
*/
public class ProcessImage extends CommandBase {
static int stepNo =1;
private static double lastTime = 0;
private static double thisTime = 0;
private static double timeLapse = 0;
double targetHeight = TrackingCamera.targetHeight; // Create a few necesarry local variables
double targetWidth = TrackingCamera.targetWidth; // for concise code and calcs.
double targetHeightFt; // Target Height in Feet
double targetWidthFt; // Target Width in Feet
double imageHeight; // Total Height in Pixels
double imageWidth; // Total Width imn Pixels
double verticalFOV; // Vertical Field of View in Feet
double horizontalFOV; // Horizontal Field of View in Feet
double verticalViewingAngle; // Vertical Camera Viewing Angle
double horizontalViewingAngle; // Horizontal Camera Viewing Angle
double horizontalRattle; // Horizontal off-centerness of center of goal
double verticalRattle; // Vertical off-centerness of center of goal
double centerDistance = 0; // Distance Variable to be used in firing Calculation
double offCenterPixels = 0;
double offCenterFt = 0;
double verticalDistanceResult = 0;
double horizontalDistanceResult = 0;
double trueDistance = 0;
double d = 0;
double pi = 3.14159262;
public ProcessImage() {
// Use requires() here to declare subsystem dependencies
// eg. requires(chassis);
requires(camera);
requires(lazySusan);
requires(leftShootingMotors);
requires(rightShootingMotors);
}
// Called just before this Command runs the first time
protected void initialize() {
stepNo = 1;
}
// Called repeatedly when this Command is scheduled to run
protected void execute() {
thisTime = Timer.getFPGATimestamp();
timeLapse = thisTime - lastTime;
if(timeLapse >= 1.0) {
getImage();
if(stepNo == 6) {
if(TrackingCamera.reports != null) {
selectGoal();
findAngle();
findDistance();
} else { // If no goals are found
System.out.println("Goal Selection and Analysis Aborted"); // Print a notifier
}
stepNo++;
}
}
}
// Make this return true when this Command no longer needs to run execute()
protected boolean isFinished() {
//todo this can just be set to true
if(stepNo == 7) {
return true;
} else if(stepNo > 7) {
System.out.println("Error Exit");
return true;
} else {
return false;
}
}
// Called once after isFinished returns true
protected void end() {
try {
if (TrackingCamera.pic != null) {
TrackingCamera.pic.free();
}
if (TrackingCamera.convexHullImage != null) {
TrackingCamera.convexHullImage.free();
}
if (TrackingCamera.thresholdHSL != null) {
TrackingCamera.thresholdHSL.free();
}
if (TrackingCamera.boundImage != null) {
TrackingCamera.boundImage.free();
}
} catch (Exception ex) {
System.out.println("Memory: "+ex);
}
lastTime = thisTime;
}
// Called when another command which requires one or more of the same
// subsystems is scheduled to run
protected void interrupted() {
end();
}
private void getImage() {
System.out.println("getImage");
try {
switch(stepNo){
case 1:
System.out.println("Running Case: 1");
TrackingCamera.pic = camera.getImageFromCamera();
System.out.println("Ran Case: 1.1");
if(TrackingCamera.pic != null) {
TrackingCamera.totalWidth = TrackingCamera.pic.getWidth();
System.out.println("Ran Case: 1.2");
TrackingCamera.totalHeight = TrackingCamera.pic.getHeight();
System.out.println("Ran Case: 1.3");
} else {
break;
}
stepNo++;
break;
case 2:
System.out.println("Running Case: 2");
TrackingCamera.thresholdHSL = TrackingCamera.pic.thresholdHSL(150, 185, 244, 255, 2, 20); //Sets a Blue light threshold
System.out.println("Ran Case: 2.1");
stepNo++;
break;
case 3:
System.out.println("Running Case: 3");
TrackingCamera.convexHullImage = TrackingCamera.thresholdHSL.convexHull(false); //Fills in the bounding boxes for the targets
System.out.println("Ran Case: 3.1");
stepNo++;
break;
case 4:
System.out.println("Running Case: 4");
TrackingCamera.boundImage = TrackingCamera.convexHullImage.particleFilter(TrackingCamera.cc);
System.out.println("Ran Case: 4.1");
stepNo++;
break;
case 5:
System.out.println("Running Case: 5");
TrackingCamera.reports = TrackingCamera.boundImage.getOrderedParticleAnalysisReports();
System.out.println("Ran Case: 5.1");
System.out.println("Reports: "+TrackingCamera.reports.length);
System.out.println("Ran Case: 5.2");
stepNo++;
break;
}
} catch (NIVisionException ex) {
System.out.println(ex);
stepNo = 60;
} catch (Exception ex) {
System.out.println(ex);
stepNo = 60;
}
}
private void selectGoal() {
System.out.println("selectGoal");
TrackingCamera.targetGoal = null;
if (TrackingCamera.reports.length == 0) {
return;
} if (TrackingCamera.reports.length == 1) {
System.out.println("Not enough goals");
TrackingCamera.targetGoal = TrackingCamera.reports[0];
} else {
//Gus says: is this right. it doesn't look right to me...
TrackingCamera.leftGoal = TrackingCamera.reports[0]; //Recognizes the
TrackingCamera.rightGoal = TrackingCamera.reports[0]; //middle goals.
int maxIndex = TrackingCamera.reports.length;
if (maxIndex > 4) {
maxIndex=4;
}
for(int i = 1; i <= maxIndex; i++) {
if(TrackingCamera.reports*.center_mass_x < TrackingCamera.leftGoal.center_mass_x) {
TrackingCamera.leftGoal = TrackingCamera.reports*;
} if(TrackingCamera.reports*.center_mass_x > TrackingCamera.leftGoal.center_mass_x) {
TrackingCamera.rightGoal = TrackingCamera.reports*;
}
}
//We have four goals in view index 1 is the left and index 2 is right
double leftWidth = TrackingCamera.leftGoal.boundingRectWidth; //Finds the widths of
double rightWidth = TrackingCamera.rightGoal.boundingRectWidth; //both middle goals.
if (leftWidth <= rightWidth) {
TrackingCamera.targetGoal = TrackingCamera.rightGoal; //Decides which goal we are
} else { //closer to and targets it.
TrackingCamera.targetGoal = TrackingCamera.leftGoal; //
}
System.out.println("Target Selected");
}
}
private void findAngle() {
System.out.println("findAngle");
if (TrackingCamera.targetGoal == null){
return;
}
TrackingCamera.horCenter = (TrackingCamera.totalWidth / 2); //Finds the pixel value of the horizontal center
TrackingCamera.targetLocale = TrackingCamera.targetGoal.center_mass_x; //Finds the center of our target
TrackingCamera.targetDiff = Math.abs(TrackingCamera.targetLocale - TrackingCamera.horCenter); // see how far away we are
//TODO: tune the 10 pixels to the right number
//there is always going to be a little error, but we want some small window
//where the lazy suzan stops moving to we can make an accurate shot.
System.out.println("Targe Diff: "+TrackingCamera.targetDiff);
if (TrackingCamera.targetDiff < 15) {
//lazySusan.setRelay(RobotMap.susanOff); //turn off
} else if (TrackingCamera.targetLocale < TrackingCamera.horCenter) { //and if we are facing right
//lazySusan.setRelay(RobotMap.susanLeft); //turn left
lazySusan.setSetpointRelative(-5);
} else { //if we face left
//lazySusan.setRelay(RobotMap.susanRight); //turn right
lazySusan.setSetpointRelative(+5);
}
SmartDashboard.putDouble("Angle",TrackingCamera.targetDiff);
}
private void findDistance() {
System.out.println("findDistance");
if (TrackingCamera.targetGoal == null){ // If no target is found
//leftShootingMotors.setSetpoint(1000); // Set Left shooting Motors to about Half Speed
//rightShootingMotors.setSetpoint(1000); // Set Right Shooting Motors to about Half Speed
System.out.println("No target set"); // Debug Print Statement
System.out.println("Checkpoint 10a");
return;
}
verticalViewingAngle = 47; // Defines the Viewing
horizontalViewingAngle = 47; // Angles of our camera
imageHeight = 480; // Image Height
targetHeight = TrackingCamera.targetGoal.boundingRectHeight; // Sets the height of our target.
targetHeightFt = 1.5; // Defines goal's constant ft height
imageWidth = 640; // Image Width
targetWidth = TrackingCamera.targetGoal.boundingRectWidth; // Sets the width of our target.
targetWidthFt = 2.0; // Defines goal's constant ft width
verticalFOV = imageHeight*(targetHeightFt/targetHeight); // Gets the Foot Value of our Vertical Field of View.
horizontalFOV = imageWidth*(targetWidthFt/targetWidth); // Gets the ft value of our horizontal Field of View.
verticalRattle = Math.abs(TrackingCamera.targetGoal.center_mass_y - (imageHeight/2)); // Finds the vertical off-ceneterness.
horizontalRattle = Math.abs(TrackingCamera.targetGoal.center_mass_x - (imageWidth/2)); // Finds the horizontal off-centerness.
verticalDistanceResult = Math.sqrt(4/3)*(verticalFOV/2)/Math.tan(verticalViewingAngle/2); // Provides the Result of our Vertically-Based Calculation.
horizontalDistanceResult = Math.sqrt(3/4)*(horizontalFOV/2)/Math.tan(horizontalViewingAngle/2); // Provides the Result of our Horizontally-Based Calculation.
centerDistance = (verticalDistanceResult + horizontalDistanceResult) / 2; // Take the average to try get a more accurate measurement.
offCenterPixels = Math.sqrt((verticalRattle*verticalRattle) + (horizontalRattle*horizontalRattle)); // Finds the Linear Distance from the Center of the Image to the Center of the Goal.
offCenterFt = offCenterPixels*(Math.sqrt((verticalFOV*verticalFOV)+(horizontalFOV*horizontalFOV))); // Converts the above Caluclated measurement into its proper Ft value.
trueDistance = Math.sqrt((centerDistance*centerDistance)+(offCenterFt*offCenterFt)); // Find the Linear Distance form the Lens of our Camera to the Center of our Goal.
//if distance to target is invalid, just set it to some number
if (TrackingCamera.distanceToTarget > 60 || TrackingCamera.distanceToTarget <= 0) {
TrackingCamera.distanceToTarget = 60;
}
d = trueDistance; // See below Calculation for conciseness
TrackingCamera.launchSpeed = 60 * (d / Math.sqrt((11 / 6 - d) / -16.1) / (2 / 3 * pi)); //Calcs the required rpms for firing
leftShootingMotors.setSetpoint(TrackingCamera.launchSpeed); // Sets the shooting Left Shooting Motors
rightShootingMotors.setSetpoint(TrackingCamera.launchSpeed); // Sets the Right Shooting Motors
/* A String of Debug Print Statements */
System.out.println();
System.out.println("Vertcal Distance Result: "+verticalDistanceResult);
System.out.println("Horizontal Distance Result: "+horizontalDistanceResult);
System.out.println("Central Distance: "+centerDistance);
System.out.println("True Distance: "+d);
System.out.println("Camera Launch Speed: "+TrackingCamera.launchSpeed);
System.out.println();
//SmartDashboard.putDouble("Vertical Distance Result", verticalDistanceResult);
//SmartDashboard.putDouble("Horizontal Distance Result", horizontalDistanceResult);
//SmartDashboard.putDouble("Center Point Distance", centerDistance);
SmartDashboard.putDouble("Distance", d);
SmartDashboard.putDouble("Launch", TrackingCamera.launchSpeed);
}
}
TrackingCamera Subsystem:
/*
* To change this template, choose Tools | Templates
* and open the template in the editor.
*/
package com.slidellrobotics.reboundrumble.subsystems;
import com.slidellrobotics.reboundrumble.commands.ProcessImage;
import edu.wpi.first.wpilibj.camera.AxisCamera;
import edu.wpi.first.wpilibj.camera.AxisCameraException;
import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.image.*;
/**
*
* @author Allister Wright
*/
public class TrackingCamera extends Subsystem {
// Put methods for controlling this subsystem
// here. Call these from Commands.
public static AxisCamera camera;
public static ColorImage pic;
public static ParticleAnalysisReport] reports;
//public static int leftGoalIndex = 0, rightGoalIndex = 0;
public static CriteriaCollection cc = new CriteriaCollection();
public static BinaryImage thresholdHSL;
public static BinaryImage convexHullImage;
public static BinaryImage boundImage;
//public static BinaryImage bigObjectsImage;
public static double totalWidth = 640, totalHeight = 480;
public static ParticleAnalysisReport targetGoal, leftGoal, rightGoal;
public static double targetLocale, horCenter, targetDiff, launchSpeed;
public static double centerDistance;
public static double distanceToTarget, targetHeight, targetHeightFeet = 1.5;
public static double horFOV, vertFOV, cameraVertFOV = 47, cameraHorizFOV = 47;
public static double d1, d2, d, targetWidth, targetWidthFeet = 2;
//public static boolean getFinished = false, selectFinished = false;
//public static boolean angleFinished = false, distanceFinished = false;
public TrackingCamera() {
if(camera == null) {
TrackingCamera.camera = AxisCamera.getInstance("10.39.46.11");
}
if(camera != null) {
System.out.println("Camera init");
} else {
System.out.println("Camera Failure");
}
}
public void initDefaultCommand() {
// Set the default command for a subsystem here.
setDefaultCommand(new ProcessImage());
}
public ColorImage getImageFromCamera() throws NIVisionException {
try {
if(camera!=null) {
if (camera.freshImage()) {
try {
System.out.println("sent image");
return camera.getImage();
} catch (AxisCameraException ex) {
System.out.println("getImageFromCamera"+ex);
} catch (NIVisionException ex) {
System.out.println(ex);
}
}
} else {
System.out.println("CAMERA IS STILL NULL!!!!");
}
} catch (Exception ex) {
System.out.println("Get Image From Camera: "+ex);
}
System.out.println("return null");
return null;
}
private class CalibrationPoint {
double distance;
double rpms;
public CalibrationPoint(double distance, double rpms) {
this.distance = distance;
this.rpms = rpms;
}
}
//just incase the formula doesn't work out here is a test based
//interpolation function
public double distanceToRMP(double distance) {
//load the test data when practing with the real launcher
// points MUST be inorder of closest to furthest away
CalibrationPoint calibrationPoints] = {new CalibrationPoint(5, 100),
new CalibrationPoint(10, 200),
new CalibrationPoint(15, 500),
new CalibrationPoint(40, 1500)};
//find the two calibration points were the input distance is between them
int upperIndex;
upperIndex = 1;
while(upperIndex < calibrationPoints.length) {
if (distance <= calibrationPoints[upperIndex].distance &&
distance > calibrationPoints[upperIndex-1].distance) {
break;
}
upperIndex++;
}
//interpolate the point
if (upperIndex >= calibrationPoints.length){
upperIndex=calibrationPoints.length-1;
System.out.println("Error calibration");
}
System.out.println("rpms "+calibrationPoints[upperIndex].rpms);
double slope = (calibrationPoints[upperIndex].rpms - calibrationPoints[upperIndex - 1].rpms)
/ (calibrationPoints[upperIndex].distance - calibrationPoints[upperIndex - 1].distance);
double intercept = calibrationPoints[upperIndex].rpms - slope * calibrationPoints[upperIndex].distance;
return slope * distance + intercept;
}
}
LazySusan Subsystem:
/*
* To change this template, choose Tools | Templates
* and open the template in the editor.
*/
package com.slidellrobotics.reboundrumble.subsystems;
import com.slidellrobotics.reboundrumble.RobotMap;
import com.slidellrobotics.reboundrumble.commands.CommandBase;
import edu.wpi.first.wpilibj.Gyro;
import edu.wpi.first.wpilibj.Relay;
import edu.wpi.first.wpilibj.Relay.Value;
import edu.wpi.first.wpilibj.command.PIDSubsystem;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
/**
*
* @author Gus Michel
*/
public class LazySusan extends PIDSubsystem {
private static final double Kp = 1.0;
private static final double Ki = 0.0;
private static final double Kd = 0.0;
private Relay susanWindow;
private Gyro susanGyro;
private final String dashName = "Lazy Susan";
// Initialize your subsystem here
public LazySusan() {
super("LazySusanPID", Kp, Ki, Kd);
System.out.println("[LazySusan] Starting");
susanWindow = new Relay(RobotMap.susanSpike);
System.out.println("[LazySusan] susanWindow initalized");
susanGyro = new Gyro(RobotMap.turretGyro);
System.out.println("[LazySusan] susanGyro initalized");
System.out.println("[LazySusan] Started");
setSetpoint(0);
enable();
getPIDController().setOutputRange(30, 30);
getPIDController().setInputRange(-360, 360);
// Use these to get going:
// setSetpoint() - Sets where the PID controller should move the system
// to
// enable() - Enables the PID controller.
}
public void initDefaultCommand() {
// Set the default command for a subsystem here.
//setDefaultCommand(new MySpecialCommand());
}
protected double returnPIDInput() {
// Return your input value for the PID loop
// e.g. a sensor, like a potentiometer:
// yourPot.getAverageVoltage() / kYourMaxVoltage;
// System.out.println("gyro: " + susanGyro.getAngle());
SmartDashboard.putDouble("Gyro", susanGyro.getAngle());
return susanGyro.getAngle();
}
protected void usePIDOutput(double output) {
// Use output to drive your system, like a motor
// e.g. yourMotor.set(output);
output = getSetpoint()-susanGyro.getAngle();
// System.out.println("lazy output: "+output);
if(output > 5.0) {
this.setRelay(RobotMap.susanRight);
//System.out.println("Left");
} else if(output < -5.0) {
this.setRelay(RobotMap.susanLeft);
//System.out.println("Right");
} else {
this.setRelay(RobotMap.susanOff);
}
}
public void setRelay(Value value) {
susanWindow.set(value);
if(value.equals(RobotMap.susanRight)) {
SmartDashboard.putString(dashName, "Right");
} else if(value.equals(RobotMap.susanLeft)) {
SmartDashboard.putString(dashName, "Left");
} else {
SmartDashboard.putString(dashName, "Off");
}
}
public Relay getSpike() {
return susanWindow;
}
public Gyro getGyro() {
return susanGyro;
}
}
FireMotors Subsystem
/*
* To change this template, choose Tools | Templates
* and open the template in the editor.
*/
package com.slidellrobotics.reboundrumble.subsystems;
import edu.wpi.first.wpilibj.Counter;
import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.command.PIDSubsystem;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
/**
*
* @author Gus Michel
*
*/
public class FireMotors extends PIDSubsystem {
private static double Kp = 0.00001;
private static double Ki = 0.0;
private static double Kd = 0.0;
private Counter counter;
private Jaguar victor;
double lastTime;
double newtime;
static final int rpmsFilterMax = 5;
Double] rpmsFilter= new Double[rpmsFilterMax];
int rpmsFilterIndex = 0;
double rpms=0;
double victorSetting=0;
String Name;
// Initialize your subsystem here
public FireMotors(String Name,int counterChannel, int victorChannel) {
super(Name, Kp, Ki, Kd);
counter = new Counter(counterChannel);
victor = new Jaguar(victorChannel);
enable();
setSetpoint(1000); //rpms
lastTime = Timer.getFPGATimestamp();
counter.start();
getPIDController().setOutputRange(-.1, .1);
setSetpointRange(0,2000);
this.Name = Name; //set class scope name to the Name passed in the constructor
System.out.println(Name + "Firemotor init");
for (int index =0;index < rpmsFilterMax;index++){
rpmsFilter[index]=Double.valueOf(0);
}
}
public void initDefaultCommand() {
// Set the default command for a subsystem here.
}
protected double returnPIDInput() {
// Return your input value for the PID loop
double timespan;
int counts;
// these three lines are time critial
newtime = Timer.getFPGATimestamp();
timespan = newtime- lastTime; //number of seconds during counting
if (timespan <= 0.130) {
//System.out.println("Firemotor return PID timspan 0");
return rpms; //don't recalculate, just give what ever it was
}
counts = counter.get(); //number of counts since last update
counter.reset(); //reset counts ASAP so we don't miss any
// System.out.println("1");
lastTime = newtime;
//System.out.println("2");
rpms = counts/ 8.0 / timespan*60.0;
//8 counts per revolution, 60 seconds per minute
//System.out.println(Name + ": " + rpms);
//System.out.println(" Counts: "+counts);
//System.out.println(" Timespan: "+timespan);
//System.out.println();
//sometimes the counter goes nuts
//this deboundes it
if (rpms > 1750)
rpms = 1750;
// System.out.println("3");
rpmsFilter[rpmsFilterIndex]=Double.valueOf(rpms);
rpmsFilterIndex++;
if (rpmsFilterIndex >= rpmsFilterMax){
rpmsFilterIndex=0;
}
//System.out.println("4");
rpms =0;
for (int index =0;index < rpmsFilterMax;index++){
rpms = rpms + rpmsFilter[index].doubleValue();
//System.out.println(index);
}
rpms = rpms / rpmsFilterMax;
return rpms;
}
protected void usePIDOutput(double output) {
//When the PID system thinks there is no error then
//set point is equal to rpms and output = 0
//soft start if we not moving, only set power to 10%
victorSetting=victor.get()-output;
if (Math.abs(victorSetting) < .3){
victorSetting = -.3;
}
//System.out.println("Use PID "+victorSetting);
//victor.set(victorSetting);
victor.set(getSetpoint());
}
public void updateStatus(){
SmartDashboard.putDouble(Name + " RPMS",rpms);
SmartDashboard.putDouble(Name + " victor",victor.get());
SmartDashboard.putDouble(Name + " victor Setting",victorSetting);
}
}
FireMotor Instances
public static FireMotors leftShootingMotors = new FireMotors("Left",RobotMap.leftFireEncoder, RobotMap.leftShootingMotor);
public static FireMotors rightShootingMotors = new FireMotors("Right",RobotMap.rightFireEncoder, RobotMap.rightShootingMotor);