EL_PROCESS Trigger_Accelerometer_Process(el_handle this_trigger){
static int previous_xyz[3] = {0,0,0};
const long threshold_s = 330L*330L;
long magnitude_s;// _s means squared;
int xyz[3];
int dx,dy,dz;
el_accelerometer_get(EL_ACCELEROMETER_ONE,EL_ACCELERATION_ALL_3V,xyz);
if(previous_xyz[0]!=0){
dx = xyz[0] - previous_xyz[0];
dy = xyz[1] - previous_xyz[1];
dz = xyz[2] - previous_xyz[2];
magnitude_s = (long)dx*dx;
magnitude_s += (long)dy*dy;
magnitude_s += (long)dz*dz;
if(magnitude_s > threshold_s){
el_led_set(EL_LED_BODY,EL_ON);
el_process_wait(200);
el_led_set(EL_LED_BODY,EL_OFF);
el_process_wait(200);
// 400 ms past, need to refresh values
el_accelerometer_get(EL_ACCELEROMETER_ONE,EL_ACCELERATION_ALL_3V,xyz);
}
}
previous_xyz[0] = xyz[0];
previous_xyz[1] = xyz[1];
previous_xyz[2] = xyz[2];
el_trigger_enable(this_trigger);
}
void BootingProcedure01_SelectorBarrier(){
while(el_get_selector_value()<4){
el_led_set(EL_LED_RING_5,1);
el_sleep(125);
el_led_set(EL_LED_RING_5,0);
el_sleep(875);
}
}
void booting_procedure01_selector_barrier(){
// do nothing until selector >= 4
while(el_get_selector_value()<4){
el_led_set(EL_LED_RING_5,1);
el_sleep(125);
el_led_set(EL_LED_RING_5,0);
el_sleep(3000 - 125);
}
}
void BootingProcedure01_SelectorBarrier(){
// do nothing until selector >= 4
while(el_get_selector_value()<4){
el_led_set(EL_LED_RING_5,EL_ON);
el_sleep(100);// note: "el_sleep" does not work in a process
el_led_set(EL_LED_RING_5,EL_OFF);
el_sleep(900);
}
}
EL_PROCESS Process_LED(void*data){
while(1){
el_led_set(EL_LED_RING_2,1);
el_led_set(EL_LED_RING_6,0);
el_process_wait(500);
el_led_set(EL_LED_RING_2,0);
el_led_set(EL_LED_RING_6,1);
el_process_wait(500);
}
}
void Process_LED_PatternA(void*arg){
while(1){
el_led_set(EL_LED_BODY,EL_ON);
el_process_wait(4800);// "el_process_wait" works only in a process
el_led_set(EL_LED_BODY,EL_OFF);
el_process_wait(200);
}
}
EL_PROCESS Process_IRRC(void*data){
int k;
int check,address,command;
while(1){
el_ir_receiver_reset();
k = el_ir_receiver_get_check();
do{
el_process_cooperate();
check = el_ir_receiver_get_check();
}while(check==k);//wait until "check" being changed
address = el_ir_receiver_get_address();
command = el_ir_receiver_get_data();
elu_println("[IRRC]\nCHECK\tADDRESS\tCOMMAND");
elu_println("%d\t%d\t%d",check,address,command);
if(address==5){
el_led_set(4,EL_TOGGLE);
}
}
}
void Process_LED_PatternB(void*arg){
int i = 0;
while(1){
el_led_set(EL_LED_RING_0 + i,EL_ON);
el_process_wait(125);
el_led_set(EL_LED_RING_0 + i,EL_OFF);
i++;
i %= 8;
}
}
void Process_LED_Control(void*arg){
char c;
while(1){
// wait until something is received
do{
/*
* In a process, "el_process_cooperate" need to be called in any
* polling loops. See the documentation of the Process module to
* learn more details.
*/
el_process_cooperate();
}while(el_uart_get_char_counter(EL_UART_1)==0);
c = el_uart_get_char(EL_UART_1);
if(c=='f'){
el_led_set(EL_LED_FRONT,EL_TOGGLE);
}
}
}
int main(){
el_initialization();
/*
* This is to let the robot automaticaly reset when TinyBootloader
* attemps to write a new HEX, so you dont need to touch the reset
* button.
* To achieve this, TinyBootloader also needs to be configured:
* in "Options" tab, set "Codes to send first" to 6.
*/
el_uart_use_reset_code(true,6);
/*
* Put the robot in silence when the selector is in 0~3.
*/
BootingProcedure01_SelectorBarrier();
// see documentation for details about "elu_printf" and the UART module
elu_printf("Hello World! This is e-puck!\n");
// setup some process, which will be concurrenly executed in "el_main_loop()"
el_launch_process(Process_LED_PatternA,NULL);
el_launch_process(Process_LED_PatternB,NULL);
el_launch_process(Process_LED_Control,NULL);
el_led_set(EL_LED_BODY,EL_ON);
elu_printf("setup finished\n");
// this it the loop to run everything, including Timer, Process and Trigger.
el_main_loop();
return 0;
}
EL_PROCESS Trigger_InfraredReceiver(el_handle this_trigger){
el_led_set(0,EL_TOGGLE);
el_trigger_enable(this_trigger);
}
EL_PROCESS Process_UART(void*data){
el_camera_image *frame;
char c;
int i;
int ir0,ir7;
el_ir_proximity_data prox[8];
el_int16 xyz[3];
/*
elu_printf("ENTER YOUR PIN >>");
elu_scanf("%d",&i);
if(i==9527){
elu_printf("PIN OK.\n");
}else{
elu_printf("PIN ERROR.\n");
}
*/
while(1){
WAIT_FOR_UART1_CHAR;
c = el_uart_get_char(EL_UART_1);
switch(c){
case 'f':
el_led_set(EL_LED_FRONT,EL_TOGGLE);
break;
case 'y':
elu_println("[CAM]\nFPS:\t%d\tNUM:\t%lu",CameraFPS,el_camera_get_frame_counter());
break;
case 't':
el_camera_lock_frame();
frame = el_camera_frame();
elu_print_camera_image(frame,el_camera_get_frame_counter());
el_camera_unlock_frame();
elu_putchar('\n');
break;
case 'g':
elu_println("[ACC]");
el_accelerometer_get(EL_ACCELEROMETER_ONE,EL_ACCELERATION_ALL_3V,xyz);
elu_println("%d\t%d\t%d",xyz[0],xyz[1],xyz[2]);
elu_putchar('\n');
break;
case 'r':
elu_println("[IR,%d]",(int)el_ir_proximity_get_counter());
el_ir_proximity_get(EL_IR_PROXIMITY_SENSOR_ALL,EL_IR_ALL_3V,(el_int16*)prox);
elu_printf("AMB:");
for(i=0;i<8;i++){
elu_printf("\t%d",prox[i].Ambient);
}
elu_putchar('\n');
elu_printf("REF:");
for(i=0;i<8;i++){
elu_printf("\t%d",prox[i].Reflection);
}
elu_putchar('\n');
break;
case 'e':
el_ir_proximity_get(EL_IR_PROXIMITY_SENSOR_0,EL_IR_REFLECTION,&ir0);
el_ir_proximity_get(EL_IR_PROXIMITY_SENSOR_7,EL_IR_REFLECTION,&ir7);
elu_println("%d,%d",ir0,ir7);
break;
case 'c':
elu_println("[IRNR]");
for(i=0;i<8;i++){
elu_printf("%d\t",el_irps_samples_NeutralReflection[i]);
}
elu_putchar('\n');
break;
case 'w':
el_set_wheel_speed(1000,1000);
break;
case 'a':
el_set_wheel_speed(-250,250);
break;
case 's':
el_set_wheel_speed(-500,-500);
break;
case 'd':
el_set_wheel_speed(250,-250);
break;
case ' ':
el_set_wheel_speed(0,0);
break;
}
}
}
void Process_ConsoleLoop(void*arg){
char c;
int i;
elu_printf("EL_EXAMPLE_04\n");
// setup the task
Task_ObjectFollowing_Setup();
el_process_wait(500);// wait for 500 ms
el_uart_flush_char(EL_UART_1);
while(1){
do{
el_process_cooperate();
}while(el_uart_get_char_counter(EL_UART_1)==0);
c = el_uart_get_char(EL_UART_1);
switch(c){
case 'f':
el_led_set(EL_LED_FRONT,EL_TOGGLE);
break;
case 'r':// report ir proximity sensor outputs
elu_println("<IR>");
el_ir_proximity_get(EL_IR_PROXIMITY_SENSOR_ALL,EL_IR_ALL_3V,(el_int16*)ProximitySensor);
elu_printf("AMB:");
for(i=0;i<8;i++){
elu_printf("\t%d",ProximitySensor[i].Ambient);
}
elu_putchar('\n');
elu_printf("REF:");
for(i=0;i<8;i++){
elu_printf("\t%d",ProximitySensor[i].Reflection);
}
elu_putchar('\n');
break;
case 'p':// report image processing result
elu_println("<IMG>");
elu_println("MASS:\t%d\t%d\t%d",vision_mass_red,vision_mass_green,vision_mass_blue);
elu_println("BIAS:\t%d\t%d\t%d",vision_bias_red,vision_bias_green,vision_bias_blue);
elu_putchar('\n');
break;
case '1':
Task_ObjectColor = TASK_OBJECT_RED;
elu_println("FOLLOW RED");
break;
case '2':
Task_ObjectColor = TASK_OBJECT_GREEN;
elu_println("FOLLOW GREEN");
break;
case '3':
Task_ObjectColor = TASK_OBJECT_BLUE;
elu_println("FOLLOW BLUE");
break;
}
}
Task_ObjectFollowing_Clear();
}