/*
* functions.c
* This file is part of the project "Remote control for CPG-based robots"
*
* Copyright (C) 2010 - Gabriel CUENDET
*/


#include "functions.h"
#include <stdio.h>
#include <math.h>

robot current_robot=0;
locomotion_mode current_locomotion_mode=WALKING;
int1 turbo=0;

state       current_state=ERROR;
state       previous_state=ERROR;


/***************
 * MENU SYSTEM *
 ***************/
main_menu   current_main_menu=0;
sub_menu    current_sub_menu=NO_SUB;
int1        display_changed=0;

char menu_labels[14][16]={
      "INFOS RC","Batt. voltage","Version radio", "Reset radio", "Reset main cpu",
      "SELECT ROBOT","Favorites","Enter ch. nb.","Scan channels",
      "ROBOT","Start robot","Stop robot","Start charging","Stop charging"};

int8 menu_lengths[4]={3, 4, 3, 4}; //< menu_lengths[0] = Number of menu
                                         //  then length of sub-menu
                                         
tab_fct menu_fct[11]={show_batt_voltage, show_version_radio, reset_radio, reset_main,
                        menu_favorites, menu_enter_channel, menu_scan_channels, 
                        menu_start_robot, menu_stop_robot, menu_start_charging, menu_stop_charging};



/***************************
   *  USER INTERFACE       ****************************************************
   *************************/

///---ENTER_VALUE----------
/// Allows the user to enter an 8 bits value (in decimal) and modify the value by reference.
/**@PARAM char* label: Display the label on the first line of the LCD as a description of the value.
   @PARAM int8* value: Reference modified parameter. 
   @RETURN  'TRUE' (1) when a value has been validated by the user 
         or 'FALSE' (0) when the user quits without having validated a value **/
int1 enter_value(char* label, int8* value)
{
   char text[16];
   unsigned int8 position=1;
   int8 release_time=0;
   *value=0;
   
   
   disable_interrupts(INT_RB);
   disable_interrupts(INT_EXT);
   disable_interrupts(INT_EXT1);
   
   clear_display();
   display(label,0x00);
   sprintf(text, "%03u", *value);
   display(text,0x40);
   // Set the cursor at the first position,the hundreds
   set_pos_display(0x40 + (position-1));
   
   // Avoid an initial 'enter' due to bouncing 
   // Wait for the user to release the button
   do
   {
      if(!input(BTN_ENTER))
      {
         release_time = 0;
      }
      
      else if (input(BTN_ENTER))
      {
         release_time += 1;
         delay_ms(1);
      }
   }while(release_time < 50);
   release_time = 0;
   
   while(!(position==1 && !input(BTN_CANCEL)))
   {
      if(!input(BTN_ENTER))
      {
         if(position==1)
         {
            set_pos_display(0x41);
            position=2;
         }
         
         else if(position==2)
         {
            set_pos_display(0x42);
            position=3;
         }
         
         else if(position==3)
         {
            return(1);
         }
         
         // Wait for the user to release the button
         do
         {
            if(!input(BTN_ENTER))
            {
               release_time = 0;
            }
            
            else if (input(BTN_ENTER))
            {
               release_time += 1;
               delay_ms(1);
            }
         }while(release_time < 50);
         release_time = 0;
         
      }
      
      if(!input(BTN_CANCEL))
      {
         if(position==2)
         {
            set_pos_display(0x40);
            position=1;
         }
         
         else if(position==3)
         {
            set_pos_display(0x41);
            position=2;
         }
         
         // Wait for the user to release the button
         do
         {
            if(!input(BTN_CANCEL))
            {
               release_time = 0;
            }
            
            else if (input(BTN_CANCEL))
            {
               release_time += 1;
               delay_ms(1);
            }
         }while(release_time < 50);
         release_time = 0;
      }
      
      if(!input(BTN_PLUS))
      {
         if(position==1)
         {
            if(*value<156)
            {
               *value = (*value + 100);
            }
            
            else if(*value>=156 && *value < 200)
            {
               *value = 200;
            }
            
            else if(*value>=200)
            {
               *value = *value - 200;
            }
            
            sprintf(text, "%03u", *value);
            display(text,0x40);
            set_pos_display(0x40 + (position-1));
         }
         else if(position==2)
         {
            *value = (*value + 10);
            
            sprintf(text, "%03u", *value);
            display(text,0x40);
            set_pos_display(0x40 + (position-1));
         }
         else if(position==3)
         {
            *value = (*value + 1);
            
            sprintf(text, "%03u", *value);
            display(text,0x40);
            set_pos_display(0x40 + (position-1));
         }
         
         // Wait for the user to release the button
         do
         {
            if(!input(BTN_PLUS))
            {
               release_time = 0;
            }
            
            else if (input(BTN_PLUS))
            {
               release_time += 1;
               delay_ms(1);
            }
         }while(release_time < 50);
         release_time = 0;
         
      }
      
      if(!input(BTN_MINUS))
      {
         if(position==1)
         {
            if(*value>=100)
            {
               *value = *value - 100;
            }
            else if(*value<100)
            {
               *value = 255;
            }
            
            sprintf(text, "%03u", *value);
            display(text,0x40);
            set_pos_display(0x40);
         }
         
         else if(position==2)
         {
            *value = (*value - 10);
            
            sprintf(text, "%03u", *value);
            display(text,0x40);
            set_pos_display(0x40 + (position-1));
         }
         
         else if(position==3)
         {
            *value = (*value - 1);
            
            sprintf(text, "%03u", *value);
            display(text,0x40);
            set_pos_display(0x40 + (position-1));
         }
                  
         // Wait for the user to release the button
         do
         {
            if(!input(BTN_MINUS))
            {
               release_time = 0;
            }
            
            else if (input(BTN_MINUS))
            {
               release_time += 1;
               delay_ms(1);
            }
         }while(release_time < 50);
         release_time = 0;
         
      }
   }
   
   enable_interrupts(INT_RB);
   enable_interrupts(INT_EXT);
   enable_interrupts(INT_EXT1);
   
   return(0);
}


///---DISPLAY_MENUS-------
/// Function display_menus() display the label corresponding to the current main menu or sub menu
void display_menus()
{
   int8 index=0;
   int8 i=0;
   char symb[1]=0x22;
   
   clear_display();
   
   for(i=current_main_menu; i>0 ; --i)
   {
       index = index + (menu_lengths[current_main_menu] + 1);
   }
   
   index = index + current_sub_menu;
   
   // BUG in the programm...The if loop shouldn't be necessary
   if(current_main_menu == 2)
   {
      index += 1;
   }
   
   display(menu_labels[index],0x00);
   char_display(symb,0x0F);
   set_pos_display(0x10);
}


///---PRESS_BUTTON--------
/// The function press_button(char button) set the current_state variable in function of the pressed button. (Implementation of the state machine model)
/** @PARAM char button: Indicates which button has been pressed **/
void press_button(char button)
{
   int8 i=0;
   int8 index=0;
   
   if(current_state == MENU)
   {
      if(button==BTN_PLUS)
      {
         if(current_sub_menu==0)
         {
            current_main_menu = (current_main_menu + 1) % menu_lengths[0] ;
         }
         else
         {
            current_sub_menu = (current_sub_menu + 1) % (menu_lengths[current_main_menu + 1] + 1);
            if(current_sub_menu==0)
            {
               current_sub_menu=1;
            }
         }
         
         display_changed = 1;
      }
      
      else if(button==BTN_MINUS)
      {
         if(current_sub_menu==0)
         {
            if(current_main_menu==0)
            {
               current_main_menu = menu_lengths[0] - 1;
            }
            else
            {
               current_main_menu -= 1;
            }
         }
         else
         {
            if(current_sub_menu==1)
            {
               current_sub_menu = menu_lengths[current_main_menu + 1];
            }
            else
            {
               current_sub_menu -= 1;
            }
         }
         
         display_changed = 1;
      }
   
      else if(button==BTN_ENTER)
      {
         if(current_sub_menu==0)
         {
            current_sub_menu=1;
            display_changed = 1;
         }
         else
         {
            index=0;
            for(i=current_main_menu; i>0 ; --i)
            {
               index += menu_lengths[current_main_menu];
            }
   
            index += (current_sub_menu - 1);
            display_changed = 1;
            
            // BUG in the programm...The if loop shouldn't be necessary
            if(current_main_menu == 2)
            {
               index += 1;
            }
            
            (menu_fct[index])();
         }
      }
      
      else if(button==BTN_CANCEL)
      {
         if(current_sub_menu==0)
         {
            current_state=previous_state;
            previous_state=MENU;
         }
         else
         {
            current_sub_menu=0;
            display_changed = 1;
         }
      }
   
   }

   else if(current_state == RC_AUTONOMOUS)
   {
      if(button == BTN_ENTER)
      {  
         output_low(PIN_C0);
         output_low(PIN_C1);
         output_low(PIN_C2);
         
         current_state=MENU;
         display_changed=1;
         current_main_menu=SELECT_ROBOT_MAIN;
         current_sub_menu=NO_SUB;
      }
      
   }
   
   else if(current_state == STOPPED)
   {
      if(button == BTN_ENTER)
      {
         current_state=MENU;
         display_changed=1;
         current_main_menu=ROBOT_MAIN;
         current_sub_menu=NO_SUB;
      }

   }
   
   else if(current_state == IN_CHARGE)
   {
      if(button == BTN_ENTER)
      {
         current_state=MENU;
         display_changed=1;
         current_main_menu=ROBOT_MAIN;
         current_sub_menu=NO_SUB; 
      }
      
      else if(button == BTN_CANCEL)
      {
         menu_stop_charging(); 
      }

   }
   
   else if(current_state == STARTED)
   {
      if(button == BTN_CANCEL)
      {
         disable_interrupts(INT_RTCC);
         output_low(PIN_C0);
         output_low(PIN_C1);
         menu_stop_robot();
      }
      
      else if(button == BTN_ENTER)
      {
         current_locomotion_mode = (current_locomotion_mode + 1) % 2;
         if(current_locomotion_mode == SWIMMING)
         {
            output_high(PIN_C0);
         }
         else if(current_locomotion_mode == WALKING)
         {
            output_low(PIN_C0);
         }
      }
      
      else if(button == BTN_PLUS)
      {
         turbo = 1;
         output_high(PIN_C1);
      }
      
      else if(button == BTN_MINUS)
      {
         turbo = 0;
         output_low(PIN_C1);
      }
      
   }
   
   else if(current_state == ERROR)
   {
      output_low(PIN_C2);
      current_state = previous_state;
      previous_state = ERROR;
   }
}


//---enter_cancel-------------------
/// Function ENTER_CANCEL(int8 delay)
/** @PARAM int8 delay : Time (in N * 100ms) the user has to press enter or cancel
   @RETURN  'TRUE' (1) when 'ENTER' button (B0) has been pressed or 'FALSE' (0) 
            when 'CANCEL' button (B1) or none of the buttons have been pressed **/
int1 enter_cancel(int8 delay)
{
   unsigned int32 i;
   
   /// Time the program will wait for the user to press 'ENTER' or 'CANCEL'
   for(i=delay;i>0;--i)
   {
      delay_ms(100);
      
      /// PIN_B0, active low, is the 'ENTER' button
      if(input(PIN_B0) == 0)
      {
         return(1);
      }
      
      /// PIN_B1, active low, is the 'CANCEL' button
      else if(input(PIN_B1) == 0)
      {
         return(0);
      }
      
   }
   
   /** When neither 'ENTER' nor 'CANCEL' is pressed under the timer delay, 
      a 'CANCEL' signal is returned **/
   return(0);
}


//---ERR----------------------------
/// Function ERR(char* description)
/** Enter the ERROR state (9) and display a short one-line description of the error on the LCD **/
void err(char* description)
{
   char text[16];
   
   if(current_state != MENU)
   {
      previous_state = current_state;
      current_state = ERROR;
   }
   
   else
   {
      current_state = ERROR;
   }
   
   clear_display();
   strcpy(text,"ERROR!");
   display(text,0x00);
   
   display(description,0x40);
   set_pos_display(0x50);
}


     /*************************
      *  MENU FUNCTIONS       *************************************************
      *************************/
      /** Functions to fill the menu table. Must be :
          void function(void); **/
void show_version_radio()
{
   char text[16];
   int8 result;
   disable_interrupts(INT_EXT);
   disable_interrupts(INT_EXT1);
   disable_interrupts(INT_RB);
   
   clear_display();
   strcpy(text,"Version radio:");
   display(text,0x00);
   
   get_reg_b(REG_INTF_VER, &result);
   
   strcpy(text,"0x");
   display(text,0x40);
   sprintf(text,"%2X",result);
   display(text,0x42);
   
   set_pos_display(0x10);
   delay_ms(1500);
   enable_interrupts(INT_EXT);
   enable_interrupts(INT_EXT1);
   enable_interrupts(INT_RB);
}
      
void show_batt_voltage()
{
   char text[16];
   disable_interrupts(INT_EXT);
   disable_interrupts(INT_EXT1);
   disable_interrupts(INT_RB);
   
   clear_display();
   strcpy(text,"Batt. voltage:");
   display(text,0x00);
   
   sprintf(text,"%4.2g",battery_voltage());
   display(text,0x40);
   
   strcpy(text,"mV");
   display(text,0x4E);
   
   set_pos_display(0x10);
   delay_ms(1500);
   enable_interrupts(INT_EXT);
   enable_interrupts(INT_EXT1);
   enable_interrupts(INT_RB);
}

void reset_radio()
{
   disable_interrupts(INT_EXT);
   disable_interrupts(INT_EXT1);
   disable_interrupts(INT_RB);
   
   // LED
   output_high(PIN_C1);
   
   output_low(PIN_B3);
   delay_ms(200);
   output_float(PIN_B3);
   
   // Time for the radio interface to boot  
   delay_ms(1000);
   
   if(!sync())
   {
      output_low(PIN_C1);
      delay_ms(200);
      reset_cpu();
   }
   
   output_low(PIN_C1);
   enable_interrupts(INT_EXT);
   enable_interrupts(INT_EXT1);
   enable_interrupts(INT_RB);
}

void reset_main()
{
   reset_cpu();
}

void menu_start_charging()
{
   char text[16];
   // Function useful only for the salamandro robotica (robot 2)
   if(current_robot == 2)
   {
      if(previous_state==STOPPED)
      {
         set_reg_b(REGB_MODE, 4);
         current_state=IN_CHARGE;
      }
   
      else if(previous_state == IN_CHARGE)
      {
         disable_interrupts(INT_EXT);
         disable_interrupts(INT_EXT1);
         disable_interrupts(INT_RB);
   
         clear_display();
         strcpy(text,"Already charging");
         display(text,0x00);
   
         set_pos_display(0x10);
         delay_ms(1500);
         enable_interrupts(INT_EXT);
         enable_interrupts(INT_EXT1);
         enable_interrupts(INT_RB);
      }
      
      else
      {
         strcpy(text,"No robot select.");
         err(text);
      }
   }
   
}

void menu_stop_charging()
{
   char text[16];
   // Function useful only for the robot Salamandra robotica (robot 2)
   if(current_robot==2)
   {
      /**The case 'previous_state==STOPPED' is included because we can't know 
         if the robot is IN_CHARGE by reading its MODE register on the old salamandra 
         (strange values)**/
      if(previous_state==IN_CHARGE || previous_state==STOPPED)
      {
         set_reg_b(REGB_MODE,0);
         previous_state=IN_CHARGE;
         current_state=STOPPED;
      }
      else
      {
        char text[16];
         strcpy(text,"No robot select.");
         err(text);
      } 
   }
}

void menu_favorites()
{
   char text[16];
   int8 value;
   char symb[1];
   int8 release_time=0;
   
   /** FAVORITES TABLE.
       To add a favorite item, increment number_favorites by the number of items 
       you want to add and make sure the label and the corresponding channel are 
       at the same position in the tables "favorites_labels" and "favorites_channel"
   **/
   const int8 number_favorites = 5;
   char favorites_labels[number_favorites][16]={"Amphibot II ","Salamandra 1", "Amphibot PIC","Salamandra 2","Amphibot ARM"};
   const int8 favorites_channels[number_favorites]={13, 108, 26, 106, 82};
   
   int8 index=0;
   
   disable_interrupts(INT_RB);
   disable_interrupts(INT_EXT);
   disable_interrupts(INT_EXT1);
   
   clear_display();
   strcpy(text,"Favorites");
   display(text,0x00);
   
   *symb = 0x10;
   char_display(symb, 0x40);
   *symb = 0x91;
   char_display(symb, 0x41);
   *symb = 0x20;
   char_display(symb, 0x4F);
   
   sprintf(text,"%s",favorites_labels[index]);
   display(text, 0x42);
   set_pos_display(0x50);
   
   
   // Avoid an initial 'enter' due to bouncing 
   // Wait for the user to release the button
   do
   {
      if(!input(BTN_ENTER))
      {
         release_time = 0;
      }
      
      else if (input(BTN_ENTER))
      {
         release_time += 1;
         delay_ms(1);
      }
   }while(release_time < 50);
   
   
   while(input(BTN_CANCEL))
   {
      if(!input(BTN_PLUS))
      {
         index = (index + 1) % number_favorites;
         
         sprintf(text,"%s",favorites_labels[index]);
         display(text,0x42);
         set_pos_display(0x50);
         
         delay_ms(200);
      }
      
      else if(!input(BTN_MINUS))
      {
         if(index>0)
         {
            index = (index - 1);
         }
         else
         {
            index = number_favorites - 1;
         }
         
         sprintf(text,"%s",favorites_labels[index]);
         display(text,0x42);
         set_pos_display(0x50);
         
         delay_ms(200);
      }
      
      else if(!input(BTN_ENTER))
      {
         set_reg_b(REG_INTF_CH,favorites_channels[index]);
         
         if(!get_reg_b(REG_RWL_CH, &value))
         {
            strcpy(text,"Turn on robot!");
            err(text);
            
            enable_interrupts(INT_RB);
            enable_interrupts(INT_EXT);
            enable_interrupts(INT_EXT1);
            
            return;
         }
   
         else
         {
            current_state = STOPPED;
            current_robot = index + 1;
            
            enable_interrupts(INT_RB);
            enable_interrupts(INT_EXT);
            enable_interrupts(INT_EXT1);
            return;
         }
         
      }
   }
   
   enable_interrupts(INT_RB);
   enable_interrupts(INT_EXT);
   enable_interrupts(INT_EXT1);
   
   return;
   
}

void menu_enter_channel()
{
   int8 channel=0;
   char text[16];
   
   disable_interrupts(INT_RB);
   disable_interrupts(INT_EXT);
   disable_interrupts(INT_EXT1);
   
   strcpy(text,"Channel number:");
   if (!enter_value(text,&channel))
   {
      clear_display();
      strcpy(text,"ERROR");
      display(text,0x00);
         
      strcpy(text,"No value selected");
      display(text,0x40);
      delay_ms(1500);
      
      enable_interrupts(INT_RB);
      enable_interrupts(INT_EXT);
      enable_interrupts(INT_EXT1);
      return;
   }
   
   set_reg_b(REG_INTF_CH, channel);
     
   if(!get_reg_b(REG_RWL_CH, &channel))
   {
      strcpy(text,"No robot on ch.");
      err(text);
   }
   
   else
   {
      /************
       * CHANNELS *
       ************/
      if(channel == 13)
      {
         current_robot = AMPHIBOT2;
      }
      else if(channel == 108)
      {
         current_robot = SALAMANDRA1;
      }
      else if(channel == 26)
      {
         current_robot = AMPHIBOT3PIC;
      }
      else if(channel == 106)
      {
         current_robot = SALAMANDRA2;
      }
      else if(channel == 82)
      {
         current_robot = AMPHIBOT3ARM;
      }
      else
      {
         strcpy(text,"UNKNOWN ROBOT");
         err(text);
         
         enable_interrupts(INT_RB);
         enable_interrupts(INT_EXT);
         enable_interrupts(INT_EXT1);
         return;
      }
      
      current_state = STOPPED;
   }
   
   enable_interrupts(INT_RB);
   enable_interrupts(INT_EXT);
   enable_interrupts(INT_EXT1);
   return;
}

void menu_scan_channels()
{
   char text[16];
   int8 value;
   int8 local_channel = 0x00;
   
   disable_interrupts(INT_RB);
   disable_interrupts(INT_EXT);
   disable_interrupts(INT_EXT1);
   
   if(scan(&local_channel))
   {
      set_reg_b(REG_INTF_CH,local_channel);
         
      if(!get_reg_b(REG_RWL_CH, &value))
      {
         strcpy(text,"Turn on robot!");
         err(text);
         
      }
      else
      {
         /************
          * CHANNELS *
          ************/
         if(value == 13)
         {
            current_robot = AMPHIBOT2;
         }
         else if(value == 108)
         {
            current_robot = SALAMANDRA1;
         }
         else if(value == 26)
         {
            current_robot = AMPHIBOT3PIC;
         }
         else if(value == 106)    // NE PEUX JAMAIS ETRE SELECTIONNEE, CAR MEME CANAL QUE SALAMANDRA1 !!!
         {
            current_robot = SALAMANDRA2;
         }
         else if(value == 82)
         {
            current_robot = AMPHIBOT3ARM;
         }
         else
         {
            strcpy(text,"UNKNOWN ROBOT");
            err(text);
            return;
         }
         current_state = STOPPED;
         
      }
   }
   
   enable_interrupts(INT_RB);
   enable_interrupts(INT_EXT);
   enable_interrupts(INT_EXT1);
}

void menu_start_robot()
{
   char text[16];
   int1 start=0;
   
   if(previous_state==STOPPED)
   {
      switch(current_robot)
      {
         case 1:  start = start_amphibot();
                  break;
         case 2:  start = start_salamander();
                  break;
         case 3:  start = start_amphibot();
                  break;
         case 4:  start = start_salamander();
                  break;
         case 5:  start = start_amphibot_mathieu();  
      }
      
      if(start)
      {
         current_state=STARTED;
      }
      else
      {
         strcpy(text,"Not starting");
         err(text);
      }
   }
   
   else if(previous_state == RC_AUTONOMOUS)
   {
      strcpy(text,"No robot select.");
      err(text);
   }
   
   else if(previous_state == IN_CHARGE)
   {
      strcpy(text,"Robot in charge");
      err(text);
   }
   
}

void menu_stop_robot()
{
   char text[16];
   
   if(current_state == STARTED)
   {
      switch(current_robot)
      {
         case 1:  stop_amphibot();
                  break;
         case 2:  stop_salamander();
                  break;
         case 3:  stop_amphibot();
                  break;
         case 4:  stop_salamander();
                  break;
         case 5:  stop_amphibot_mathieu();
                  break;
      }
      
      current_state=STOPPED;
      previous_state=STARTED;
      return;
   }
   
   else if(previous_state==IN_CHARGE || previous_state==STOPPED)
   {
      strcpy(text,"Already stopped");
      err(text);
      return;
   }
   
   else if(previous_state == RC_AUTONOMOUS)
   {
      strcpy(text,"No robot select.");
      err(text);
      return;
   }
}


/***************************
   *  BATTERY MANAGEMENT   ****************************************************
   *************************/
   
///---BATTERY_VOLTAGE------
/// Getter for the voltage register of DS2764
/** **/
float battery_voltage()
{
   int16 result[2];
   float voltage;
   
   i2c_start();
   i2c_write(BATADDR);
   i2c_write(0x0C);
   i2c_start();
   i2c_write(BATADDR + 1);      // Slave address + READ(1)
   result[0] = i2c_read(i2c,0);
   i2c_stop();
   
   i2c_start();
   i2c_write(BATADDR);
   i2c_write(0x0D);
   i2c_start();
   i2c_write(BATADDR + 1);      // Slave address + READ(1)
   result[1] = i2c_read(i2c,0);
   i2c_stop();
   
   result[0] = result[0]<<3 | result[1]>>5;
   voltage = 4.88 * result[0];
   
   return(voltage);
   
}


///---BATTERY_TEMP---------
/// Getter for the temperature register of DS2764
/** **/
float battery_temp()
{
   int16 result[2];
   float temperature;
   
   i2c_start();
   i2c_write(BATADDR);
   i2c_write(0x18);
   i2c_start();
   i2c_write(BATADDR + 1);      // Slave address + READ(1)
   result[0] = i2c_read(i2c,0);
   i2c_stop();
   
   i2c_start();
   i2c_write(BATADDR);
   i2c_write(0x19);
   i2c_start();
   i2c_write(BATADDR + 1);      // Slave address + READ(1)
   result[1] = i2c_read(i2c,0);
   i2c_stop();
   
   result[0] = result[0]<<3 | result[1]>>5;
   temperature = 0.125 * result[0];
   
   return(temperature);
}


///---BATTERY_CURRENT------
/// Getter for the current register of DS2764
/** Returns the current in mA. A positive current means the battery is charging, 
    while a negative current means the battery is discharging**/
float battery_current()
{
   int16 result[2];
   float current;
   
   i2c_start();
   i2c_write(BATADDR);
   i2c_write(0x0E);
   i2c_start();
   i2c_write(BATADDR + 1);      // Slave address + READ(1)
   result[0] = i2c_read(i2c,0);
   i2c_stop();
   
   i2c_start();
   i2c_write(BATADDR);
   i2c_write(0x0F);
   i2c_start();
   i2c_write(BATADDR + 1);      // Slave address + READ(1)
   result[1] = i2c_read(i2c,0);
   i2c_stop();
   
   if(!(result[0] & 0x0080))
   {
      result[0] = (result[0]<<5 | result[1]>>3) & (0b0000111111111111);
      current = 0.625 * result[0];
   }
   
   else
   {
      result[0] = !(result[0]<<5 | result[1]>>3)+1;
      current = -0.625 * result[0];
   }
   return(current);
   
}


///---BATTERY_ACCUMULATED_CURRENT------
/// Getter for the accumulated current register of DS2764
/** **/
float battery_accumulated_current()
{
   int16 result[2];
   float accumulated_current;
   
   i2c_start();
   i2c_write(BATADDR);
   i2c_write(0x10);
   i2c_start();
   i2c_write(BATADDR + 1);      // Slave address + READ(1)
   result[0] = i2c_read(i2c,0);
   i2c_stop();
   
   i2c_start();
   i2c_write(BATADDR);
   i2c_write(0x11);
   i2c_start();
   i2c_write(BATADDR + 1);      // Slave address + READ(1)
   result[1] = i2c_read(i2c,0);
   i2c_stop();
   
   result[0] = result[0] | result[1];
   accumulated_current = 0.25 * result[0];
   
   return(accumulated_current);
   
}


/***************************
   *   DISPLAY FUNCTIONS   ****************************************************
   *************************/
   
///---INIT_DISPLAY---------
/// Initialization of the display
/** **/
void init_display()
{
   i2c_start();
   i2c_write(LCDADDR);
   i2c_write(0x00);     // Control byte -> CO: 0, RS: 0 
   
   i2c_write(0x34);     // FUNCTION SET -> DL: 8 bits, M:  two line, SL: 1:18, H: normal instruction set
   i2c_write(0x0F);     // DISPLAY_CTL  -> D: on, C: on, B:on
   i2c_write(0x06);     // ENTRY_MODE_SE-> I_D: increments to the right, S: no shift
   
   i2c_write(0x35);     // FUNCTION SET -> DL: 8 bits, M: 16 by two line, SL: 1:18, H: extended instruction set
   i2c_write(0x07);     // DISP_CONF    -> P: right to left, Q: bottom to top 
   i2c_write(0x43);     // HV_GEN       -> HV do not use (11)
   i2c_write(0xE8);     // VLCD_SET     -> set Vlcd to 5V, store to VB
   i2c_write(0xA8);     // VLCD_SET     -> set Vlcd to 5V(A8), store to VA
   i2c_write(0x08);
   
   i2c_write(0x34);     // FUNCTION SET -> DL: 8 bits, M:  two line, SL: 1:18, H: normal instruction set
   i2c_write(0x80);     // SET_CGRAM    -> DDRAM Address set to 0x00
   i2c_write(0x01);     // CLEAR_DISPLAY
   delay_us(20);
   i2c_stop();
   
   clear_display();

}


///---SET_POS_DISPLAY------
/// Set the cursor position to the specified DDRAM address
/** **/
void set_pos_display(int8 pos)
{
   i2c_start();
   i2c_write(LCDADDR);
   i2c_write(0x00);     // Control byte -> CO: 0, RS: 0
   i2c_write(0x80 | pos);     // SET_DDRAM    -> ADD: 0x00
   i2c_stop();          
}


///---CLEAR_DISPLAY--------
/// Write blank character in all the DDRAM addresses
/** **/
void clear_display()
{
   int8 i;

   set_pos_display(0x00);          
   
   i2c_start();
   i2c_write(LCDADDR);
   i2c_write(0x40);
   // Write the blank pattern 0x91 into all the DDRAM adresses of 1st line
   for(i=0;i<40;++i)
   {
      i2c_write(0x91);
   }
   i2c_stop();
   
   set_pos_display(0x40);
   
   i2c_start();
   i2c_write(LCDADDR);
   i2c_write(0x40);
   // Write the blank pattern 0x91 into all the DDRAM adresses of 2nd line
   for(i=0;i<40;++i)
   {
      i2c_write(0x91);
   }
   i2c_stop();
   
   i2c_start();
   i2c_write(LCDADDR);
   i2c_write(0x00);
   i2c_write(0x02); // Return at position 0 of the first line
   i2c_stop();
}


///---DISPLAY--------------
/// Display the specified string (char* str) at the specified position (int8 pos)
/** WARNING :  This function works for ascii character between 0x20 (space) and 
               0x7A (z) ONLY because of the conversion between ASCII and Philips'
               character set 'R'**/
void display(char* str,int8 pos)
{
   int i;
   
   set_pos_display(pos);
   
   i2c_start();
   i2c_write(LCDADDR);
   i2c_write(0x40);
   
   for(i=0 ; i<40 && str[i]!=0 ; ++i)
   {
      i2c_write(0x80 + str[i]);
   }
   i2c_stop();
}


///---CHAR_DISPLAY---------
/// Display the specified character (char* character) at the specified position (int8 pos)
/** WARNING :  Philips' character set 'R' is used and doesn't correspond to ASCII**/
void char_display(char* character,int8 pos)
{
   int i;
   
   set_pos_display(pos);
   
   i2c_start();
   i2c_write(LCDADDR);
   i2c_write(0x40);
   
   for(i=0 ; i<40 && character[i]!=0 ; ++i)
   {
      i2c_write(character[i]);
   }
   i2c_stop();
}


/***************************
   * LOCOMOTION PARAMETERS ****************************************************
   *************************/

///---UPDATE_SALAMANDER----
///
/** **/
int1 update_salamander(float spd, float dir)
{
   float FREQ_CORRECT = 2.326;
   float drive = 0.0;
   float turn = 0.0;
   float bp[8] = {1, 5, 0.5, 1.3, 0, 33.333, 60, 0}; // Body params
   float lp[8] = {1, 3, 0.2, 0.6, 0, 33.333, 60, 0}; // Leg params
   float nu_b;
   float r_b;
   float nu_l;
   float r_l;
   
   if(current_robot == 4)
   {  
      if(turbo)
      {
         bp[0] = 1.0;
         bp[1] = 5.0;
         bp[2] = 1.0;
         bp[3] = 2.6;
         bp[4] = 0.0;
         bp[5] = 20.0; 
         bp[6] = 40.0;
         bp[7] = 0.0; // TURBO Body params
         
         lp[0] = 1.0;
         lp[1] = 3.0;
         lp[2] = 0.4;
         lp[3] = 1.2;
         lp[4] = 0.0;
         lp[5] = 20.0;
         lp[6] = 40.0;
         lp[7] = 0.0; // TURBO Leg params
      }
      else
      {
         bp[0] = 1.0;
         bp[1] = 5.0;
         bp[2] = 0.3;
         bp[3] = 1.2;
         bp[4] = 0.0;
         bp[5] = 10.0; 
         bp[6] = 40.0;
         bp[7] = 0.0; // Body params
         
         lp[0] = 1.0;
         lp[1] = 3.0;
         lp[2] = 0.2;
         lp[3] = 0.6;
         lp[4] = 0.0;
         lp[5] = 20.0;
         lp[6] = 40.0;
         lp[7] = 0.0; // Leg params
      }
      FREQ_CORRECT = 1;
   }
   
   if(current_locomotion_mode == SWIMMING)
   {
      set_reg_b(REGB_TURN, (from_turn(-dir) << 4) | from_turn(-dir));
      drive = 3.001 + 1.999 * spd;
   }
   
   else
   {
      set_reg_b(REGB_PHASE, (unsigned int8) 127.5);
      set_reg_b(REGB_TURN, (from_turn(-dir) << 4) | from_turn(0));
      drive = 0.999 + 1.999 * spd;
   }
   
   turn = dir;
   
   // TURBO
   if(turbo && current_robot==2)
   {
      bp[0] = 1.0;
      bp[1] = 5.0;
      bp[2] = 1.0;
      bp[3] = 2.6;
      bp[4] = 0.0;
      bp[5] = 33.333; 
      bp[6] = 60.0;
      bp[7] = 0.0; // TURBO Body params
      
      lp[0] = 1.0;
      lp[1] = 3.0;
      lp[2] = 0.4;
      lp[3] = 1.2;
      lp[4] = 0.0;
      lp[5] = 33.333;
      lp[6] = 60.0;
      lp[7] = 0.0; // TURBO Leg params
   }
   
   if(drive < bp[0] || drive > bp[1])
   {
      nu_b = bp[4];
      r_b = bp[7];
   }
   else
   {
      nu_b = bp[2] + (bp[3] - bp[2])/(bp[1] - bp[0]) * (drive - bp[0]);
      r_b = bp[5] + (bp[6] - bp[5])/(bp[1] - bp[0]) * (drive - bp[0]);
   }
   
   if(drive < lp[0] || drive > lp[1])
   {
      nu_l = lp[4];
      r_l = lp[7];
   }
   else
   {
      nu_l = lp[2] + (lp[3] - lp[2])/(lp[1] - lp[0]) * (drive - lp[0]);
      r_l = lp[5] + (lp[6] - lp[5])/(lp[1] - lp[0]) * (drive - lp[0]);
   }
   
   set_reg_b(REGB_AMPL_L, (unsigned int8)(r_l * 4.2058));
   set_reg_b(REGB_AMPL_B, (unsigned int8)(r_b * 4.2058));
   set_reg_b(REGB_FREQ_L, (unsigned int8)(nu_l * FREQ_CORRECT * 63.8));
   set_reg_b(REGB_FREQ_B, (unsigned int8)(nu_b * FREQ_CORRECT * 63.8));
   
   return(1);
}


///---UPDATE_AMPHIBOT------
///
/** **/
int1 update_amphibot(float spd, float dir)
{
   float FREQ_CORRECT = 1.8;
   
   if(current_robot == 3)
   {
      FREQ_CORRECT = 1.0;
   }
   
   
   if(current_locomotion_mode == SWIMMING)
   {
      set_reg_b(REGB_FREQ, (unsigned int8)((spd*0.75 + 2) * FREQ_CORRECT * 63.8));
      set_reg_b(REGB_PHASE, (unsigned int8)(127.5));
      
      if(spd>0.1)
      {
         set_reg_b(REGB_AMPL, (unsigned int8)(55*4.2058));
      }
      else
      {
         set_reg_b(REGB_AMPL, (unsigned int8)((spd*300 + 0.1)*4.2058));
      }
   }
   else
   {
      set_reg_b(REGB_AMPL, (unsigned int8)(0.1 * 4.2058));
   }
   
   set_reg_b(REGB_TURN, (unsigned int8)(-dir * 127.0));
   
   return(1);
}


///---UPDATE_AMPHIBOT_MATHIEU------
///
/** **/
int1 update_amphibot_mathieu(float spd, float dir)
{
   if(current_locomotion_mode == SWIMMING)
   {
      set_reg_b(2, (unsigned int8)(((0.75*spd) - (-1.0)) / 2.0 * 255.0));
      set_reg_b(4, (unsigned int8)(0.25*255.0));
      
      if(spd > 0.1)
      {
         set_reg_b(3, (unsigned int8)((30.0/50.0)*255.0));
         set_reg_b(10, (unsigned int8)((30.0/50.0)*255.0));
      }
      else
      {
         set_reg_b(3, (unsigned int8)(((spd*300.0 + 0.1)/(50.0))*255.0));
         set_reg_b(10, (unsigned int8)(((spd*300.0 + 0.1)/(50.0))*255.0));
      }
   }
   else
   {
      set_reg_b(3, (unsigned int8)((0.1/50.0)*255.0));
      set_reg_b(10, (unsigned int8)((0.1/50.0)*255.0));
   }
   
   set_reg_b(9, (unsigned int8)(((dir + 1.0)/2.0) * 255.0));
   return(1);
}


///---FROM_TURN------------
///
/** **/
int8 from_turn(float d)
{
   if(d < (-1))
   {
      d = -1.0;
   }
   if(d > 1)
   {
      d = 1.0;
   }
   
   return((unsigned int8)(floor((d +1.0) * 7.0)) + 1);
}


///---SET_FLAT_GAINS-------
///
/** **/
int1 set_flat_gains(float gain)
{
   int8 elc=0;
   int8 i=0;
   int8* dt;
   int1 res=0;
   float factor = 100.0; // 245.0 for snakes and 100.0 for salamanders
   
   if(current_robot == 1 || current_robot == 3)
   {
      factor = 254.0;
   }
   
   get_reg_b(REGB_ELCOUNT,&elc);
   
   for(i=0;i<elc;++i)
   {
      dt[i] = (unsigned int8)(gain * factor);
   }
   
   for(i=0; i<5; ++i)
   {
      res = set_reg_mb(REGMB_GAINS, dt, elc);
      if(res)
      {
         break;
      }
   }
   
   return(res);
   
}


///---SET_PHASE------------
///
/** **/
void set_phase(float phase){
   set_reg_b(REGB_PHASE, (int8)(phase * 127.5));
}










/***************************
   * START/STOP THE ROBOT  ****************************************************
   *************************/
   
/// ---START_ROBOT --------
///
//* *//
int1 start_salamander()
{
   int8 mode;
   int8 elc;
   int8 i;
   char text[16];
   
   turbo = 0;
   current_locomotion_mode = WALKING;
   
   output_high(PIN_C1);
   clear_display();
   strcpy(text, "Checking remote ");
   display(text,0x00);
   strcpy(text,"robot type...   ");
   display(text,0x40);
   set_pos_display(0x50);
   
   get_reg_b(REGB_VER, &i);
   if(i==5 || i==2)
   {
      strcpy(text,"OK");
      display(text,0x4E);
   }
   else
   {
      sprintf(text,"Incorrect ver.%u",i);
      err(text);
      output_low(PIN_C1);
      // Time for the user to read the error (reading time)
      delay_ms(1000);
      return(0);
   }
   
   set_reg_b(REGB_AMPL_B,0);
   set_reg_b(REGB_AMPL_L,0);
   set_reg_b(REGB_FREQ_B,0);
   set_reg_b(REGB_FREQ_L,0);
   set_reg_b(REGB_TURN,(from_turn(0) << 4) | from_turn(0));
   
   get_reg_b(REGB_ELCOUNT, &elc);
   clear_display();
   strcpy(text,"Detected element");
   display(text,0x00);
   sprintf(text,"count: %u",elc);
   display(text,0x40);
   
   // Reading time
   delay_ms(500);

   get_reg_b(REGB_MODE, &mode);
   if(*mode == 5)
   {
      clear_display();
      strcpy(text,"Robot already   ");
      display(text,0x00);
      strcpy(text,"initialized.");
      display(text,0x40);
   }
   
   else
   {
      clear_display();
      strcpy(text,"Initializing el.");
      display(text,0x00);
      set_reg_b(REGB_MODE,1);
      get_reg_b(REGB_MODE, &mode);
      while(mode == 1)
      {
         delay_ms(100);
         get_reg_b(REGB_MODE, &mode);
      }
      strcpy(text,"OK");
      display(text,0x4E);
      
      // Reading time
      delay_ms(500);
      
      clear_display();
      strcpy(text,"Enabling CPG...");
      display(text,0x00);
      set_pos_display(0x50);
      
      for(i=0;i<3;++i)
      {
         set_reg_b(REGB_MODE, 5);
         delay_ms(5);
         get_reg_b(REGB_MODE,&mode);
         if(mode == 5)
         {
            break;
         }
      }
      
      if(mode == 5)
      {
         strcpy(text,"OK");
         display(text,0x4E);
      }
      else
      {
         strcpy(text,"FAILED");
         display(text,0x4A);
      }
      
      // Reading time
      delay_ms(500);
      
      //set_flat_gains(0.8);
      set_reg_b(REGB_PHASE, (unsigned int8)127.5); 
   }
   
   get_reg_b(REGB_MODE, &mode);
   output_low(PIN_C1);
   
   return(mode == 5);
   
}

int1 start_amphibot()
{
   int8 mode;
   int8 elc;
   int8 i;
   char text[16];
   
   turbo = 0;
   current_locomotion_mode = SWIMMING;
   
   output_high(PIN_C1);
   clear_display();
   strcpy(text, "Checking remote ");
   display(text,0x00);
   strcpy(text,"robot type...   ");
   display(text,0x40);
   set_pos_display(0x50);
   
   get_reg_b(REGB_VER, &i);
   if(i==2)
   {
      strcpy(text,"OK");
      display(text,0x4E);
   }
   else
   {
      sprintf(text,"Incorrect ver.%u",i);
      err(text);
      // reading time
      delay_ms(1000);
      return(0);
   }
   
   set_reg_b(REGB_AMPL,0);
   set_reg_b(REGB_FREQ,0);

   get_reg_b(REGB_ELCOUNT, &elc);
   clear_display();
   strcpy(text,"Detected element");
   display(text,0x00);
   sprintf(text,"count: %u",elc);
   display(text,0x40);
   
   // Reading time
   delay_ms(500);
   
   get_reg_b(REGB_MODE, &mode);
   if(*mode == 5)
   {
      clear_display();
      strcpy(text,"Robot already   ");
      display(text,0x00);
      strcpy(text,"initialized.");
      display(text,0x40);
   }
   
   else
   {
      clear_display();
      strcpy(text,"Initializing el.");
      display(text,0x00);
      set_reg_b(REGB_MODE,1);
      get_reg_b(REGB_MODE, &mode);
      while(mode == 1)
      {
         delay_ms(100);
         get_reg_b(REGB_MODE, &mode);
      }
      strcpy(text,"OK");
      display(text,0x4E);
      
      // Reading time
      delay_ms(500);
      
      clear_display();
      strcpy(text,"Enabling CPG...");
      display(text,0x00);
      set_pos_display(0x50);
      
      for(i=0;i<3;++i)
      {
         set_reg_b(REGB_MODE, 2);
         delay_ms(5);
         get_reg_b(REGB_MODE,&mode);
         if(mode == 2)
         {
            break;
         }
      }
      
      if(mode == 2)
      {
         strcpy(text,"OK");
         display(text,0x4E);
      }
      else
      {
         strcpy(text,"FAILED");
         display(text,0x4A);
      }
      
      // Reading time
      delay_ms(500);
      
      //set_flat_gains(0.8);
      set_reg_b(REGB_PHASE, (unsigned int8)127.5); 
   }
   
   get_reg_b(REGB_MODE, &mode);
   output_low(PIN_C1);
   
   return(mode == 2);
   
}

int1 start_amphibot_mathieu()
{
   int8 mode;
   int8 elc;
   int8 i;
   char text[16];
   
   turbo = 0;
   current_locomotion_mode = SWIMMING;
   
   output_high(PIN_C1);
   clear_display();
   /*
   strcpy(text, "Checking remote ");
   display(text,0x00);
   strcpy(text,"robot type...   ");
   display(text,0x40);
   set_pos_display(0x50);
   
   get_reg_b(REGB_VER, &i);
   if(i==2)
   {
      strcpy(text,"OK");
      display(text,0x4E);
   }
   else
   {
      sprintf(text,"Incorrect ver.%u",i);
      err(text);
      return(0);
   }
   */
   set_reg_b(3, 0);   //< set_ampl(0.0)   
   set_reg_b(10, 0);  
   set_reg_b(2, 0);   //< set_freq(0.0)

   get_reg_b(1, &elc);
   clear_display();
   strcpy(text,"Detected element");
   display(text,0x00);
   sprintf(text,"count: %u",elc);
   display(text,0x40);
   
   // Reading time
   delay_ms(500);
   
   get_reg_b(REGB_MODE, &mode);
   if(mode == 7) //< MODE_CPG = 7
   {
      clear_display();
      strcpy(text,"Robot already   ");
      display(text,0x00);
      strcpy(text,"initialized.");
      display(text,0x40);
   }
   
   else
   {
      clear_display();
      strcpy(text,"Initializing el.");
      display(text,0x00);
      set_reg_b(REGB_MODE,1);
      get_reg_b(REGB_MODE, &mode);
      while(mode == 1)
      {
         delay_ms(100);
         get_reg_b(REGB_MODE, &mode);
      }
      strcpy(text,"OK");
      display(text,0x4E);
      
      // Reading time
      delay_ms(500);

      set_reg_b(5,255); //< set REG8_COUPLING to the maximum value
      set_reg_b(6,(unsigned int8)(0.2*255.0)); //< set REG8_TRANS_SPEED to 20%
      
      clear_display();
      strcpy(text,"Enabling CPG...");
      display(text,0x00);
      set_pos_display(0x50);
      
      for(i=0;i<3;++i)
      {
         set_reg_b(REGB_MODE, 7);
         delay_ms(100);
         get_reg_b(REGB_MODE,&mode);
         if(mode == 7)
         {
            break;
         }
      }
      
      if(mode == 7)
      {
         strcpy(text,"OK");
         display(text,0x4E);
      }
      else
      {
         strcpy(text,"FAILED");
         display(text,0x4A);
      }
      
      // Reading time
      delay_ms(500);
      
      set_reg_b(4, (unsigned int8)(0.5 * 255.0)); //< set_phase(1.0) while MAX_NWAVE = 2.0 and MIN_NWAVE = 0.0
      
   }
   
   get_reg_b(REGB_MODE, &mode);
   output_low(PIN_C1);
   
   return(mode == 7);
   
}


/// ---STOP_ROBOT ----------
///
//* *//
int1 stop_salamander()
{
   int8 mode;
   
   // Indicate to process of the function (sort of 'WAIT')
   output_high(PIN_C1);
  
   set_reg_b(REGB_AMPL_L, 0);
   set_reg_b(REGB_AMPL_B, 0);
   set_reg_b(REGB_FREQ_L, 0);
   set_reg_b(REGB_FREQ_B, 0);
   
   delay_ms(500);
   
   set_reg_b(REGB_MODE, 3);
   get_reg_b(REGB_MODE, &mode);
   while(mode != 0)
   {
      delay_ms(100);
      get_reg_b(REGB_MODE, &mode);
   }
   
   output_low(PIN_C1);
   return(1);
}

int1 stop_amphibot()
{
   int8 mode=25;
   output_high(PIN_C1);
   
   set_reg_b(REGB_MODE, MODE_STOP);
   get_reg_b(REGB_MODE, &mode);
   while(mode != MODE_STOP)
   {
      delay_ms(100);
      get_reg_b(REGB_MODE, &mode);
   }
   
   set_reg_b(REGB_MODE, MODE_IDLE);
   get_reg_b(REGB_MODE, &mode);
   while(mode != MODE_IDLE)
   {
      delay_ms(100);
      get_reg_b(REGB_MODE, &mode);
   }
   
   output_low(PIN_C1);
   return(1);
}

int1 stop_amphibot_mathieu()
{
   int8 mode=25;
   output_high(PIN_C1);
   
   set_reg_b(REGB_MODE, MODE_IDLE);
   get_reg_b(REGB_MODE, &mode);
   while(mode != MODE_IDLE)
   {
      delay_ms(100);
      get_reg_b(REGB_MODE, &mode);
   }
   
   set_reg_b(REGB_MODE, 3);    //< stop the PID controllers
   get_reg_b(REGB_MODE, &mode);
   while(mode != 3)
   {
      delay_ms(100);
      get_reg_b(REGB_MODE, &mode);
   }
   
   set_reg_b(REGB_MODE, 0);
   get_reg_b(REGB_MODE, &mode);
   while(mode != 0)
   {
      delay_ms(100);
      get_reg_b(REGB_MODE, &mode);
   }
   
   output_low(PIN_C1);
   return(1);
}

//---SYNC-------------------
/// Function SYNC() tests the synchronization by emitting 0xAA and waiting for the radio to answer 0xAA
/** @PARAM void
   @RETURN 'TRUE' (1) or 'FALSE' (0) **/
int1 sync()
{
   int8 b=0xFF;
   unsigned int i=0;
   
   for (i=0;i<24;++i){
      putchar(0xFF);
   }
   
   b=0xAA;
   putchar(0xAA);
   i=0;

   while(!kbhit() && i<50)
   {
      delay_ms(1);
      i += 1;
   }

   if(kbhit())
   {
      return(getchar() == 0xAA);
   }
   
   return(0);
}


/************************
 * REGISTERS OPERATIONS *******************************************************
 ************************/
 
//---REG_OP-----------------
/// Function REG_OP(int8 op, int16 addr, int8 *data, int len) 
/**@PARAM 
   @PARAM int16 addr : is the address of the register
   @RETURN 'TRUE' (1) or 'FALSE' (0) **/
int1 reg_op(int8 op, int16 addr, int8* data, int len)
{
   int8 confirm = 13;
   unsigned int i;
   int8 req[32];
   
   req[0] = (op << 2) | ((addr & 0x300) >> 8);
   req[1] = (addr & 0xFF);
   for(i=0;i<len;++i)
   {
      req[i+2] = data[i];
   }
   
   for(i=0 ; i < len+2;++i)
   {
      putc(req[i]);
   }
   
   confirm = getc();
   return(confirm == 6);
}


//---GET_REG_B--------------
/// Function GET_REG_B(int16 address, int8* result) is a getter for a 8 bits register located at the specified address 
/** @PARAM int16 addr : is the address of the register
   @PARAM int8* result : are the 8 bits contained in the register
   @RETURN 'TRUE' (1) or 'FALSE' (0) **/
int1 get_reg_b(int16 addr, int8* result){
   if(reg_op(0x00, addr, 0, 0))
   {
      *result = getc();
      return(1);
   }
   
   else
   {
      return(0);
   }
      
}


//---GET_REG_W--------------
/// Function GET_REG_W(int16 address, int16* result) is a getter for a 16 bits register located at the specified address 
/** @PARAM int16 addr : is the address of the register
   @PARAM int16* result : are the 16 bits contained in the register 
   @RETURN 'TRUE' (1) or 'FALSE' (0) **/
int8 get_reg_w(int16 addr, int16* result)
{
   int8 buffer[2];
   
   if(reg_op(0x01, addr, 0, 0))
   {
      buffer[0] = getc();
      buffer[1] = getc();
      
      *result = buffer;
      return(1);
   }
   
   else
   {
      return(0);
   }
   
}
   

//---GET_REG_DW--------------
/// Function GET_REG_DW(int16 address, int32* result) is a getter for a 32 bits register located at the specified address 
/** @PARAM int16 addr : is the address of the register
   @PARAM int32* result : are the 32 bits contained in the register
   @RETURN 'TRUE' (1) or 'FALSE' (0) **/
int8 get_reg_dw(int16 addr, int32* result)
{
   int8 buffer[4];
   unsigned int i;
   if(reg_op(0x02, addr, 0, 0))
   {
      for(i=0;i<4;++i)
      {
         buffer[i]=getc();
      }
      *result = buffer;
      return(1);
   }
   
   else
   {
      return(0);
   }
}


//---GET_REG_MB--------------
/// Function GET_REG_MB(int16 address, int8 &len, int8* result) is a getter for a various length's register at the specified address
/** @PARAM int16 addr : is the address of the register
   @PARAM int8 len : the length of the register
   @RETURN int8 : the bits contained in the register **/
int1 get_reg_mb(int16 addr, int8 &len, int8* result)
{
   unsigned int i;
   int8 buffer[32];
   
   if(reg_op(0x03, addr, 0, 0))
   {
      len = getc();
   
      for(i=0;i<len;++i)
      {
         buffer[i] = getc(); 
      }
   
      /////////////////////////////
      // ATTENTION, A VERIFIER !!!/
      /////////////////////////////
      *result=buffer;
      return(1);
   }
   
   else
   {
      return(0);
   }
}
   
   
//---SET_REG_B--------------
/// Function SET_REG_B(int16 address, int8 data) is a setter for a 8 bits register located at the specified address
/** @PARAM int16 addr : is the address of the register
   @PARAM int8 data : are the data to be written in the register
   @RETURN 'TRUE' (1) or 'FALSE' (0) **/
int1 set_reg_b(int16 addr, int8 data)
{
   return(reg_op(0x04, addr, &data, 1));
}


//---SET_REG_W--------------
/// Function SET_REG_W(int16 address, int16 data) is a setter for a 16 bits register located at the specified address
/** @PARAM int16 addr : is the address of the register
   @PARAM int16 data : are the data to be written in the register
   @RETURN 'TRUE' (1) or 'FALSE' (0) **/
int1 set_reg_w(int16 addr, int16 data)
{
   return(reg_op(0x05, addr, (int8 *) &data, 2));
}


//---SET_REG_DW-------------
/// Function SET_REG_DW(int16 address, int32 data) is a setter for a 32 bits register located at the specified address
/** @PARAM int16 addr : is the address of the register
   @PARAM int32 data : are the data to be written in the register
   @RETURN 'TRUE' (1) or 'FALSE' (0) **/
int1 set_reg_dw(int16 addr, int32 data)
{
   return(reg_op(0x06, addr, (int8 *) &data, 4));
}


//---SET_REG_MB-------------
/// Function SET_REG_MB(int16 address, int8* data, int8 len) is a setter for a multi bytes register located at the specified address
/** @PARAM int16 addr : is the address of the register
   @PARAM int8* data : are the data to be written in the register
   @PARAM int8 len : is the length of the data (number of bytes)
   @RETURN 'TRUE' (1) or 'FALSE' (0) **/
int1 set_reg_mb(int16 addr, int8* data, int8 len)
{
   int8 buffer[32];
   unsigned int i;
   
   /// Tests the length : cannot be over 29 (cf. structure of a multi-bytes reg)
   if(len>29)
   {
      return(0);
   }
   
   buffer[0] = len;
   for(i=0;i<len;++i)
   {
      buffer[i+1]=data[i];
   }
   
   return(reg_op(0x07, addr, buffer, len));
}


//---SCAN-------------------
/// Function SCAN(int8* local_channel)
/** @PARAM int8* local_channel : Number of the channel on wich the communication 
                                 with the robot has been established
    @RETURN 'TRUE' (1) when a channel has been found or 'FALSE' (0) when not **/
int1 scan(int8* local_channel)
{
   char text[16];
   int8 remote_channel = 0xFF;
   int8 number_contiguous = 0;
   *local_channel = 0x00;
   
   do
   {
      clear_display();
      strcpy(text,"Scanning...");
      display(text,0x00);
      sprintf(text, "channel %3u", *local_channel);
      display(text,0x40);
      set_pos_display(0x10);
         
      set_reg_b(REG_INTF_CH,*local_channel);
      
      if(!get_reg_b(REG_RWL_CH, &remote_channel))
      {
         *local_channel = *local_channel + 1;
      }
   
      else
      {
         for(number_contiguous=1; ;++number_contiguous)
         {
            set_reg_b(REG_INTF_CH,((*local_channel) + (number_contiguous-1)));
            if(!get_reg_b(REG_RWL_CH, &remote_channel))
            {
               break;
            }
         }
         
         --number_contiguous;
         
         if(number_contiguous > 1)
         {
            *local_channel = *local_channel + (number_contiguous - 1) / 2;
         }
         
         /**Display the description of the robot, contained in the multi-byte
             register 0xXXX on the LCD**/
         clear_display();
         sprintf(text,"Robot on ch. %u",*local_channel);
         display(text,0x00);
         
         // Description of the robot (16 characters)
         //sprintf(text,"%s",get_reg_mb(a completer));
         //display(text,0x40);
         set_pos_display(0x50);
                  
         if(enter_cancel(20))
         {
            
            return(1);
         }
         
         else
         {
            if(number_contiguous > 1)
            {
               *local_channel = *local_channel + (number_contiguous - 1) / 2;
            }
            *local_channel = *local_channel + 1;
         }
      }
   } while(*local_channel > 0x00 && input(BTN_CANCEL));
   
   /// If none of the channels have been responding
   return(0);
}