void follow_sensor_calibrate() {
int i, j;
extern char buffer[60];
long sensor[8];
for (i=0; i<8; i++) {
sensor[i]=0;
}
for (j=0; j<32; j++) {
for (i=0; i<8; i++) {
sensor[i]+=e_get_prox(i);
}
wait(10000);
}
for (i=0; i<8; i++) {
follow_sensorzero[i]=(sensor[i]>>5);
sprintf(buffer, "%d, ", follow_sensorzero[i]);
e_send_uart1_char(buffer, strlen(buffer));
}
sprintf(buffer, " calibration done\r\n");
e_send_uart1_char(buffer, strlen(buffer));
wait(100000);
}
/*! \brief The function to send char values to matlab
* \param data The array of char data you want to send
* \param array_size The length of the array
*/
void e_send_char_to_matlab(char* data, int array_size) {
e_send_uart1_char("C",1);
e_send_uart1_char((char *) &array_size, sizeof(int));
e_send_uart1_char(data, array_size);
e_send_uart1_char("EOF\0",4);
return;
}
/*! \brief The function to send int values to matlab
* \param data The array of int data you want to send
* \param array_size The length of the array
*/
void e_send_int_to_matlab(int* data, int array_size) {
int size = 2*array_size;
e_send_uart1_char("I",1);
e_send_uart1_char((char *) &size, sizeof(int));
e_send_uart1_char((char*)data, size);
e_send_uart1_char("EOF\0",4);
return;
}
void ve_send_int(int integer)
{
char message[2];
message[0] = integer & 0x00FF;
message[1] = (integer & 0xFF00) >> 8;
e_send_uart1_char(message, SIZEOF_CHAR);
while( e_uart1_sending() ){}
e_send_uart1_char(message+1, SIZEOF_CHAR);
while( e_uart1_sending() ){}
}
// *** behaviour insired by a dust cleaner
void run_DustCleaner() {
int i;
int sensor;
int leftwheel, rightwheel;
int spiral=100;
int weightleft[8] = {-10,-10,-5,0,0,5,10,10};
int weightright[8] = {10,10,5,0,0,-5,-10,-10};
while (1) {
e_led_clear();
e_set_body_led(0);
e_set_front_led(0);
if (e_get_prox(0)>600)
e_set_led(0,1);
if (e_get_prox(1)>600)
e_set_led(1,1);
if (e_get_prox(2)>600)
e_set_led(2,1);
if (e_get_prox(3)>600)
e_set_led(3,1);
if (e_get_prox(4)>600)
e_set_led(4,1);
if (e_get_prox(5)>600)
e_set_led(5,1);
if (e_get_prox(6)>600)
e_set_led(6,1);
if (e_get_prox(7)>600)
e_set_led(7,1);
if (e_get_prox(0)>600 || e_get_prox(1)>600 || e_get_prox(2)>600 ||
e_get_prox(5)>600 || e_get_prox(6)>600 || e_get_prox(7)>600 ){ // obstacle
e_set_body_led(1);
// *** avoid
leftwheel=200;
rightwheel=200;
for (i=0; i<8; i++) {
sensor=e_get_prox(i); //-sensorzero[i];
sprintf(buffer, "%d, ", sensor);
e_send_uart1_char(buffer, strlen(buffer));
leftwheel+=weightleft[i]*(sensor>>4);
rightwheel+=weightright[i]*(sensor>>4);
}
sprintf(buffer, "setspeed %d %d\r\n", leftwheel, rightwheel);
e_send_uart1_char(buffer, strlen(buffer));
if (leftwheel>800) {leftwheel=800;}
if (rightwheel>800) {rightwheel=800;}
if (leftwheel<-800) {leftwheel=-800;}
if (rightwheel<-800) {rightwheel=-800;}
e_set_speed_left(leftwheel);
e_set_speed_right(rightwheel);
} else { // spiral
void run_translatorI2C() {
unsigned char mod, reg, val;
uart_send_static_text("\f\a"
"WELCOME to the RS232 - I2C translator on e-Puck\r\n"
" 1 byte protocol ASL-EPFL 2006\r\n");
e_i2cp_init();
while (1) {
e_getchar_uart1(&mod); // #module
mod=(unsigned char) (mod<<1);
e_getchar_uart1(®); // #register
if (reg>127) { // read
e_i2cp_enable();
val= e_i2cp_read(mod, reg&0x7f); // read I2C
e_i2cp_disable();
e_send_uart1_char(&val, 1);
} else { // write
e_getchar_uart1(&val); // #value
e_i2cp_enable();
e_i2cp_write(mod, reg, val); // write I2C
e_i2cp_disable();
}
}
}
void sendNewLine() {
char buffer[3];
sprintf(buffer, "\n\r\0");
e_send_uart1_char(buffer, strlen(buffer));
while(e_uart1_sending());
}
void sendLong(long l) {
char buffer[25];
sprintf(buffer, "%ld \0",l);
e_send_uart1_char(buffer, strlen(buffer));
while(e_uart1_sending());
}
void sendPointer(float* i) {
char buffer[25];
sprintf(buffer, "%p \0",i);
e_send_uart1_char(buffer, strlen(buffer));
while(e_uart1_sending());
}
void sendInt(int i) {
char buffer[25];
sprintf(buffer, "%i \0",i);
e_send_uart1_char(buffer, strlen(buffer));
while(e_uart1_sending());
}
void sendDouble(float i) {
char buffer[25];
sprintf(buffer, "%f \0",i);
e_send_uart1_char(buffer, strlen(buffer));
while(e_uart1_sending());
}
void epuckPlayer()
{
//e_start_agendas_processing();
//e_init_motors();
//e_init_prox();
//e_init_uart1();
/* Must send anything here or it don't work. Is it a bug? */
e_send_uart1_char("epuckSide_v3.0", 14);
unsigned a,b;
/* Flash the LED's in a singular manner for show that this program is in
* epuck memory */
for(a=0; a<8; a++)
for(b=0; b<20000; b++)
e_set_led(a ,1); /* LED ON */
for(a=0; a<8; a++)
for(b=0; b<20000; b++)
e_set_led(a ,0); /* LED OFF */
char command;
while(1)
{
command = recv_char();
switch(command)
{
case 0x13:
recv_vel();
break;
case 0x14:
send_steps();
break;
case 0x16:
read_ir_sensors();
break;
case 0x15:
stop_motors();
break;
case 0x17:
read_camera();
break;
case 0x18:
set_LEDs();
break;
case 0x01:
sendVersion();
break;
case 0x02:
config_camera();
break;
}
}
}
void run_SensDispl() {
int cam_mode,cam_width,cam_heigth,cam_zoom,cam_size;
int i;
unsigned long cam_light;
unsigned char *buf_ptr;
/*Cam default parameter*/
cam_mode=GREY_SCALE_MODE;
cam_width=20;
cam_heigth=20;
cam_zoom=8;
cam_size=cam_width*cam_heigth;
e_poxxxx_init_cam();
e_poxxxx_config_cam((ARRAY_WIDTH -cam_width*cam_zoom)/2,(ARRAY_HEIGHT-cam_heigth*cam_zoom)/2,cam_width*cam_zoom,cam_heigth*cam_zoom,cam_zoom,cam_zoom,cam_mode);
e_poxxxx_set_mirror(1,1);
e_poxxxx_write_cam_registers();
e_acc_calibr();
while (1) {
e_poxxxx_launch_capture(&buffer[0]); // start camera capture
e_led_clear();
e_set_body_led(0);
e_set_front_led(0);
if (e_get_micro_volume(0)>30 || e_get_micro_volume(1)>30 || e_get_micro_volume(2)>30)
e_set_body_led(1);
if (e_get_prox(0)>400)
e_set_led(0,1);
if (e_get_prox(1)>400)
e_set_led(1,1);
if (e_get_prox(2)>400)
e_set_led(2,1);
if (e_get_prox(3)>400)
e_set_led(3,1);
if (e_get_prox(4)>400)
e_set_led(4,1);
if (e_get_prox(5)>400)
e_set_led(5,1);
if (e_get_prox(6)>400)
e_set_led(6,1);
if (e_get_prox(7)>400)
e_set_led(7,1);
while(!e_poxxxx_is_img_ready()); // wait end of capture
cam_light=0;
buf_ptr=(unsigned char*)&buffer[0];
for (i=0; i<cam_size; i++) {
cam_light+=*buf_ptr;
buf_ptr++;
}
sprintf(buffer, "Cam light %lu\r\n", cam_light);
e_send_uart1_char(buffer, strlen(buffer));
if (cam_light>48000) // 20*20pixels*120grayValue
e_set_front_led(1);
wait(5000);
}
}
void run_braitenberg() {
int i;
int sensor;
char buffer[80];
int leftwheel, rightwheel;
// Init sensors
e_init_port();
e_init_motors();
e_init_prox();
// Calibrate sensors
e_set_led(8, 1);
braitenberg_sensor_calibrate();
e_set_led(8, 0);
//
while (1) {
leftwheel=200;
rightwheel=200;
for (i=0; i<8; i++) {
sensor=e_get_prox(i)-braitenberg_sensorzero[i];
sprintf(buffer, "%d, ", sensor);
e_send_uart1_char(buffer, strlen(buffer));
leftwheel+=braitenberg_weightleft[i]*(sensor>>4);
rightwheel+=braitenberg_weightright[i]*(sensor>>4);
}
sprintf(buffer, "setspeed %d %d\r\n", leftwheel, rightwheel);
e_send_uart1_char(buffer, strlen(buffer));
if (leftwheel>800) {leftwheel=800;}
if (rightwheel>800) {rightwheel=800;}
if (leftwheel<-800) {leftwheel=-800;}
if (rightwheel<-800) {rightwheel=-800;}
e_set_speed_left(leftwheel);
e_set_speed_right(rightwheel);
wait(100000);
}
}
int main()
{
e_init_port();
e_init_uart1();
int i;
for (i = 0; i < 14; i++)
{
char c;
while (!e_ischar_uart1());
e_getchar_uart1(&c);
}
while (1)
{
char c;
while (!e_ischar_uart1());
e_getchar_uart1(&c);
e_send_uart1_char(&c, 1);
while (e_uart1_sending());
}
return 0;
}
void run_CameraTurn() {
int cam_mode,cam_width,cam_heigth,cam_zoom,cam_size;
int i;
unsigned char *buf_ptr, pixel, lightest;
unsigned int left, right, lightPos;
#include "DataEEPROM.h"
/*read HW version from the eeprom (last word)*/
int HWversion=0xFFFF;
int temp = 0;
temp = ReadEE(0x7F,0xFFFE,&HWversion, 1);
temp = temp & 0x03; // get the camera rotation from the HWversion byte
/*Cam default parameter*/
cam_mode=GREY_SCALE_MODE;
if ((temp==3)||(temp==0)) { // 0' and 180' camera rotation
cam_width=1;
cam_heigth=60;
} else {
cam_width=60;
cam_heigth=1;
}
cam_zoom=8;
cam_size=cam_width*cam_heigth;
e_poxxxx_init_cam();
e_poxxxx_config_cam((ARRAY_WIDTH -cam_width*cam_zoom)/2,(ARRAY_HEIGHT-cam_heigth*cam_zoom)/2,cam_width*cam_zoom,cam_heigth*cam_zoom,cam_zoom,cam_zoom,cam_mode);
e_poxxxx_set_mirror(1,1);
e_poxxxx_write_cam_registers();
while (1) {
e_poxxxx_launch_capture(&buffer[0]); // start camera capture
e_led_clear();
e_set_body_led(0);
e_set_front_led(0);
while(!e_poxxxx_is_img_ready()); // wait end of capture
buf_ptr=(unsigned char*)&buffer[0];
left=0; right=0; lightPos=0; lightest=0;
for (i=0; i<30; i++) { //left
pixel=*buf_ptr;
buf_ptr++;
left+=pixel;
if (pixel>lightest) {
lightest=pixel;
lightPos=i;
}
}
for (; i<cam_heigth; i++) { //right
pixel=*buf_ptr;
buf_ptr++;
right+=pixel;
if (pixel>lightest) {
lightest=pixel;
lightPos=i;
}
}
if (lightPos<20) { //led on at lightest position
e_set_led(7,1); }
else if (lightPos<40) {
e_set_led(0,1); }
else {
e_set_led(1,1); }
if ((temp==3)||(temp==2)) { // 0' and 90' camera rotation
e_set_speed_left(10*(lightPos-30)); // motor speed in steps/s
e_set_speed_right(-10*(lightPos-30));
} else {
e_set_speed_left(-10*(lightPos-30)); // motor speed in steps/s
e_set_speed_right(10*(lightPos-30));
}
sprintf(buffer, "left %u, right %u, lightest %u, lightPos %u\r\n", left, right, lightest, lightPos);
e_send_uart1_char(buffer, strlen(buffer));
wait(5000);
}
}
void btcomSendStringStatic (char* buffer)
{
e_send_uart1_char(buffer, sizeof(*buffer) - 1);
while(e_uart1_sending());
}
void sendString2(char* s, int len) {
e_send_uart1_char(s, len);
while(e_uart1_sending());
}
void sendString(char* s) {
e_send_uart1_char(s, mystrlen(s));
while(e_uart1_sending());
}
void btcomSendString (char* buffer)
{
e_send_uart1_char(buffer, strlen(buffer));
while(e_uart1_sending());
}
void btcomSendChar (char c)
{
e_send_uart1_char(&c, 1);
while(e_uart1_sending());
}
void transmitShort(unsigned short i) {
e_send_uart1_char(&i,2);
while(e_uart1_sending());
}
void send_char(char character)
{
e_send_uart1_char(&character, SIZEOF_CHAR);
while(e_uart1_sending());
}
int main(void) {
// Locals General. -----------------------------------------------------
char message[50]; // Any message to send by UART1
char command[20], response[50];
int c;
int i, version;
// Init Oscillator. ----------------------------------------------------
InitOscillator(); // Initialize the PLL (also disables wdt)
WaitMiliSec(50);
// Init mcu ports ------------------------------------------------------
init_port(); // Initialize ports
// Init UARTS. ---------------------------------------------------------
init_UART1(); // Initialize the serial communication (TTL / RS-232)
init_UART2(); // Initialize the serial communication (TTL / RS-232)
LED_ORNG=0;
// LED_ORNG =1;
// Welcome Message-----------------------------------------------------
uart1_send_static_text("\n\n\rWELCOME to CoaX Bluetooth configuration"); // Welcome msg
uart1_send_static_text("\n\rPress H (return) for help");
while(1)
{
i = 0;
c=0;
do
{
if (e_getchar_uart1(&command[i]))
{
c=command[i];
i++;
}
}
while (((char)c != '\n')&&((char)c != '\x0d'));
command[i]='\0';
switch (command[0])
{
case 'P': e_bt_read_local_pin_number(message);
sprintf(response,"\n\rPIN code = %s",message);
break;
case 'O': sscanf(command,"O,%s\n",message);
if(e_bt_write_local_pin_number(message))
sprintf(response,"\n\rError writting PIN");
else
sprintf(response,"\n\rPIN code = %s",message);
break;
case 'M': sscanf(command,"M,%s\n",message);
if(e_bt_write_local_name(message))
sprintf(response,"\n\rError writting Name");
else
sprintf(response,"\n\rFriendly name = %s",message);
break;
case 'S': sscanf(command,"S,%s\n",message);
if(e_bt_write_local_pin_number(message))
sprintf(response,"\n\rError writting PIN");
else
sprintf(response,"\n\rPIN code = %s",message);
e_send_uart1_char(response,strlen(response));
while(e_uart1_sending());
sprintf(command,"CoaX_%s",message);
if(e_bt_write_local_name(command))
sprintf(response,"\n\rError writting Name");
else
sprintf(response,"\n\rFriendly name = %s",command);
break;
case 'N': e_bt_read_local_name(message);
sprintf(response,"\n\rFriendly name = %s",message);
break;
case 'R': version=e_bt_reset();
sprintf(response,"\n\rReset ok Firmware = %d",version);
break;
case 'H': uart1_send_static_text("\n\r \"M,Name\" Write Name for Friendly Bluetooth name");
uart1_send_static_text("\n\r \"N\" Read actual Friendly Bluetooth name");
uart1_send_static_text("\n\r \"O,#\" Write # PIN number");
uart1_send_static_text("\n\r \"P\" Read actual PIN number");
uart1_send_static_text("\n\r \"R\" Soft reset Bluetooth module");
uart1_send_static_text("\n\r \"S,#\" Write # PIN number and same time CoaX_#");
response[0]='\n';
response[1]='\0';
break;
default: sprintf(response,"\n\rz,Command not found");
break;
}
e_send_uart1_char(response,strlen(response));
while(e_uart1_sending());
}
}
int run_asercom(void) {
static char c1,c2,wait_cam=0;
static int i,j,n,speedr,speedl,positionr,positionl,LED_nbr,LED_action,accx,accy,accz,sound;
static int cam_mode,cam_width,cam_heigth,cam_zoom,cam_size,cam_x1,cam_y1;
static char first=0;
char *ptr;
static int mod, reg, val;
#ifdef IR_RECEIVER
char ir_move = 0,ir_address= 0, ir_last_move = 0;
#endif
static TypeAccSpheric accelero;
//static TypeAccRaw accelero_raw;
int use_bt=0;
//e_init_port(); // configure port pins
//e_start_agendas_processing();
e_init_motors();
//e_init_uart1(); // initialize UART to 115200 Kbaud
//e_init_ad_scan();
selector = getselector(); //SELECTOR0 + 2*SELECTOR1 + 4*SELECTOR2 + 8*SELECTOR3;
if(selector==10) {
use_bt=0;
} else {
use_bt=1;
}
#ifdef FLOOR_SENSORS
if(use_bt) { // the I2C must remain disabled when using the gumstix extension
e_i2cp_init();
}
#endif
#ifdef IR_RECEIVER
e_init_remote_control();
#endif
if(RCONbits.POR) { // reset if power on (some problem for few robots)
RCONbits.POR=0;
RESET();
}
/*read HW version from the eeprom (last word)*/
static int HWversion=0xFFFF;
ReadEE(0x7F,0xFFFE,&HWversion, 1);
/*Cam default parameter*/
cam_mode=RGB_565_MODE;
cam_width=40; // DEFAULT_WIDTH;
cam_heigth=40; // DEFAULT_HEIGHT;
cam_zoom=8;
cam_size=cam_width*cam_heigth*2;
if(use_bt) {
e_poxxxx_init_cam();
//e_po6030k_set_sketch_mode(E_PO6030K_SKETCH_COLOR);
e_poxxxx_config_cam((ARRAY_WIDTH -cam_width*cam_zoom)/2,(ARRAY_HEIGHT-cam_heigth*cam_zoom)/2,cam_width*cam_zoom,cam_heigth*cam_zoom,cam_zoom,cam_zoom,cam_mode);
e_poxxxx_set_mirror(1,1);
e_poxxxx_write_cam_registers();
}
e_acc_calibr();
if(use_bt) {
uart1_send_static_text("\f\a"
"WELCOME to the SerCom protocol on e-Puck\r\n"
"the EPFL education robot type \"H\" for help\r\n");
} else {
uart2_send_static_text("\f\a"
"WELCOME to the SerCom protocol on e-Puck\r\n"
"the EPFL education robot type \"H\" for help\r\n");
}
while(1) {
if(use_bt) {
while (e_getchar_uart1(&c)==0)
#ifdef IR_RECEIVER
{
ir_move = e_get_data();
ir_address = e_get_address();
if (((ir_address == 0)||(ir_address == 8))&&(ir_move!=ir_last_move)){
switch(ir_move) {
case 1:
speedr = SPEED_IR;
speedl = SPEED_IR/2;
break;
case 2:
speedr = SPEED_IR;
speedl = SPEED_IR;
break;
case 3:
speedr = SPEED_IR/2;
speedl = SPEED_IR;
break;
case 4:
speedr = SPEED_IR;
speedl = -SPEED_IR;
break;
case 5:
speedr = 0;
speedl = 0;
break;
case 6:
speedr = -SPEED_IR;
speedl = SPEED_IR;
break;
case 7:
speedr = -SPEED_IR;
speedl = -SPEED_IR/2;
break;
case 8:
speedr = -SPEED_IR;
speedl = -SPEED_IR;
break;
case 9:
speedr = -SPEED_IR/2;
speedl = -SPEED_IR;
break;
case 0:
if(first==0){
e_init_sound();
first=1;
}
e_play_sound(11028,8016);
break;
default:
speedr = speedl = 0;
}
ir_last_move = ir_move;
e_set_speed_left(speedl);
e_set_speed_right(speedr);
}
}
#else
;
#endif
} else {
while (e_getchar_uart2(&c)==0)
#ifdef IR_RECEIVER
{
ir_move = e_get_data();
ir_address = e_get_address();
if (((ir_address == 0)||(ir_address == 8))&&(ir_move!=ir_last_move)){
switch(ir_move) {
case 1:
speedr = SPEED_IR;
speedl = SPEED_IR/2;
break;
case 2:
speedr = SPEED_IR;
speedl = SPEED_IR;
break;
case 3:
speedr = SPEED_IR/2;
speedl = SPEED_IR;
break;
case 4:
speedr = SPEED_IR;
speedl = -SPEED_IR;
break;
case 5:
speedr = 0;
speedl = 0;
break;
case 6:
speedr = -SPEED_IR;
speedl = SPEED_IR;
break;
case 7:
speedr = -SPEED_IR;
speedl = -SPEED_IR/2;
break;
case 8:
speedr = -SPEED_IR;
speedl = -SPEED_IR;
break;
case 9:
speedr = -SPEED_IR/2;
speedl = -SPEED_IR;
break;
case 0:
if(first==0){
e_init_sound();
first=1;
}
e_play_sound(11028,8016);
break;
default:
speedr = speedl = 0;
}
ir_last_move = ir_move;
e_set_speed_left(speedl);
e_set_speed_right(speedr);
}
}
#else
;
#endif
}
if (c<0) { // binary mode (big endian)
i=0;
do {
switch(-c) {
case 'a': // Read acceleration sensors in a non
// filtered way, some as ASCII
accx = e_get_acc_filtered(0, 1);
accy = e_get_acc_filtered(1, 1);
accz = e_get_acc_filtered(2, 1);
//accx = e_get_acc(0); //too much noisy
//accy = e_get_acc(1);
//accz = e_get_acc(2);
buffer[i++] = accx & 0xff;
buffer[i++] = accx >> 8;
buffer[i++] = accy & 0xff;
buffer[i++] = accy >> 8;
buffer[i++] = accz & 0xff;
buffer[i++] = accz >> 8;
/*
accelero_raw=e_read_acc_xyz();
ptr=(char *)&accelero_raw.acc_x;
buffer[i++]=(*ptr);
ptr++;
buffer[i++]=(*ptr);
ptr++;
ptr=(char *)&accelero_raw.acc_y;
buffer[i++]=(*ptr);
ptr++;
buffer[i++]=(*ptr);
ptr++;
ptr=(char *)&accelero_raw.acc_z;
buffer[i++]=(*ptr);
ptr++;
buffer[i++]=(*ptr);
ptr++;
*/
break;
case 'A': // read acceleration sensors
accelero=e_read_acc_spheric();
ptr=(char *)&accelero.acceleration;
buffer[i++]=(*ptr);
ptr++;
buffer[i++]=(*ptr);
ptr++;
buffer[i++]=(*ptr);
ptr++;
buffer[i++]=(*ptr);
ptr=(char *)&accelero.orientation;
buffer[i++]=(*ptr);
ptr++;
buffer[i++]=(*ptr);
ptr++;
buffer[i++]=(*ptr);
ptr++;
buffer[i++]=(*ptr);
ptr=(char *)&accelero.inclination;
buffer[i++]=(*ptr);
ptr++;
buffer[i++]=(*ptr);
ptr++;
buffer[i++]=(*ptr);
ptr++;
buffer[i++]=(*ptr);
break;
case 'b': // battery ok?
buffer[i++] = BATT_LOW;
break;
case 'D': // set motor speed
if(use_bt) {
while (e_getchar_uart1(&c1)==0);
while (e_getchar_uart1(&c2)==0);
} else {
while (e_getchar_uart2(&c1)==0);
while (e_getchar_uart2(&c2)==0);
}
speedl=(unsigned char)c1+((unsigned int)c2<<8);
if(use_bt) {
while (e_getchar_uart1(&c1)==0);
while (e_getchar_uart1(&c2)==0);
} else {
while (e_getchar_uart2(&c1)==0);
while (e_getchar_uart2(&c2)==0);
}
speedr=(unsigned char)c1+((unsigned int)c2<<8);
e_set_speed_left(speedl);
e_set_speed_right(speedr);
break;
case 'E': // get motor speed
buffer[i++] = speedl & 0xff;
buffer[i++] = speedl >> 8;
buffer[i++] = speedr & 0xff;
buffer[i++] = speedr >> 8;
break;
case 'I': // get camera image
if(use_bt) {
e_poxxxx_launch_capture(&buffer[i+3]);
wait_cam=1;
buffer[i++]=(char)cam_mode&0xff;//send image parameter
buffer[i++]=(char)cam_width&0xff;
buffer[i++]=(char)cam_heigth&0xff;
i+=cam_size;
}
break;
case 'L': // set LED
if(use_bt) {
while (e_getchar_uart1(&c1)==0);
while (e_getchar_uart1(&c2)==0);
} else {
while (e_getchar_uart2(&c1)==0);
while (e_getchar_uart2(&c2)==0);
}
switch(c1) {
case 8:
if(use_bt) {
e_set_body_led(c2);
}
break;
case 9:
if(use_bt) {
e_set_front_led(c2);
}
break;
default:
e_set_led(c1,c2);
break;
}
break;
case 'M': // optional floor sensors
#ifdef FLOOR_SENSORS
if(use_bt) {
e_i2cp_init();
e_i2cp_enable();
e_i2cp_read(0xC0, 0);
for(j = 0; j < 6; j++) {
if (j % 2 == 0) buffer[i++] = e_i2cp_read(0xC0, j + 1);
else buffer[i++] = e_i2cp_read(0xC0, j - 1);
}
#ifdef CLIFF_SENSORS
for(j=13; j<17; j++) {
if (j % 2 == 0) buffer[i++] = e_i2cp_read(0xC0, j - 1);
else buffer[i++] = e_i2cp_read(0xC0, j + 1);
}
#endif
e_i2cp_disable();
}
#else
for(j=0;j<6;j++) buffer[i++]=0;
#endif
break;
case 'N': // read proximity sensors
if(use_bt) {
for(j=0;j<8;j++) {
n=e_get_calibrated_prox(j); // or ? n=e_get_prox(j);
buffer[i++]=n&0xff;
buffer[i++]=n>>8;
}
} else {
for(j=0;j<10;j++) {
n=e_get_calibrated_prox(j); // or ? n=e_get_prox(j);
buffer[i++]=n&0xff;
buffer[i++]=n>>8;
}
}
break;
case 'O': // read light sensors
if(use_bt) {
for(j=0;j<8;j++) {
n=e_get_ambient_light(j);
buffer[i++]=n&0xff;
buffer[i++]=n>>8;
}
} else {
for(j=0;j<10;j++) {
n=e_get_ambient_light(j);
buffer[i++]=n&0xff;
buffer[i++]=n>>8;
}
}
break;
case 'Q': // read encoders
n=e_get_steps_left();
buffer[i++]=n&0xff;
buffer[i++]=n>>8;
n=e_get_steps_right();
buffer[i++]=n&0xff;
buffer[i++]=n>>8;
break;
case 'u': // get last micro volumes
n = e_get_micro_volume(0);
buffer[i++] = n & 0xff;
buffer[i++] = n >> 8;
n = e_get_micro_volume(1);
buffer[i++] = n & 0xff;
buffer[i++] = n >> 8;
n = e_get_micro_volume(2);
buffer[i++] = n & 0xff;
buffer[i++] = n >> 8;
break;
case 'U': // get micro buffer
ptr=(char *)e_mic_scan;
if(use_bt) {
e_send_uart1_char(ptr,600);//send sound buffer
} else {
e_send_uart2_char(ptr,600);//send sound buffer
}
n=e_last_mic_scan_id;//send last scan
buffer[i++]=n&0xff;
break;
default: // silently ignored
break;
}
if(use_bt) {
while (e_getchar_uart1(&c)==0); // get next command
} else {
while (e_getchar_uart2(&c)==0); // get next command
}
} while(c);
