fisheyev3.c

example of communication between the Cerebellum 16f877 pic board and the CMUcam

}

// we try one of 4 'colors' and go through them one at a time each
// time this is called.
void start_opportu() {
    opporcnt = opporcnt+1;
    opporcnt = opporcnt%4;
    if (opporcnt == FINDRED)   start_tc(80, 240, 5, 40, 5, 40);
    if (opporcnt == FINDGREEN) start_tc(5, 60, 80, 240, 5, 40);
    if (opporcnt == FINDBLUE)  start_tc(5, 60, 5, 40, 80, 240);
    if (opporcnt == FINDWHITE) start_tc(180, 240, 150, 240, 100, 240);   
} // start_opportu() //

// note that for get-mean we don't clamp camera
void start_gm() {
    ser_putstring("gm\r");
    camstate = GMSTREAM;
}

// stops any camera data stream and unclamps camear parms
void stop_stream() {
    ser_putstring("\r");
    delay_ms(50);
    while (ser_rcv_nb() != 0) ;
    autogainon();
    camstate = IDLE;
}

// take whatever current data is back from tracking stream
// and use it to servo the fisheye head to center the object
// visually. Because of CMUcam's misalignment the correct
// goal is not x=40 and y=70 but instead
// x=55 and y=45. way off on misalignment!
// note that whenever possible the servoes are disabled
void head_track() {
   char tiltchange;
   char panchange;
   tiltchange = 0;
   panchange = 0;
   if (ctrx < 50) panpos--;
   if (ctrx < 53) { panpos--; panchange = 1; }
   if (ctrx > 58) { panpos++; panchange = 1; }
   if (ctrx > 60) panpos++;
   if (panpos < 10) { panpos = 10; panchange = 0; }
   if (panpos > 240) { panpos = 240; panchange = 0; }
   if (panchange == 1) servo_pos[PAN] = panpos;
     else servo_pos[PAN] = 0;

   if (ctry < 30) tiltpos++;
   if (ctry < 40) { tiltpos++; tiltchange = 1; }
   if (ctry > 50) { tiltpos--; tiltchange = 1; }
   if (ctry > 60) tiltpos--;
   if (tiltpos < 10) {tiltpos = 10; tiltchange=0; }
   if (tiltpos > 240) {tiltpos = 240; tiltchange=0; } 
   if (tiltchange == 1) servo_pos[TILT] = tiltpos;                    
    else servo_pos[TILT] = 0;
} // head_track() //

// if human is really close, shake. else, modify tilt to 
// point approx. toward head of human.
void ranger_action()
{
        timer = timer - 1;
        if (lastdist > CLOSE) { // human really close...
            shake();
            lastdist = 0;
        } 
        if (lastdist > MED) {  // human medium close
            tilttodist(); trackcnt = trackcnt+1; timer=200;        
        } // end if (lastdist > MED) //
        if (timer == 175) {  // no human for 25 steps, re-center head
            head_ctr(); trackcnt=0;
        }
} // ranger_action()

// main loop for fisheye
// assuming that CMUcam is already set up and in IDLE
// controls state transition between asleep, searching and tracking
// basic idea is that the system is usually searching. in searching
// mode, human triggering rangefinder causes movement. if human stays
// there, then a track window attempts to start tracking the human.
// If no human at rangefinder, then the system in SEARCHING state tries
// to find something red, green, blue or white to track and if it finds it
// tracks it.  No matter what is being tracked (successfully or miserably),
// the system times out after a minute or two.
// TRACKING is the state during tracking. Finally, after a very long timeout,
// more like 15 minutes, the system goes to 'sleep.'  Once ASLEEP, system
// is awakened either by sudden light changes or by rangefinder triggering.
void fisheyev3(void)
{
 char i;
 cyclespd = 50; lastdist = 0; opporcnt = 0;
 mystate = ASLEEP; timer = 20;
 start_gm();
    
 while (1) {   
   set_bit(PORTB, GREEN);  // green LED on

   if (mystate == ASLEEP) {
        if (timer > 0) timer = timer-1;
        personck(); // update range sensor
        gm_parse(); // peel data from get mean data stream
        if ((lastdist > MED) || (rdiff > 30)) { // close person or light chnges
          if (timer == 0) { // forces remaining in ASLEEP for awhile
            mystate = SEARCHING; stop_stream();
            head_ctr();
            cyclespd = 10; timer = 200; // blinks LED fast, sets up timers for
                                        // use in SEARCHING mode. 
            sleeptmr = 200; sleeptmr2 = 5; trackcnt=0; 
          } // end if (timer == 0) //         
        } 
   } else if (mystate == SEARCHING) {
   // searching stuff //
        personck(); // update range sensor
        sleeptmr = sleeptmr - 1;
        if (sleeptmr == 0) {sleeptmr=200; sleeptmr2=sleeptmr2-1;}
        if (sleeptmr2 == 0) { // sleeping countdown is done. go ASLEEP
            mystate = ASLEEP;
            head_ctr();       // center the head,
            timer = 20; // forces remaining ASLEEP for 20 cycles 
            servoTo(TILT, 170, 15, 1); // then ppoint slightly down
            cyclespd = 50; // slower green LED blink when sleeping
            start_gm(); lastdist = 0; // begin GetMean stream
            // note that next two if's must check to see if we just went
            // to ASLEEP and if so not to do anything. if..else works in
            // this compiler but not if..else if..else if multiple times!
        } // endif (sleeptimer == 0)
        
        // if we've been rangefinder-staring at something for 20 cycles,
        // then go ahead and run TrackWindow on it.
        if ((trackcnt == 20) && (mystate == SEARCHING)) { 
            mystate = TRACKING;
            set_bit(PORTB, YELLOW); // turn on yellow LED whenever tracking
            trackcnt = 0; cyclespd = 0;
            timer = 200; timer2 = 0; // Because timer is 200, will attempt
                                     // to track object for at least 200 cycles
            start_tw();
        }
        ranger_action(); // twitch as appropriate if rangefinder's triggered
        
        // if we haven't seen anything on the rangefinder in a while,
        // then go ahead and try to choose some color opportunistically
        // and see if it happens to track (red, green, blue, white).
        // note that timer=10 here, so patience is low. Will only try for
        // 10 cycles, or about 0.5 seconds, if there is no confidence during
        // the tracking attempt
        if ((trackcnt == 0) && (mystate == SEARCHING)) {
            mystate = TRACKING;
            set_bit(PORTB, YELLOW);
            trackcnt = 0; cyclespd = 0;
            timer = 10; timer2 = 0;
            start_opportu(); // this command actually calls TrackColor with
               // varying parameters
        }                               
   } else if (mystate == TRACKING) {
   // tracking stuff //
        track_parse(); // peel off data from CMUcam on object position
        if (conf > 5) { // if object is seen, servo the head
            timer = 200;
            head_track();
            trackcnt = trackcnt+1; // even if object is seen, count up
                                // using trackcnt and timer2.  when timer2
                                // hits a limit we will timeout and stop
                                // tracking anyway
            if (trackcnt == 200) {
              timer2 = timer2 + 1;
              trackcnt = 0;
            }
        } else { // this is the case when we have no object with confidence
            timer = timer - 1; // countdown our "patience" timer
            servo_pos[PAN] = 0; // power off the servoes to save energy
            servo_pos[TILT]=0;
        } // endelse (confidence is low //
        if ((timer == 0) || (timer2 == 5)) { // both timeouts cause a 
                                               // return to SEARCHING state
                 mystate = SEARCHING;
                 trackcnt = 0; timer=200;
                 clear_bit(PORTB, YELLOW); // use yellow LED only when tracking
                 cyclespd = 10; stop_stream();
                 if (timer2 == 5) head_ctr();
        } // if (timer == 0... // 
   } // endelse (mystate == TRACKING)  
        
   delay_ms( 5 );
   clear_bit(PORTB, GREEN);
   
   // variable cycle time
   for (i = 0; i < cyclespd; i++) {
    delay_ms( 10 );
   }
 } // end while

} // end fisheyev1() //

void main(void)
{
    init_cerebellum(); // configure cerebellum
    ser_init(SER_115200); // start up serial port handling
    //ser_putstring("\n\rWelcome to Cerebellum!  Mr. Fish Eye V3.0\n\r");
    servo_init(); // start up servo control
    servo_state = 1;
    set_bit(INTCON, GIE); // start up interrupts- required for servo control

    ADCON1 = 0; // initialize analog-digital converter //

    //pwm_init(); // initialize pwm motor output
    
    setupCam(); // autogain on, etc.
    // now move head to show alive //
    head_ctr();
    nod(); shake();

    // now start fisheye // 
    fisheyev3();
    
    while (1) ;  // without this line, if and when main completes,
                    // it starts AGAIN!!!! //
     
}