void e_init_randb ( unsigned char mode ) { if ( mode == I2C ) { /* Init I2C */ e_i2cp_init(); e_i2cp_enable(); } else { /* Init UART2 */ e_init_uart2(); /* Clean UART buffer */ char msg; while(e_getchar_uart2(&msg)); /* Start Agendas */ e_start_agendas_processing(); /* Locate e-randb task */ e_activate_agenda(e_randb_get_uart2, 2); /* Tell the board we work on UART mode */ e_randb_set_uart_communication(UART); } /* Calculations are made on the BOARD*/ calcOnBoard = TRUE; /* Init Global variables */ erandbFinished = FALSE; erandbState = WAITING; erandbCounter = 0; }
int main() { /*system initialization */ e_init_port(); /* Init UART1 for bluetooth */ e_init_uart1(); /* Init UART2 for e-randb */ e_init_uart2(); /* Init IR */ e_init_prox(); //init motors e_init_motors(); /* Wait for a command comming from bluetooth IRCOMTEST on pc directory*/ btcomWaitForCommand('s'); /* Start agendas processing which will take care of UART interruptions */ e_start_agendas_processing(); /* Init E-RANDB board */ e_init_randb(); /* Range is tunable by software. * 0 -> Full Range (1m. approx depending on light conditions ) * 255 --> No Range (0cm. approx, depending on light conditions */ e_randb_uart_set_range(0); /* At some point we tought that the board could just take * data en leave the calculations for the robot. * At the moment, it is better to allow the board to do the calculations */ e_randb_uart_set_calculation(ON_BOARD); /* Store light conditions to use them as offset for the calculation * of the range and bearing */ e_randb_uart_store_light_conditions(); e_randb_set_uart_communication(1); finalDataRegister data; //tabla comunciacion double comunicacionAngulos[2]; int comunicacionRangos[2]; //subsuncion int CURRENT_STATE; int subsuncion[2][2]; int debug_var = 0; subsuncion[0][0]=SPACING; subsuncion[1][0]=COHESION; int i; for (i=0;i<2;i++){ subsuncion[i][1]=0; } //proximity sensors reading int prox_first_reading[8]; int prox_reading[8]; /* Angles in rad for IRs starting at 0. Left direction. */ const double prox_directions[8] = {5.9865, 5.4105, 4.7124, 3.6652, 2.6180, 1.5708, 0.8727, 0.2967}; /* Get the first reading to take ambient light */ for(i=0; i < 8; i++){ prox_first_reading[i]=e_get_prox(i); } /* Print on the bluetooth */ char tmp2[50]; sprintf(tmp2,"-- CHASER --\n"); btcomSendString(tmp2); while(1) { //comprobacion proximidad int maxProx = 0; /* Get readings and substract the first reading */ for(i=0; i < 8; i++){ prox_reading[i] = e_get_prox(i) - prox_first_reading[i]; if(prox_reading[i] < 0) {prox_reading[i] = 0; } if ( prox_reading[i]>maxProx){ maxProx = prox_reading[i]; } } if(maxProx > PROX_THRES){ subsuncion[0][1]=1; // Collission } else{ // Chasing subsuncion[0][1]=0; } CURRENT_STATE = 1; //by default. chasing for(i=0;i<2;i++){ if(subsuncion[i][1]==1){ CURRENT_STATE = i; break; } } char tmp[30]; double vector_repelent[2] = {0.0,0.0}; double ang_repelent; double ang_comunicacion = 0; if (e_randb_get_data_uart2(&data)){ //actualizar tabla comun if((data.bearing > -2*PI) && (data.bearing < 2*PI)){ switch(data.data) { case 0: comunicacionAngulos[0]=data.bearing; comunicacionRangos[0]=data.range; sprintf(tmp2,"Sigue Lider. Ang: %f, Rango: %f \n", data.bearing, data.range); btcomSendString(tmp2); break; case 1: comunicacionAngulos[1]=data.bearing; comunicacionRangos[1]=data.range; sprintf(tmp2,"Sigue Sucker. Ang: %f, Rango: %f \n", data.bearing, data.range); btcomSendString(tmp2); break; } } } switch(CURRENT_STATE){ case 0: //Collission sprintf(tmp2,"-- COLISION max= %d --\n",maxProx); btcomSendString(tmp2); /* Calc vector Sum */ vector_repelent[0] = 0.0; vector_repelent[1] = 0.0; for (i = 0 ; i < 8; i ++ ) { vector_repelent[0] += prox_reading[i] * cos ( prox_directions[i] ); vector_repelent[1] += prox_reading[i] * sin ( prox_directions[i] ); } /* Calc pointing angle */ ang_repelent = atan2(vector_repelent[1], vector_repelent[0]); /* Create repelent angle */ ang_repelent -= PI; //calculate and set velocity setAngularVelocity(ang_repelent,1); break; // Case 0 case 1: // Chasing sprintf(tmp2,"-- PERSIGUIENDO--\n"); btcomSendString(tmp2); ang_comunicacion = (comunicacionAngulos[0] + comunicacionAngulos[1])/2; if(ang_comunicacion>0.6 || ang_comunicacion<-0.6){ // Chasing sprintf(tmp2,"-- GIRANDO ang= %02f --\n",ang_comunicacion); btcomSendString(tmp2); //calculate and set velocity setAngularVelocity(ang_comunicacion,2); } else { // Walk sprintf(tmp2,"-- RECTO--\n"); btcomSendString(tmp2); e_set_speed_left(SPEED); e_set_speed_right(SPEED); } break; // Case 1 } // End switch //if (e_randb_get_data_uart2(&data)){ // sprintf(tmp,"%d %02f %d %2f %d\n",debug_var, data.bearing, data.range); // btcomSendString(tmp); /* Send the data through one sensor */ //e_randb_uart_send_all_data(data); } return 0; }