fisheyev3.c

a zipped example of communication between the Cerebellum 16f877 pic board and the CMUcam. There are

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

// API provided for cereb //////////////////////
//
// call init_cerebellum() first in your main
// turn on an led with set_bit(PORTB, (GREEN|YELLOW)); turn off with clear_bit
// (PORTB & BTNn) is true if button n is depressed and false otherwise
// 
// call ser_init(SER_115200) to set up hardware serial port
// ser_tx(char c) sends a character out over the serial line
// ser_putstring(const char *text) writes a CONST string out the serial line
// ser_writechar(char c) writes out a character in decimal (good for debugging!)
// ser_rcv() is a blocking receive char call that returns a char
//        note that if the input buffer has overflowed it dumps the buffer
//        and continues waiting for a new character to return!
// ser_rcv_nb() is a nonblocking receive char call that returns a char
//          or if there is nothing waiting, returns a 0. Does NOT check for
//        frame error and overflow conditions!!!
// to save memory, if not using the serial port, don't call ser_init() and then
// don't use any of the above functions.
// 
// to use servoes, begin by calling servo_init() in your main.
// then before expecting servoes to get used make servo_state=1;
// servoes use interrupts, so make sure you're starting interrupts:
//        set_bit(INTCON, GIE);
// then command a servo by commanding servo_pos[n]=p. This will drive
// the servo on port Dn to position p.  If p is zero, it deactivates the servo.
// to save memory, if not using servoes don't call servo_init() and also
// go to cereb.c and find void interrupt(void) and comment out the line that
// calls do_servo().
//
// to use the analog-digital input lines, first in main you must initialize
// with the command: ADCON1=0;
// to read analog input channel ch, use adc_read(ch) which returns a char
// ch should be 0-7 inclusive. numbering on cerebellum goes from A0 as channel
// zero, left-to-right and top-to-bottom, so that E2 is analog channel 7
//
// to use the motor PWM outputs, first call pwm_init() in your code.
// if both commands are in there, call ser_init() first. The motor command
// is pwm_setvel8(n,dir,duty) where n=0 or 1 (0 means Motor1 on Cerebellum
// and 1 means Motor2 on Cerebellum). dir is direction (0 or 1) and duty is
// a char, with 0 meaning off and 255 meaning full-on.
// To save memory, if you are not using motor PWM then comment out pwm_init() in
// main().
// // 
////////////////////
// 

// 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();