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; }