Camera programming progress

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.

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.

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

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* = targets*;
                    }
                } 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.**

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

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.

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