Camera programming progress

[nathanww]

Team 1678 started seriously integrating our kicking and tracking systems today and ran into some pretty serious speed issues which we're now optimizing. I was wondering how many people actually have a high-speed, working camera tracking system on their robot.

[TPNigl]

We've been able to move the servos and get images. We're now trying to get the tracking itself to work, but we're having problems. We posted a thread about it too.

http://www.chiefdelphi.com/forums/sh...ad.php?t=83015

If anyone who has gotten their camera to track could help, it would mean a lot.

[Patrick Chiang]

We're using Java and it's working for us.
We ripped the Target class of the example code, but here's our Camera class:

package pronto;

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


/**
 * General parameters for the camera.
 * @author Team Pronto Programmers: Patrick Chiang, Nicholas Escalona, Cody Ryu
 */
public class Camera {
    static final int SENSITIVITY_CAMERA_H = 10;
    static final int SENSITIVITY_CAMERA_V = 3;

    int targetPosition;
    double kScoreThreshold = 0.01;
    
    Timer timer;
    Servo cameraHServo,cameraVServo;
    AxisCamera cam;
    /**
     * Class designated for initializing necessary components for the physical movement of the camera
     * @param horizontalServo Servo designated for turning the camera horizontally
     * @param verticalServo Servo designated for turning the camera vertically
     * @param camera Camera designated for seeing things
     */
    public Camera(Servo horizontalServo, Servo verticalServo, AxisCamera camera){
        super();
        cameraHServo = horizontalServo;
        cameraVServo = verticalServo;
        timer = new Timer();
        cam = camera;
    }
    /**
     * Class used to specify a joystick to control the camera
     * @param cameraStick Joystick designated to control the camera
     */
    public void manual(Joystick cameraStick){
        if(cameraStick.getRawButton(1)){
            track();
            aim();
        }
        if(cameraStick.getRawButton(10)){
            cameraHServo.set(0.5);
            cameraVServo.set(0.5);
        } else{
            move(cameraStick);
        }
    }

    /**
     * Class for allowing constantly centering the ball in the camera's field of vision.
     * @param maxAngle Maximum allowed 
     * @param targetNum
     */
    public int track(){
        // System.out.println("Target Angle: " + cameraHServo.getRaw()); //TODO uncomment
        // based on code from CircleTrackerDemo

        try {
            if (cam.freshImage()) {
                ColorImage image = cam.getImage();
                Thread.yield();
                Target[] targets = Target.findCircularTargets(image);
                Thread.yield();
                image.free();
                if (targets.length == 0 || targets[0].m_score < kScoreThreshold) {
                    // targetNotFoundCount++; // DEBUG
                    // System.out.println("No target found" + targetNotFoundCount + "times"); // DEBUG
                    Target[] newTargets = new Target[targets.length + 1];
                    newTargets[0] = new Target();
                    newTargets[0].m_majorRadius = 0;
                    newTargets[0].m_minorRadius = 0;
                    newTargets[0].m_score = 0;
                    for (int i = 0; i < targets.length; i++) {
                        newTargets[i + 1] = targets[i];
                    }
                } else {
                    // System.out.println(targets[0]); //TODO uncomment
                    // System.out.println("Target Angle: " + targets[targetNum].getHorizontalAngle()); //TODO uncomment
                    // targetFoundCount++; // DEBUG
                    // System.out.println("Num targets found" + targetFoundCount); // DEBUG
                    if(Math.abs(targets[0].getHorizontalAngle())<500){
                        targetPosition = (int)targets[0].getHorizontalAngle();
                        // System.out.println(targets[0].toString());
                    }
                }                
            }
        } catch (NIVisionException e) {
            e.printStackTrace();
        } catch (AxisCameraException e) {
            e.printStackTrace();
        }
        return targetPosition;
    }

    public void aim(){
       if(Utils.isBetween(targetPosition,3,252)){
            cameraHServo.setRaw(cameraHServo.getRaw() + targetPosition);
        } else if((cameraHServo.getRaw() + targetPosition)<3){
            cameraHServo.setRaw(cameraHServo.getRaw() + 3);
        } else if((cameraHServo.getRaw() + targetPosition)>252){
            cameraHServo.setRaw(252);
        }
    }
    /**
     * Allows for manual operation of the camera's movement
     * @param shooterStick Control stick for the camera. Controlled by Buttons 2-4
     */
    public void move(Joystick shooterStick){
        
        // camera control
        if (shooterStick.getRawButton(4)) { // left
            cameraHServo.setRaw(cameraHServo.getRaw() - SENSITIVITY_CAMERA_H);
        }
        if (shooterStick.getRawButton(5)) { // right
            cameraHServo.setRaw(cameraHServo.getRaw() + SENSITIVITY_CAMERA_H);
        }
        if (shooterStick.getRawButton(2)) { // down
            cameraVServo.setRaw(cameraVServo.getRaw() - SENSITIVITY_CAMERA_V);
        }
        if (shooterStick.getRawButton(3)) { // up
            cameraVServo.setRaw(cameraVServo.getRaw() + SENSITIVITY_CAMERA_V);
        }
    }
}

Use track() and aim() together to make the robot's Servo "look" at the target. It works for side to side (horizontal) tracking, but no vertical yet.

[bronxbomber92]

What do the magical constants 3 and 252 mean in the aim method?

[Patrick Chiang]

Those prevent the servos from going below 3 or above 255. In theory, they go from 0-255, but they're there just in case.

[bronxbomber92]

Hmm, ok. I'm just surprised that works since you're treating it like an angle (since targetPosition is supposedly an angle).