cmucam.java

控制移到机器人的例子程序

import java.lang.*;
public class CMUcam extends java.lang.Object
{
    TypeA dataA;
    TypeC dataC;
    TypeM dataM;
    serialPort myPort;
    public int[][] frameMem = new int[100][500];
    // remember the frame is actually 80 x 143, but has r, g and b cells
    // packed, so it is actually rgbrgbrgb...
    
    /* Here is the CMUcam constructor. Use this to instantiate a camera */ 
    public CMUcam() {
            dataA = new TypeA();
            dataC = new TypeC();
            dataM = new TypeM();
            myPort = new serialPort();
    } // CMUcam() constructor
    
    /* Port should be the COMX number.
       The rate should be the baud rate for CMUcam, which is normally
       115200 which is rate = 5.  Note that if this
       fails to open the serial port you get a false return value
       so you should DEFINITELY check for this in your code */
    public boolean initCamera(int port, int rate) {
        int error;
        error = myPort.openSerial(port, rate);
        if (error != 0) {
            System.err.println("Error " + error + " camera openSerial()");
            return false;
        } else {
            myPort.setReadTimeout(500);      
            System.out.println("camera serial successfully opened!"); 
            return true;
        }                
    } // initCamera()
    
    /* ------------------------------ */
    /* CAMERA INTERFACE COMMANDS      */
    /*  -this is what you use!        */
    /* ------------------------------ */
    
    // returns true if the camera acknowledges messages //        
    public boolean checkCamera() {
        this.sendLine(" ");
        if (this.detectAck()) {
            return this.toColon();
        } else {this.toColon(); return false;}
    } // checkCamera() //

    /* For our class, this sets RGB mode, auto white balance on, 
       autogain on, mass mode enabled, poll mode enabled (IMPORTANT),
       noise filter on, full window size */
    // WARNING: if this returns false, call it again! //
    public boolean resetCamera() {
        if (!this.checkCamera()) return false; 
        // rgb mode, auto gain and white balance on //
		if (!this.commandNoResponse("cr 18 44 19 33")) return false;
		// mass mode on //
        if (!this.commandNoResponse("mm 1")) return false;
        // poll mode on //
        if (!this.commandNoResponse("pm 1")) return false;
        // noise filter on //
		if (!this.commandNoResponse("nf 1")) return false;
		// set full window size (no subwindowing) //
		if (!this.commandNoResponse("sw")) return false;
		return true;         
    } // resetCamera() //
    
    // Sets the window for all functions. Remember, maximum
    // width is 80 and height is 143
    // so full screen is setWindow(1,1,80,143) //
    public boolean setWindow(int x1, int y1, int x2, int y2) {
        return this.commandNoResponse("sw " + x1 + " " + y1 + " " +
                                     x2 + " " + y2);
    } // setWindow() //
    
    // returns a string from the camera responding to get version //    
    public String getVersion() {
        this.toColon();
        return this.commandStringResponse("gv");        
    } // getVersion() //    
    
    // returns true if dataA is updated with new statistics from camera
    // assumes poll mode is on!!!!! //
    public boolean getMean() {      
        return this.commandTypeA("gm", dataA);
    } // getMean() //
    
    // NOTE: trackColor assumes that MassMode is on! Otherwise, write
    // your own function to call commandTypeC instead.
    // returns true if DataM is updated by camera
    public boolean trackColor(int rmin, int rmax, int gmin, int gmax,
                              int bmin, int bmax) {
         return this.commandTypeM("tc " + rmin + " " + rmax + " " +
                                 gmin + " " + gmax + " " + bmin + " " +
                                 bmax, dataM);
    } // trackColor() //
    
    //This initializes frameMem then grabs a frame -many frames' columns
    // actually- and populates the part only set by setwindow
    // frameMem[i][j] is read as follows. i is the column number,
    // j is the rgb values of the rows, so frameMem[3][9], [3][10], [3][11]
    // would be the rgb values of pixel (3,3)
    // now the tricky thing. The origin for frameMem, [0][0], 
    // corresponds to the bottom left of the image I think //
    // tricky thing: the parameters are based on origin at top right
    // in 16x62 because camera is upside-down. Otherwise it's bottom left
    public boolean grabFrame(int winx1, int winy1, int winx2, int winy2) {
		int value, xiter, yiter, rgbcounter;
		int xstart = 80 - winx2; int ystart = 3*(143 - winy2);
		xiter = xstart; yiter = ystart; rgbcounter=0;		
		for (int i=0; i< 100; i++) 
		    for (int j=0; j<500; j++)
		        frameMem[i][j] = 0;	 // wipe frameMem clean	- black        
		this.sendLine("df");
		if (this.detectAck()) {
		    while (true) {
		        value = myPort.readByte();
		        if (rgbcounter == 3) {
		            rgbcounter = 0; yiter+=3;
		        }
		        if (value == 1) System.out.println("1");
		        else if (value == 2) {		            
		            System.out.println("2 " + xiter + " " + yiter);
		            xiter++; yiter = ystart; rgbcounter = 0;
		        } else { // note value!
		            frameMem[xiter][yiter+rgbcounter] = value;	
		            rgbcounter++;
		        }
		        
		        if (value == 3) { 
		            System.out.println("3"); 
		            this.toColon();		            
		            return true;
		        }
		        if (value == -1) {
		            System.out.println("ERROR: timeout during frame grab");
		            return false;
		        }
		    } // end while
		} else {
		    System.out.println("Error: Dumpframe did not get ACK");
		    return false;
		} // end else        
    } // grabFrame() //
        
    /* ------------------------------    */
    /* CAMERA INTERFACE HELPER COMMANDS  */
    /* ------------------------------    */
    
    // used for all commands that expect nothing but an ACK back
    // returns true if the command got acknowledged by the camera
    public boolean commandNoResponse(String cmdString) {
        /* CR, NF, PM, SW, MM  */
        this.sendLine(cmdString);
        if (this.detectAck()) {
            return this.toColon();
        } else { 
            this.toColon(); return false; }                    
    } // commandNoResponse() 
    
    // for the one command, GetVersion, that returns a string //
    public String commandStringResponse(String cmdString) {
        /* GV */
        String returnStr;
        this.sendLine(cmdString);
        if (this.detectAck()) {
            returnStr = this.readLine();
            this.toColon();
        } else { 
            returnStr = "no ACK!";
            this.toColon();
        }
        return returnStr;
    } // commandStringResponse() //
    
    // For any command that returns a type A packet //
    // That means GetMean basically //
    public boolean commandTypeA(String cmdString, TypeA packetA) {
        /* GM -- WARNING: we assume poll mode here!*/
        // packetA should already be instantiated  //
        String returnStr;
        boolean success;
        this.sendLine(cmdString);
        if (this.detectAck()) {
            returnStr = this.readLine();
            success = packetA.parseString(returnStr);            
            this.toColon();
            return success;   
        } else {
            this.toColon();
            return false;
        }
    } // commandTypeA() //
    
    // For any command that returns TypeC data //
    // That would be TrackColor when MassMode is off (which never happens) //
    public boolean commandTypeC(String cmdString, TypeC packetC) {
        // TC -- WARNING: we assume poll mode here! 
        // packetC should already be instantiated  //
        String returnStr;
        boolean success;
        this.sendLine(cmdString);
        if (this.detectAck()) {
            returnStr = this.readLine();
            success = packetC.parseString(returnStr);
            this.toColon();
            return success;   
        } else {
            this.toColon();
            return false;
        }
    } // commandTypeC() //
    
    // For any command that returns TypeM data //
    // That would be TrackColor when MassMode is on (always by default) //
    public boolean commandTypeM(String cmdString, TypeM packetM) {
        // TC -- WARNING: we assume poll mode here! 
        // packetM should already be instantiated  //
        String returnStr;
        boolean success;
        this.sendLine(cmdString);
        if (this.detectAck()) {
            returnStr = this.readLine();
            success = packetM.parseString(returnStr);
            this.toColon();
            return success;   
        } else {
            this.toColon();
            return false;
        }
    } // commandTypeM() //
        
    /* ------------------------------ */
    /* COMMUNICATION HELPER FUNCTIONS */
    /* ------------------------------ */
    
    // read bytes until you see the colon. True returned means
    // success. False means timeout. Camera not ready/working.
    public boolean toColon() {
        int value;
        while (true) {
            value = myPort.readByte();
            if (value == 58) {
                return true;
            } else if (value == -1) { return false; }
        } // while
    } // toColon
    
    // returns true if ACK read, else returns false if NCK
    // detected or other stuff seen (reads only one line)
    public boolean detectAck() {        
        String firstLine;
        firstLine = this.readLine();
        if (firstLine.startsWith("ACK",0)) return true; else return false;
    } // detectAck()
    
    public void clearBuffer() {
        int value;
        while (true) {
            value = myPort.readByte();
            if (value == -1) {
                break;
            } 
        } // while   
    } // clearBuffer //

    // Send a string as a series of bytes
    public int sendString(String s) {
        int i;
        byte[] bytes = s.getBytes();        
        for (i = 0; i < bytes.length; i++) {
            int error = myPort.sendByte(bytes[i]);
            if (error != 0) {
                break;
            }
        }        
        return i;
    } // sendString
    
    public int sendLine(String s) {
        return sendString(s + "\r");
    } // sendLine() //
    
    // returns a String and, in case of timeout, whatever partially it has
    public String readLine() {
        int value;
        String s = "";
        char[] c = new char[1];
        
        while (true) {
            value = myPort.readByte();
            if (value == 13 || value == -1) {
                if (value == -1) System.out.println("Camera: Timeout..");
                break;
            }            
            if (value != 13) {
                c[0] = (char) value;
                s += new String(c);
            }
        }        
        return s;
    } // readLine() //
    
} // CMUcam CLASS!