Chief Delphi - Camera Tracking Problems...

Code:
/*

 * To change this template, choose Tools | Templates

 * and open the template in the editor.

 */

package edu.wpi.first.wpilibj.templates.commands;







import edu.wpi.first.wpilibj.Timer;

import edu.wpi.first.wpilibj.camera.AxisCamera;

import edu.wpi.first.wpilibj.camera.AxisCameraException;

import edu.wpi.first.wpilibj.image.*;



public class CameraSeek extends CommandBase {

  

    AxisCamera camera;

    CriteriaCollection cc;    

     

     public CameraSeek() {

        

        // Use requires() here to declare subsystem dependencies

        // eg. requires(chassis);

    }



    // Called just before this Command runs the first time

    protected void initialize() {

        String camip = "10.29.77.11";

        camera = AxisCamera.getInstance(camip);

        cc = new CriteriaCollection();      // create the criteria for the particle filter

        cc.addCriteria(NIVision.MeasurementType.IMAQ_MT_BOUNDING_RECT_WIDTH, 20, 400, false);

        cc.addCriteria(NIVision.MeasurementType.IMAQ_MT_BOUNDING_RECT_HEIGHT, 20, 300, false); 

    try {

                ColorImage picture = camera.getImage();   //should read image from camera and then save it to a jpg file   

                picture.write("runthis.jpg");

                

                ColorImage trypicture = new RGBImage("runthis.jpg");    //workaround that should allow us to process the image

                

                //BinaryImage thresholdImage = trypicture.thresholdRGB(205, 255, 205, 255, 205, 255);   // keep only black objects

                BinaryImage thresholdImage = trypicture.thresholdHSL(126, 255, 6, 255, 201, 255);     //Saturation/Hue/Lumination filter

                BinaryImage bigObjectsImage = thresholdImage.removeSmallObjects(false, 2);  // remove small artifacts

                BinaryImage convexHullImage = bigObjectsImage.convexHull(false);          // fill in occluded rectangles

                BinaryImage filteredImage = convexHullImage.particleFilter(cc);           // find filled in rectangles                

                ParticleAnalysisReport[] reports = filteredImage.getOrderedParticleAnalysisReports();  // get list of results 

                

                //System.out.println("Step 2 Achieved-Threshold set, image filtered");

              

        

               

    for (int i = 0; i < reports.length; ++i) {                               // print results              

                ParticleAnalysisReport r = reports[i];

                System.out.println("Particle: " + i + ":  Center of mass x: " + r.center_mass_x_normalized);                   

                System.out.println("Particle: " + i + ": Center of mass y: " + r.center_mass_y_normalized);

                

                if (r.center_mass_x_normalized < 0.7) {

                  System.out.println("Found it, its to the left");

                  shootrun.Pan(r.center_mass_x_normalized);

                  

                    if (r.center_mass_x_normalized < 0.0) {

                  Timer.delay(-1.0 * r.center_mass_x_normalized); 

                  }

                   else if (r.center_mass_x_normalized > 0.0) {

                   Timer.delay(r.center_mass_x_normalized);     

                   }

                    shootrun.Pan(0);

                }  

                else if (r.center_mass_x_normalized < -0.9) {

                  System.out.println("Found it, its to the right");

                  shootrun.Pan(r.center_mass_x_normalized);

                   if (r.center_mass_x_normalized < 0.0) {

                  Timer.delay(-1.0 * r.center_mass_x_normalized); 

                  }

                   else if (r.center_mass_x_normalized > 0.0) {

                   Timer.delay(r.center_mass_x_normalized);     

                   }

                  shootrun.Pan(0);    

                }

                else 

                  shootrun.Pan(0);  

                  System.out.println("It be trippen, your right on top of it.");

                

    }

           shootrun.Pan(0);

            filteredImage.write("A-5-filteredpic.jpg");

            bigObjectsImage.write("A-3-bigimage.jpg");

            convexHullImage.write("A-4-convex.jpg");

            thresholdImage.write("A-2-threshold.jpg");

            picture.write("A-1-Fresh.jpg");

            trypicture.write("A-6-riu.jpg");

            //thresholdImagebw.write("A-7-Secondarythreshold");

            

                System.out.println(filteredImage.getNumberParticles() + "  " + Timer.getFPGATimestamp()); 

                filteredImage.free();

                convexHullImage.free();

                bigObjectsImage.free();

                thresholdImage.free();

                picture.free();

                trypicture.free();

                //thresholdImagebw.free();

                

                //System.out.println("Step 3 Achieved-Images Freed");   

                  

            } catch (AxisCameraException ex) {

            } catch (NIVisionException ex) {



            }

        

       

    }



    // Called repeatedly when this Command is scheduled to run

    protected void execute() {

        

    

    }



    // Make this return true when this Command no longer needs to run execute()

    protected boolean isFinished() {

        return false;

    }



    // Called once after isFinished returns true

    protected void end() {

    }



    // Called when another command which requires one or more of the same

    // subsystems is scheduled to run

    protected void interrupted() {

    }

}