//////////////////////////////////////////////////////////////////
// Super I-Cybie CMUCam Demo
// by Danh Trinh 2002
//
// A simple program for tracking a pink ball. Super I-Cybie will
// either backup up, turn left or turn right depending on the ball
// position. The user essentially positions the ball in order to
// maneuver Super I-Cybie to a desired position.
//
// A future version would allow I-Cybie to seek and discover the ball.
//
// Illustrates the basic code needed to interface with a CMUCam.
//
// Note: the Super I-Cybie serial.c library was modified for 9600
// baud

#include "sicl.h" // Super ICybie Library

#include "sicl_adv.h" // advanced

// functions
void init_cmucam();
void wait_for_colon();
void get_ball_position();

// 11 byte array for holding the CMUCam raw data packet
byte array[11];

// CMUCam raw data defines
#define mx array[2]
#define my array[3]
#define x1 array[4]
#define y1 array[5]
#define x2 array[6]
#define y2 array[7]
#define pixels array[8]
#define confidence array[9]

// define motions
#define motion_backup_straight sicl_perform_motion(1001)
#define motion_forward_left sicl_perform_motion(1002)
#define motion_forward_right sicl_perform_motion(1003)

//////////////////////////////////////////////////////////////////////////////
//
// main

void main()
{
sicl_play_sound(4);        //3 barks on power up
sicl_sleep(500);
sicl_play_sound(4);
sicl_sleep(500);
sicl_play_sound(4);

init_cmucam();             //initialize CMUCam

while (1)                  //loop forever
  {
  sicl_sleep(100);         //toggle eye color while checking
  sicl_show_eyes_yellow(); // for a ball
  get_ball_position();

  sicl_sleep(100);
  sicl_show_eyes_red();
  get_ball_position();
  }
}

/////////////////////////////////////////////////////////////////
//
// initialize CMUCam camera

void init_cmucam()
{
sicl_show_eyes_red();               //eyes red
sicl_sleep(1000);                   //wait
printf("rs\r");                     //reset CMUCam
wait_for_colon();                   //wait for colon

sicl_show_eyes_yellow();            //eyes yellow
sicl_sleep(1000);                   //wait
printf("pm 1\r");                   //enable poll mode
wait_for_colon();                   //wait for colon

sicl_show_eyes_red();               //eyes red
sicl_sleep(1000);                   //wait
printf("mm 1\r");                   //enable middle mass mode
wait_for_colon();                   //wait for colon

sicl_show_eyes_yellow();            //eyes yellow
sicl_sleep(1000);                   //wait
printf("tc 239 241 15 17 15 17\r"); //set RGB track color for pink ball
wait_for_colon();                   //wait for colon

sicl_show_eyes_red();               //eyes red
sicl_sleep(1000);                   //wait
printf("rm 3\r");                   //enable raw transfer mode
wait_for_colon();                   //wait for colon

}

/////////////////////////////////////////////////////////////
//
// wait for colon to be returned by CMUCam
// to signal end of data

void wait_for_colon()
{
do{}while(sicl_get_serial_byte() != ':');
}

/////////////////////////////////////////////////////////////
//
// Get 11 bytes of raw data returned by the cmucam
//
// 11 bytes are as follows -
// array position --> 0 1 2 3 4 5 6 7 8 9 10
// value --> 255 M mx my x1 y1 x2 y2 pixels confidence :
//
// 255 -> beginning of packet header for raw transfer mode
// M -> type m packet
// mx -> middle mass x position
// my -> middle mass y position
// x1 -> x1 coordinate window position
// y1 -> y1 coord window position
// x2 -> x2 coord window position
// y2 -> y2 coord window position
// pixels -> mass size
// confidence -> confidence
// : -> colon returned by CMUCam afer issuing the "tc" track color command
//

void get_ball_position()
{
int j;

printf("tc\r");        //start tracking, transmit "tc" and carriage return

for (j=0; j < 11; j++) //get 11 bytes of raw data
{
array[j] = sicl_get_serial_byte();
}

if (array[10] != ':')  //cry if last element is not ':'
{
sicl_play_sound(9);
}

else if (pixels > 100) //backup if ball too close
{
motion_backup_straight;
}

else if (mx > 60 && pixels < 60) //turn right if ball is to right and not too close
{
motion_forward_right;
}

else if (mx > 0 && mx < 40 && pixels < 60) //turn left if ball is to left and not too close
{
motion_forward_left;
}

}