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 set_LEDs()
{
char ring, body_front;
ring = recv_char();
body_front = recv_char();
/* Each one of eight bits of "char ring" represents one of eight ring *
* LEDs in e-puck, the rightmost bit meaning the LED 0. With a bit being *
* true the LED will be on, with a bit being false the LED will be off. */
int led;
for(led = 0; led< RING_LED_NUMBER; led++)
{
if((ring&(0x01<<led))!=0)
{
e_set_led(led, LED_ON);
}
else
{
e_set_led(led, LED_OFF);
}
}
if((body_front & 0x01) != 0)
{
e_set_front_led(LED_ON);
}
else
{
e_set_front_led(LED_OFF);
}
if((body_front & 0x02) != 0)
{
e_set_body_led(LED_ON);
}
else
{
e_set_body_led(LED_OFF);
}
send_char(1);/* Send a char to signalize the end set LED operation. */
}
void uplink_cmd_handler(void) {
static char cmd[20];
static uint8_t index = 0;
// No data received, return
if(buffer_level(UPLINK_USART,RX) < 1) return;
// flush cmd buffer if cmd is out of a valid length
if(index == MAX_CMD_LENGTH) {
index = 0;
}
// append char to cmd
recv_char(UPLINK_USART, &cmd[index]);
switch(cmd[index]) {
case '\r':
// carriage return received, replace with stringtermination and parse
send_str(UPLINK_USART, "\r\n");
cmd[index] = '\0';
parse_cmd(cmd);
index = 0;
break;
case '\n':
// do nothing, but avoid index from incrementing
break;
case '\b':
// backspace, remove last received char
index--;
send_char(UPLINK_USART, '\b');
break;
// char is part of an ESC sequence
case 0x1B:
case 0x5B:
index++;
break;
// each other if the last two char was not part of an ESC sequence
default:
if(cmd[index - 1] == 0x5B && cmd[index - 2] == 0x1B) {
index = index - 2;
} else {
send_char(UPLINK_USART, cmd[index]);
index++;
}
}
}
//-----------------------------------------------------------------------------
static size_t read_msg (int sock, struct recv_buf *recv_buf, char *msg_buf) {
size_t i, delim_pos;
for (i = 0, delim_pos = 0; i < MSG_MAX-1 && delim_pos < delim.len; i++) {
signed char c = recv_char (sock, recv_buf);
if (c < 0)
return 0;
if (c == delim.str[delim_pos])
delim_pos++;
else
delim_pos = 0;
msg_buf[i] = c;
}
msg_buf[i] = '\0';
return i;
}
