Camera Tracking help please

[2472Programer]

This year, my team has for the first time decided to actually use the camera instead of it just being a place holder on the robot. The only problem with this is that I don't know how to program the camera to track the reflective targets on the field. I, so far, have gotten as far as getting the camera to take a picture and filter it down to the green box it is going to track. Now I need to get the robot it react and change its position accordingly. Any help is appreciated. A copy of my code is attached if anyone wants to check if i already messed something up.

package edu.wpi.first.wpilibj.templates;


import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.camera.AxisCamera;
import edu.wpi.first.wpilibj.image.BinaryImage;
import edu.wpi.first.wpilibj.image.ColorImage;
import edu.wpi.first.wpilibj.image.CriteriaCollection;
import edu.wpi.first.wpilibj.image.NIVision;
import edu.wpi.first.wpilibj.image.NIVisionException;
import edu.wpi.first.wpilibj.image.ParticleAnalysisReport ;

/**
* The VM is configured to automatically run this class, and to call the
* functions corresponding to each mode, as described in the IterativeRobot
* documentation. If you change the name of this class or the package after
* creating this project, you must also update the manifest file in the resource
* directory.
*/
public class Main extends IterativeRobot {
AxisCamera camera = AxisCamera.getInstance();
Joystick leftStick = new Joystick(1);
Joystick rightStick = new Joystick(2);
Joystick joypad = new Joystick(3);
CriteriaCollection cc;
RobotDrive tank = new RobotDrive(1,2,3,4);
/**
* This function is run when the robot is first started up and should be
* used for any initialization code.
*/
public void robotInit() {

cc = new CriteriaCollection();

cc.addCriteria(NIVision.MeasurementType.IMAQ_MT_AR EA, 30, 400, false);

cc.addCriteria(NIVision.MeasurementType.IMAQ_MT_AR EA, 40, 400, false);

}

/**
* This function is called periodically during autonomous
*/
public void autonomousPeriodic() {

}

/**
* This function is called periodically during operator control
*/
public void teleopPeriodic() {

tank.tankDrive(leftStick, rightStick);

if(joypad.getRawButton(1)){

centerCalculate();
}


}

/**
* This function is called periodically during test mode
*/
public void testPeriodic() {

}

private void centerCalculate() {

ColorImage image = null;

BinaryImage thresholdImage = null;

BinaryImage bigObjectsImage = null;

BinaryImage convexHullImage = null;

BinaryImage filteredImage = null;

try{

image = camera.getImage();

thresholdImage = image.thresholdRGB(0, 45, 25, 225, 0, 45);

bigObjectsImage = thresholdImage.removeSmallObjects(false, 2);

convexHullImage = bigObjectsImage.convexHull(false);

filteredImage = convexHullImage.particleFilter(cc);

ParticleAnalysisReport[] reports = filteredImage.getOrderedParticleAnalysisReports();

for(int i = 0; i <reports.length +1;i++){

System.out.println(reports[i].center_mass_x);
}


}catch(Exception ex){

}finally{

}

try{

filteredImage.free();

convexHullImage.free();

bigObjectsImage.free();

thresholdImage.free();

image.free();

}catch(NIVisionException ex){

}
}

}

[Johnbot]

What you want to do is repeatedly re-run that code as you rotate your robot to face the target. Look at the reports[i].center_mass_x, as that will tell you the center of the target. Generally, the index, i, goes from 0 being the highest target to the left, and reports.length-1 being the lowest to the right.

You will want the center_mass_x value to be your video resolution divided by two.

You can go about this by using a PID loop for accurate results, though that requires tuning. If you don't need it to be super precise, you can just have it turn the robot at a speed proportional to the distance between the center_mass_x value and the center of your camera, and stop turning when you are within, say, 10 pixels.

[sarangmittal]

In order to start to use the data that you get from the camera, I suggest finding the corners of the targets, which shouldn't be too hard.Then, use that to find the center of the targets.

There was a white paper on this kinda thing last year, but I can't find it so I'll try and summarize

Once you find the center, there is some simple math you can do with pixels.
You should know the resolution of your camera and its FOV (as in the angle created by the left most point and the right most point the camera can see).

First, use the pixel value for the length or height of the target and compare it to the known length or height of the target to create a ratio of pixels to feet for that specific image. Once you have this, you can calculate the width of the camera image (resolution * feet/pixel). And since you know the FOV of the camera, you can calculate the direct distance from the camera to the center of the target. Then using the physical vertical distance from the camera to the center of the target, you can calculate the horizontal difference.

As for left/right movement, just compare the center pixel of the camera to where the center of the target is and adjust accordingly (assuming your camera is mounted in the middle of the robot).

I hope this helps, and if not, I can post our code from last year.

[2472Programer]

We got a somewhat tracking code working today, but i would like the people of chief delphi to take a look at it. I've posed errors in the code (as comments) that my mentor and I can't figure out. PLZ HELP!

package edu.wpi.first.wpilibj.templates;


import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.Relay;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.camera.AxisCamera;
import edu.wpi.first.wpilibj.image.BinaryImage;
import edu.wpi.first.wpilibj.image.ColorImage;
import edu.wpi.first.wpilibj.image.CriteriaCollection;
import edu.wpi.first.wpilibj.image.NIVision;
import edu.wpi.first.wpilibj.image.NIVisionException;
import edu.wpi.first.wpilibj.image.ParticleAnalysisReport ;





public class RobotTemplate extends IterativeRobot {

Joystick leftStick = new Joystick(1);

Joystick rightStick = new Joystick(2);

Joystick joypad = new Joystick(3);

RobotDrive tank = new RobotDrive(3,1,4,2);

Victor shooter = new Victor(5);

Relay ballLift = new Relay(1);

Relay ballHolder = new Relay(2);

Relay shooterAngle = new Relay(3);

AxisCamera camera = AxisCamera.getInstance();

CriteriaCollection cc;

public int centerup = 172;

public int centerdown = 152;

DigitalInput shootermin = new DigitalInput(1);


DigitalInput shootermax = new DigitalInput(2);

public void robotInit() {

cc = new CriteriaCollection();

cc.addCriteria(NIVision.MeasurementType.IMAQ_MT_AR EA, 30, 400, false);

cc.addCriteria(NIVision.MeasurementType.IMAQ_MT_AR EA, 40, 400, false);





}

/**
* This function is called periodically during autonomous
*/
public void autonomousPeriodic() {
//
}

/**
* This function is called periodically during operator control
*/
public void teleopPeriodic() {

tank.tankDrive(leftStick, rightStick);

if(joypad.getRawButton(5)){

ballLift.set(Relay.Value.kForward);

ballHolder.set(Relay.Value.kForward);

}else if(joypad.getRawButton(7)){

ballLift.set(Relay.Value.kReverse);

ballHolder.set(Relay.Value.kReverse);

}else{

ballLift.set(Relay.Value.kOff);

ballHolder.set(Relay.Value.kOff);
}

if(joypad.getRawButton(4)){

shooter.set(1);

}else if(joypad.getRawButton(3)){

shooter.set(-1);

}else{



shooter.set(0);

}

if(joypad.getRawButton(6)){

increaseShooterAngle();

}else if(joypad.getRawButton(8)){

decreaseShooterAngle();

}else{

shooterAngle.set(Relay.Value.kOff);

}

if(joypad.getRawButton(2)){

System.out.println("Button 2 Pressed");

centerShooter();

}

if(joypad.getRawButton(1)){

readLimitswiches();

}



}

/**
* This function is called periodically during test mode
*/
public void testPeriodic() {

}

private void centerShooter() {

ColorImage image = null;

BinaryImage thresholdImage = null;

BinaryImage bigObjectsImage = null;

BinaryImage convexHullImage = null;

BinaryImage filteredImage = null;

try{

image = camera.getImage();

thresholdImage = image.thresholdRGB(0, 45, 25, 225, 0, 45);

bigObjectsImage = thresholdImage.removeSmallObjects(false, 2);

convexHullImage = bigObjectsImage.convexHull(false);

filteredImage = convexHullImage.particleFilter(cc);

ParticleAnalysisReport[] reports = filteredImage.getOrderedParticleAnalysisReports();

for(int i = 0; i <reports.length +1;i++){

System.out.println(reports[i].center_mass_y);

if(reports[i].center_mass_y>centerdown){

increaseShooterAngle();

Timer.delay(.1);

shooterAngle.set(Relay.Value.kOff);

}else if(reports[i].center_mass_y<centerup){

decreaseShooterAngle();

Timer.delay(.1);

shooterAngle.set(Relay.Value.kOff);
}else{

System.out.println("Your Good");

}
}



}catch(Exception ex){
System.out.println("Failed");
System.out.println(ex);
//java.lang.ArrayIndexOutOfBoundsException: on [edu.wpi.first.wpilibj.image.ParticleAnalysisReport of length 0 with index 0

}finally{

}

try{

filteredImage.free();

convexHullImage.free();

bigObjectsImage.free();

thresholdImage.free();

image.free();

}catch(NIVisionException ex){

}
}

private void increaseShooterAngle() {


if(!shootermax.get()){

shooterAngle.set(Relay.Value.kForward);

}else{

shooterAngle.set(Relay.Value.kOff);

System.out.println("At Maximum Angle");

}

}

private void decreaseShooterAngle() {


if(!shootermin.get()){

shooterAngle.set(Relay.Value.kReverse);

}else{

shooterAngle.set(Relay.Value.kOff);

System.out.println("At Minnimum Angle");

}
}

private void readLimitswiches() {

if(shootermin.get()){
System.out.println("Shooter Min is true");
}else{
System.out.println("Shooter Min is False");
}
}





}

[F22Rapture]

Not a response but just a suggestion: use [code] tags around your code so that it's scrollable to improve the readability of the thread. They also leave the formatting intact.

[Johnbot]

Originally Posted by 2472Programer

We got a somewhat tracking code working today, but i would like the people of chief delphi to take a look at it. I've posed errors in the code (as comments) that my mentor and I can't figure out. PLZ HELP!

package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.Relay;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.camera.AxisCamera;
import edu.wpi.first.wpilibj.image.BinaryImage;
import edu.wpi.first.wpilibj.image.ColorImage;
import edu.wpi.first.wpilibj.image.CriteriaCollection;
import edu.wpi.first.wpilibj.image.NIVision;
import edu.wpi.first.wpilibj.image.NIVisionException;
import edu.wpi.first.wpilibj.image.ParticleAnalysisReport;





public class RobotTemplate extends IterativeRobot {
    
    Joystick leftStick = new Joystick(1);
    
    Joystick rightStick = new Joystick(2);
    
    Joystick joypad = new Joystick(3);
    
    RobotDrive tank = new RobotDrive(3,1,4,2);
    
    Victor shooter = new Victor(5);
    
    Relay ballLift = new Relay(1);
    
    Relay ballHolder = new Relay(2);
    
    Relay shooterAngle = new Relay(3);
    
    AxisCamera camera = AxisCamera.getInstance();
    
    CriteriaCollection cc;
    
    public int centerup = 172;
    
    public int centerdown = 152;
    
    DigitalInput shootermin = new DigitalInput(1);
    
    
    DigitalInput shootermax = new DigitalInput(2);

    public void robotInit() {
        
        cc = new CriteriaCollection();
        
        cc.addCriteria(NIVision.MeasurementType.IMAQ_MT_AREA, 30, 400, false);
        
        cc.addCriteria(NIVision.MeasurementType.IMAQ_MT_AREA, 40, 400, false);
        
        
        
        

    }

    /**
     * This function is called periodically during autonomous
     */
    public void autonomousPeriodic() {
//
    }

    /**
     * This function is called periodically during operator control
     */
    public void teleopPeriodic() {
        
        tank.tankDrive(leftStick, rightStick);
        
        if(joypad.getRawButton(5)){
            
            ballLift.set(Relay.Value.kForward);
            
            ballHolder.set(Relay.Value.kForward);
            
        }else if(joypad.getRawButton(7)){
            
            ballLift.set(Relay.Value.kReverse);
            
            ballHolder.set(Relay.Value.kReverse);
            
        }else{
            
            ballLift.set(Relay.Value.kOff);
            
            ballHolder.set(Relay.Value.kOff);
        }
        
        if(joypad.getRawButton(4)){
            
            shooter.set(1);
            
        }else if(joypad.getRawButton(3)){
            
            shooter.set(-1);
            
        }else{
            
            
            
            shooter.set(0);
            
        }
        
        if(joypad.getRawButton(6)){
            
            increaseShooterAngle();
            
        }else if(joypad.getRawButton(8)){
                      
            decreaseShooterAngle();
            
        }else{
            
            shooterAngle.set(Relay.Value.kOff);
            
        }
        
        if(joypad.getRawButton(2)){
            
            System.out.println("Button 2 Pressed");
            
            centerShooter();
            
        }
        
        if(joypad.getRawButton(1)){
            
            readLimitswiches();
            
        }
        
        
        
    }
    
    /**
     * This function is called periodically during test mode
     */
    public void testPeriodic() {
    
    }
    
  private void centerShooter() {
        
        ColorImage image = null;
        
        BinaryImage thresholdImage = null;
        
        BinaryImage bigObjectsImage = null;
        
        BinaryImage convexHullImage = null;
        
        BinaryImage filteredImage = null;
        
        try{
            
            image = camera.getImage();
            
            thresholdImage = image.thresholdRGB(0, 45, 25, 225, 0, 45);
            
            bigObjectsImage = thresholdImage.removeSmallObjects(false, 2);
            
            convexHullImage =  bigObjectsImage.convexHull(false);
            
            filteredImage = convexHullImage.particleFilter(cc);
            
            ParticleAnalysisReport[] reports = filteredImage.getOrderedParticleAnalysisReports();
            
            for(int i = 0; i <reports.length +1;i++){
                
                System.out.println(reports[i].center_mass_y);
            
                if(reports[i].center_mass_y>centerdown){
                   
                    increaseShooterAngle();
                    
                    Timer.delay(.1);
                    
                    shooterAngle.set(Relay.Value.kOff);
                    
                }else if(reports[i].center_mass_y<centerup){
                    
                    decreaseShooterAngle();
                    
                    Timer.delay(.1);
                    
                    shooterAngle.set(Relay.Value.kOff);
                }else{
                    
                    System.out.println("Your Good");
                    
                }
            }
            
            
                     
        }catch(Exception ex){
            System.out.println("Failed");
            System.out.println(ex);
            //java.lang.ArrayIndexOutOfBoundsException: on [edu.wpi.first.wpilibj.image.ParticleAnalysisReport of length 0 with index 0
            
        }finally{
            
        }
        
        try{
            
            filteredImage.free();
            
            convexHullImage.free();
            
            bigObjectsImage.free();
            
            thresholdImage.free();
            
            image.free();
            
        }catch(NIVisionException ex){
            
        }
    }

    private void increaseShooterAngle() {
        
        
        if(!shootermax.get()){
            
            shooterAngle.set(Relay.Value.kForward);
            
        }else{
            
            shooterAngle.set(Relay.Value.kOff);
            
            System.out.println("At Maximum Angle");
            
        }
        
    }

    private void decreaseShooterAngle() {
        
        
        if(!shootermin.get()){
            
            shooterAngle.set(Relay.Value.kReverse);
            
        }else{
            
            shooterAngle.set(Relay.Value.kOff);
            
            System.out.println("At Minnimum Angle");
            
        }
    }

    private void readLimitswiches() {
        
        if(shootermin.get()){
            System.out.println("Shooter Min is true");
        }else{
            System.out.println("Shooter Min is False");  
        }
    }

}

When you get your ArrayIndexOutOfBoundsException it is because you currently don't have any targets to aim for, and you are accessing an array past its final index. Before you do anything with the reports array, make sure it is not empty/have a length of zero.