#use "xbcserial.ic"

/* This file contais a number of useful files for running an iRobot Create from an XBC.
   There is a separate but similar library for the Roomba.
   Note, an XBC v3 with a XBC-Roomba cable is needed, along with IC 7.08 or higher.
   Written by David Miller, 4/07
*/


#define twopi 6.28318531
//CREATE_BUSY keeps any  create commands in other processes from interfering with one another.
#define CREATE_BUSY while(g_create_busy) defer(); g_create_busy = 1;
#define CREATE_FREE g_create_busy = 0;

int g_create_busy = 0;
int g_create_connected = 0;

// Create must be turned on and connected.  This routine establishes serial link
// Note that this routine should be called once, and must have been called before 
// any of the other create functions will work.
// Once this function is called, you cannot talk to the XBC over the USB connection
// until the create_disconnect function ahs been called or the GBA on the XBC has been powercycled.
// If Create is connected, power light goes yellow and play led turns on
void create_connect()
{
    serial_set_mode(BAUD_57K);
    serial_set_extra(1);
    beep(); sleep(0.1);
    CREATE_BUSY;
    serial_write_byte(128);//send SCI start
    sleep(0.5);
    serial_write_byte(131);//send SCI control
    sleep(0.5);
    serial_write_byte(139);
    serial_write_byte(8);
    serial_write_byte(0);
    serial_write_byte(128);
    CREATE_FREE;
    create_play_led(1);
    create_power_led(100,255);
    g_create_connected = 1;
    beep();
}

// Same as above but for serial XBCs (versions 1 & 2)
void create_connect_s()
{
    serial_set_mode(BAUD_57K);
    beep(); sleep(0.1);
    CREATE_BUSY;
    serial_write_byte(128);//send SCI start
    sleep(0.5);
    serial_write_byte(131);//send SCI control
    sleep(0.5);
    serial_write_byte(139);
    serial_write_byte(8);
    serial_write_byte(0);
    serial_write_byte(128);
    CREATE_FREE;
    create_play_led(1);
    create_power_led(100,255);
    g_create_connected = 1;
    beep();
}


// returns the serial connections on XBC to normal communications over the USB port.
// Turns of play LEd and returns power light to green
void create_disconnect() {
    create_play_led(0);
    create_power_led(0,255);
    g_create_connected =0; serial_set_mode(0); 
    serial_set_extra(0);
}

// Same as above but for serial XBCs
void create_disconnect_s() {
    create_play_led(0);
    create_power_led(0,255);
    g_create_connected =0; serial_set_mode(0); 
}



// See the create open interface document for more information on these functions
void create_safe() {CREATE_BUSY; s_write_byte(131); CREATE_FREE;}

void create_full() {CREATE_BUSY; s_write_byte(132); CREATE_FREE;}

void create_spot() {CREATE_BUSY; s_write_byte(134); CREATE_FREE;}

void create_cover() {CREATE_BUSY; s_write_byte(135); CREATE_FREE;}

void create_demo(int d) {CREATE_BUSY; s_write_byte(136); s_write_byte(d); CREATE_FREE;}

void create_cover_dock() {CREATE_BUSY; s_write_byte(143); CREATE_FREE;}


//////////////Create Sensor Routines///////////////



//globals are updated by the functions below to give access to many key Create sensors
int gc_lbump, gc_rbump, gc_ldrop, gc_rdrop, gc_fdrop, gc_rcliff, gc_rfcliff, gc_lcliff, gc_lfcliff;
int gc_distance, gc_angle, gc_total_angle, gc_advance_button, gc_play_button, gc_wall;

//this function will update the values for all of the sensor globals above
void create_sensor_update()
{
    create_bumpdrop();
    create_cliffs();
    create_angle();
    create_distance();
    create_buttons();
    create_wall();
}

// updates the right wall sensor
void create_wall() {
    int b;
    CREATE_BUSY;
    s_write_byte(142);
    s_write_byte(8);//drops and bumps
    while(serial_buffer_count()==0);
    gc_wall=s_read_byte();
    CREATE_FREE;
}

// updates the bumper and wheel drop globals with the current values from the Create
void create_buttons() {
    int b;
    CREATE_BUSY;
    s_write_byte(142);
    s_write_byte(18);//buttons
    while(serial_buffer_count()==0);
    b=s_read_byte();
    CREATE_FREE;
    gc_play_button=b & 0x1;
    gc_advance_button=(b >> 2) & 0x1;
}

// updates the bumper and wheel drop globals with the current values from the Create
void create_bumpdrop() {
    int b;
    CREATE_BUSY;
    s_write_byte(142);
    s_write_byte(7);//drops and bumps
    while(serial_buffer_count()==0);
    b=s_read_byte();
    CREATE_FREE;
    gc_fdrop=(b >> 4) & 0x1;
    gc_ldrop=(b >> 3) & 0x1;
    gc_rdrop=(b >> 2) & 0x1;
    gc_lbump=(b >> 1) & 0x1;
    gc_rbump=b & 0x1;
}

// updates the cliff sensor globas with the current sensor values from the Create
void create_cliffs() {
    CREATE_BUSY;
    s_write_byte(142);
    s_write_byte(9);
    while(serial_buffer_count()==0);
    gc_lcliff=s_read_byte();
    s_write_byte(142);
    s_write_byte(10);
    while(serial_buffer_count()==0);
    gc_lfcliff=s_read_byte();
    s_write_byte(142);
    s_write_byte(11);
    while(serial_buffer_count()==0);
    gc_rfcliff=s_read_byte();
    s_write_byte(142);
    s_write_byte(12);
    while(serial_buffer_count()==0);
    gc_rcliff=s_read_byte();
    CREATE_FREE;
}

// this function updates gc_angle which stores a normalized angle between 0 and 359 degrees
// and the global gc_total_angle which is not normalized and can be larger than 360 and less than 0.
// CCW angles are positive and CW turns decrement the angle value.
void create_angle() {
    int newangle=0;
    CREATE_BUSY;
    s_write_byte(142);
    s_write_byte(20);
    while(serial_buffer_count()<2);
    newangle = (256*s_read_byte() + s_read_byte());
    CREATE_FREE;
    gc_total_angle = gc_total_angle + newangle;
    gc_angle=(gc_angle+ newangle) % 360;
    if(gc_angle < 0) gc_angle = gc_angle + 360;
}

// this function updates the global gc_distance which has the average distance traveled of the two
// wheels (vehicle odometry) in mm.  Turning in place does not change this value.  
// Forward increments, backwards decrements
void create_distance() {
    CREATE_BUSY;
    s_write_byte(142);
    s_write_byte(19);
    while(serial_buffer_count()<2);
    gc_distance=gc_distance+(256*s_read_byte() + s_read_byte());
    CREATE_FREE;
}



////////////////////CREATE MOVEMENT FUNCTIONS///////////////////////////////////////////////

// This command drives the robot along a curve with radius (in mm) radius; and at a speed (mm/sec) of speed
// a radius of 32767 will drive the robot straight
// a radius of 1 will spin the robot CCW
// a radius of -1 will spin the robot CW
// Negative radii will be right turns, positive radii left turns
void create_drive (int speed, int radius)
{
    CREATE_BUSY;
    serial_write_byte(137);
    serial_write_byte(get_high_byte(speed));
    serial_write_byte(get_low_byte(speed));
    serial_write_byte(get_high_byte(radius));
    serial_write_byte(get_low_byte(radius));
    CREATE_FREE;
}

// special version of command above drives robot at speed speed.  Negative speed will drive robot backwards
void create_drive_straight (int speed)
{
    CREATE_BUSY;
    serial_write_byte(137);
    serial_write_byte(get_high_byte(speed));
    serial_write_byte(get_low_byte(speed));
    serial_write_byte(get_high_byte(32767));
    serial_write_byte(get_low_byte(32767));
    CREATE_FREE;
}

// special version of command spins robot CW with the wheels turning at speed speed
void create_spin_CW (int speed)
{
    CREATE_BUSY;
    serial_write_byte(137);
    serial_write_byte(get_high_byte(speed));
    serial_write_byte(get_low_byte(speed));
    serial_write_byte(get_high_byte(-1));
    serial_write_byte(get_low_byte(-1));
    CREATE_FREE;
}

// special version of command spins robot CCW with the wheels turning at speed speed
void create_spin_CCW (int speed)
{
    CREATE_BUSY;
    serial_write_byte(137);
    serial_write_byte(get_high_byte(speed));
    serial_write_byte(get_low_byte(speed));
    serial_write_byte(get_high_byte(1));
    serial_write_byte(get_low_byte(1));
    CREATE_FREE;
}

// this function drives the right wheel at r_speed and the left wheel at l_speed
// speeds for all of these functions are +/-500mm/sec.
void create_drive_direct(int r_speed, int l_speed)
{
    CREATE_BUSY;
    serial_write_byte(145);
    serial_write_byte(get_high_byte(r_speed));
    serial_write_byte(get_low_byte(r_speed));
    serial_write_byte(get_high_byte(l_speed));
    serial_write_byte(get_low_byte(l_speed));
    CREATE_FREE;
}    


/////////////////////////CREATE LEDs///////////////////

int gc_leds[3]={0,0,0};

// turns on and off the advance (>>) LED (0 is off, 1 is on)
void create_advance_led(int on) 
{
    if(on){
        if(!(gc_leds[0] & 0b00001000)) gc_leds[0]=gc_leds[0]+ 8;
    }
    else{
        if(gc_leds[0] & 0b00001000) gc_leds[0]=gc_leds[0]- 8;
    }
    CREATE_BUSY;
    serial_write_byte(139);
    serial_write_byte(gc_leds[0]);            
    serial_write_byte(gc_leds[1]);            
    serial_write_byte(gc_leds[2]);  
    CREATE_FREE;
}

// turns on and off the play LED (0 is off, 1 is on)
void create_play_led(int on) 
{
    if(on){
        if(!(gc_leds[0] & 0b00000010)) gc_leds[0]=gc_leds[0]+ 2;
    }
    else{
        if(gc_leds[0] & 0b00000010) gc_leds[0]=gc_leds[0]- 2;
    }
    CREATE_BUSY;
    serial_write_byte(139);
    serial_write_byte(gc_leds[0]);            
    serial_write_byte(gc_leds[1]);            
    serial_write_byte(gc_leds[2]);  
    CREATE_FREE;
}


//sets create power led. Color =0 is green and color = 255 is red -- with intermediate colors
//brightness of 0 is off and 255 is fully bright
void create_power_led(int color, int brightness) 
{
    gc_leds[1]=color;
    gc_leds[2]=brightness;
    CREATE_BUSY;
    serial_write_byte(139);
    serial_write_byte(gc_leds[0]);            
    serial_write_byte(gc_leds[1]);            
    serial_write_byte(gc_leds[2]);      
    CREATE_FREE;
}


/////////// CREATE MUSIC////////////
int gc_song_array[16][33];

// this loads a song into the robot's memory. 
// song can be numbered 0 to 15. the first element in each row of the array should be
// the number of notes (1-16) the subsequent pairs of bytes should be tone and duration
// see the roomba SCI manual for note codes.
// user's program should load the song data into the array before calling this routine
void create_load_song(int num)
{
    int i, numnotes;
    numnotes=gc_song_array[num][0];
    if(num >= 0 && num <=15 && numnotes > 0 && numnotes <= 16){
        CREATE_BUSY;
        serial_write_byte(140);
        serial_write_byte(num);
        serial_write_byte(numnotes);
        for(i=1; i< 2*numnotes+1; i++)
          serial_write_byte(gc_song_array[num][i]);
        CREATE_FREE;
    }
    else printf("illegal song #%d is %d notes long being written to memory\n", num, gc_song_array[num][0]);
}

// see the roomba SCI manual for note codes.
void create_play_song(int num)
{
    int i;
    if(num >= 0 && num <=15 ){
        CREATE_BUSY;
        serial_write_byte(141);
        serial_write_byte(num);
        CREATE_FREE;
    }
    else printf("Song array reference is out of bounds\n");
}

/////////// The functions below are communication and byte packing utitilities
int get_high_byte(int v)
{
    return (v >>8);
}

int get_low_byte(int v)
{
    return (v & 0b0000000011111111);
}

void clear_serial_buffer()
{
    while (serial_buffer_count()) serial_read_byte();
}

void s_write_byte(int byte)
{
    callml(181, byte);
}

//Returns 0 if there is nothing to be read
int s_read_byte() 
{
    return callml(180, 0);
}