fisheyev3.c

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

#include "cerebv41.h" // load declarations
#include "cerebv41.c" // load useful cerebellum functions
// mr fish eye (fish sculpture with cmucam)
// 

////////////////////
// 

// servo numbers and values are as follows: 

// sensors and servoes on this sculpture
#define IR 1    // the Sharp rangefinder
#define PAN 5   // pan servo in digital 5
#define TILT 4  // tilt servo in digital 4
/* on PAN servo:
small pos is pan left; large pos is pan right
   on TILT servo:
small pos is tilt up; large pos is tilt down
   other sensors
     adc1 is the ir distance sensor (IR)   */
// below are the 3 main states of this fisheye system //
#define ASLEEP 0 // facing slightly down in this mode
#define SEARCHING 1  // looking for a trackable
#define TRACKING 2 // tracking an object

// defines thresholds for the IR rangefinder //
#define CLOSE 70  // distance measure for close person
#define MED 20    // distance threshold for person at all

// defines the 3 states the CMUcam can be in here. we do NOT use poll mode here
// for speed, all is done streaming
#define IDLE 0     // this and below are camera states, camstate
#define GMSTREAM 1
#define TRACKSTREAM 2

// 4 types of cmucam 'searches' are done as defined below //
#define FINDRED 0
#define FINDGREEN 1
#define FINDBLUE 2
#define FINDWHITE 3

char mystate;  // variable assigned one of above states: asleep, searching,tracking
char tiltpos;  //the last commanded tilt servo pos
char panpos;   // last commanded pan servo pos
char cyclespd; // how fast green led blinks (diagnostic transparency)
char timer;    // counter used to measure timeouts
char timer2;   // 2nd counter used with above for longer timeouts
char sleeptmr; // used to go from awake, searching to asleep every so often
char sleeptmr2;
char trackcnt; // used to count successful tracking series
char buf[10];  // buffer for data from CMUcam
char rmean; char gmean; char bmean; // get_mean cmucam data
char rdiff; // derived from rmean this time and last time
char ctrx; char ctry; char conf;    // tracking cmucam data
char camstate; // used to track camera's state according to define's above
char lastdist; // last value distance sensor measured
char opporcnt; // used to keep track of which opportunistic
         // color searches have been done by cmucam (red, green, blue, white)
                
/* to do:
    update robot; observe and increment
    */

// smoothly go from current position to a new position       
void servoTo(char servoNum, char finalpos, char mswait, char stepsize) {
     char curpos;
     if (servoNum == TILT) curpos = tiltpos;
     else curpos = panpos;
     for (; curpos < finalpos; curpos=curpos+stepsize) {
         servo_pos[servoNum] = curpos;
         delay_ms(mswait);
        }  
     for (; curpos > finalpos; curpos=curpos-stepsize) {
          servo_pos[servoNum] = curpos;
          delay_ms(mswait);
         }
     if (servoNum == TILT) tiltpos = curpos; else panpos = curpos;  
     servo_pos[servoNum] = 0;
}  // servoTo()

// center the fisheye head. note that servoTo also disables both servoes.
void head_ctr() {
     servoTo(PAN, 127, 15, 2);
     servoTo(TILT, 127, 15, 2);
} // head_ctr() //

// sets lastdist global to current rangefinder value
void personck()
{
    lastdist = adc_read(IR);
} // end personck() //
  
// simple scripted nodding of the head.
void nod()
  {
       servoTo(TILT, 200, 15, 2);
       servoTo(TILT, 60, 15, 2);
       servoTo(TILT, 127, 15, 2);         
  } // end nod()
  
// simple scripted shaking (no).
void shake()
  {
       servoTo(PAN, 110, 15, 2);
       servoTo(PAN, 150, 15, 2);
       servoTo(PAN, 127, 15, 2);         
  } // end shake()
   
// take tilt and approximately point at face
// of a 6 foot person given rangefinder reading
void tilttodist() {
  char newpos;
  newpos = lastdist; if (newpos > 60) newpos = 60;
  newpos = newpos * 2;
  newpos = 170 - newpos;
  servo_pos[TILT] = newpos;
  tiltpos = newpos;
} // end tilttodist() //

// set camera to autogain and auto white balance on, rgb
void autogainon() {
    ser_putstring("cr 18 44 19 33\r"); 
    while (ser_rcv() != ':') ; // read all reply characters
} // autogainon(); //

// freeze camera gain and wb before tracking
void clamp() {
    ser_putstring("cr 18 40 19 32\r"); 
    while (ser_rcv() != ':') ;
} // clamp() //

// set up camera for use; raw mode on; noise filter on;
// poll mode off; full window mode; etc.
// exits with green led on to signify success
void setupCam() {
    char inc;
    delay_ms(200);
    clear_bit(PORTB, GREEN);
     inc = 'B';
     while (inc != 'A') {  // raw mode on //
      ser_putstring("rm 1\r"); inc = ser_rcv();
      while (ser_rcv() != ':') ;
     }
    autogainon();      
     inc = 'B';
     while (inc != 'A') {  //  noise filter on //
      ser_putstring("nf 1\r"); inc = ser_rcv();
      while (ser_rcv() != ':') ;
     } 
     inc = 'B';
     while (inc != 'A') {  //  poll mode off //
      ser_putstring("pm 0\r"); inc = ser_rcv();
      while (ser_rcv() != ':') ;
     } 
     inc = 'B';
     while (inc != 'A') {  // full window mode //
      ser_putstring("sw\r"); inc = ser_rcv();
      while (ser_rcv() != ':') ;
     } 
    set_bit(PORTB, GREEN);
    camstate = IDLE;    
} // end setupCam() //

// the next two assume we are in streaming mode
// and parse in for tracking and get-mean data respectively
// setting global variables with the resulting data we use
void track_parse() {
 char bufp;
  while (ser_rcv() != 255) ;
  ser_rcv();
  for (bufp = 0; bufp < 8; bufp++) {
    buf[bufp] = ser_rcv();
  }
  conf = buf[7];
  ctrx = buf[0];
  ctry = buf[1];
} // track_parse() //

void gm_parse() {
 char bufp;
 char rlast;
 while (ser_rcv() != 255) ;
 ser_rcv();  // the 'S' packet type label //
 for (bufp= 0; bufp < 6; bufp++) {
   buf[bufp] = ser_rcv();
 }
 rlast = rmean;
 rmean = buf[0];
 gmean = buf[1];
 bmean = buf[2];
 if (rmean > rlast) rdiff = rmean - rlast;
 else rdiff = rlast - rmean;
} // gm_parse() //

// starts Track Window (starts track color using
// whatever is in the center of the camera fov)
void start_tw() {
    clamp();
    ser_putstring("tw\r");
    camstate = TRACKSTREAM;
}

// call track color with specified parms
void start_tc(int rmin, int rmax, int gmin, int gmax, int bmin, int bmax) {
     clamp();
     ser_putstring("tc ");
     ser_writechar(rmin);
     ser_tx(' ');
     ser_writechar(rmax);
     ser_tx(' ');
     ser_writechar(gmin);
     ser_tx(' ');
     ser_writechar(gmax);
     ser_tx(' ');
     ser_writechar(bmin);
     ser_tx(' ');
     ser_writechar(bmax);
     ser_tx('\r');
     camstate = TRACKSTREAM;