void readValues() { if(!lockReading) { values[0]+= e_get_calibrated_prox(S_FRONT_RIGHT); values[1]+= e_get_calibrated_prox(S_RIGHT); values[2]+= e_get_calibrated_prox(S_LEFT); values[3]+= e_get_calibrated_prox(S_FRONT_LEFT); readTimes++; } }
/*! \brief Calcul the speed to set on each wheel for avoiding * * Here we do a level-headed sum to take advantage of each captor * depending of there position. For exemple if the captor number 0 * detect something, he has to set the right speed high and set * the left speed low. */ void shock_neuron(void) { for (m = 0; m < 2; m++) { potential[m] = 0; for (s = 0; s < 8; s++) potential[m] += (matrix_prox[m][s]*e_get_calibrated_prox(s)); // get values from proximity sensors speed[m] = (potential[m]/PROXSCALING_SHOCK + BASICSPEED); } if((speed[1] < 50 && speed[1] > -50) && (speed[0] < 50 && speed[0] > -50)) { speed[1] = speed[1] * 20; speed[0] = speed[0] * 20; } if (speed[1] > 1000) speed[1] = 1000; else if (speed[1] < -1000 ) speed[1] = -1000; if (speed[0] > 1000) speed[0] = 1000; else if (speed[0] < -1000 ) speed[0] = -1000; e_set_speed_left(speed[1]); e_set_speed_right(speed[0]); }
int approachButton() { e_set_led(5,0); e_set_led(3,0); e_set_speed_left(300); e_set_speed_right(300); while(e_get_calibrated_prox(S_FRONT_LEFT) < 3000 && e_get_calibrated_prox(S_FRONT_RIGHT) < 3000) { if(e_get_calibrated_prox(S_RIGHT) > 200 && e_get_calibrated_prox(S_FRONT_RIGHT) < 200){ correctRobot(); return 0; } if(e_get_calibrated_prox(S_LEFT) > 200 && e_get_calibrated_prox(S_FRONT_LEFT) < 200){ correctRobot(); return 0; } } unsigned int i; unsigned int m; for(i = 0 ; i < 10000 ; i++) m = sqrt(i*100); e_set_speed_left(-200); e_set_speed_right(-200); for(i = 0 ; i < 10000 ; i++) m = sqrt(i*100); e_set_speed_left(-200); e_set_speed_right(200); while(e_get_calibrated_prox(S_FRONT_RIGHT) > 0); for(i = 0 ; i < 4000 ; i++) m = sqrt(i*100); e_set_speed_left(0); e_set_speed_right(0); keepFinding = 0; }
void correctRobot() { if(e_get_calibrated_prox(S_LEFT) > 10) { correctRight = 0; e_set_speed_left(100); e_set_speed_right(-100); while(e_get_calibrated_prox(S_FRONT_LEFT) > 20); waitABit(); e_set_speed_left(700); e_set_speed_right(700); waitABit(); e_set_speed_left(0); e_set_speed_right(0); }else if(e_get_calibrated_prox(S_RIGHT) > 10) { correctRight = 1; e_set_speed_left(-100); e_set_speed_right(100); while(e_get_calibrated_prox(S_FRONT_RIGHT) > 20); waitABit(); e_set_speed_left(700); e_set_speed_right(700); waitABit(); e_set_speed_left(0); e_set_speed_right(0); }else if(e_get_calibrated_prox(S_FRONT_LEFT) > 100 || e_get_calibrated_prox(S_FRONT_RIGHT) > 100) { e_set_speed_left(-300); e_set_speed_right(-300); waitABit(); e_set_speed_left(0); e_set_speed_right(0); } }
void follow_neuron(void) { int lin_speed = lin_speed_calc((e_get_calibrated_prox(7)+e_get_calibrated_prox(0))/2, 6); int angle_speed = angle_speed_calc((e_get_calibrated_prox(0)+e_get_calibrated_prox(1)) - (e_get_calibrated_prox(7)+e_get_calibrated_prox(6)), 4); e_set_speed_left (lin_speed - angle_speed); e_set_speed_right(lin_speed + angle_speed); }
void obstacleAvoidance() { // check if an obstacle is perceived double reading = 0.0; int obstaclePerceived = 0; int i=0; double x = 0.0, y = 0.0; for (i = 0; i < 8; i++) { reading = e_get_calibrated_prox(i); // if signal above noise if(reading >= obstacleAvoidanceThreshold) { obstaclePerceived = 1; // compute direction to escape double signal = reading - obstacleAvoidanceThreshold; x += -cos(sensorDir[i]) * signal / 8.0; y += sin(sensorDir[i]) * signal / 8.0; } } // no obstacles to avoid, return immediately if (obstaclePerceived == 0) { // go straight forward // change movement direction e_set_speed_left(obstacleAvoidanceSpeed); e_set_speed_right(obstacleAvoidanceSpeed); // return obstaclePerceived; return; } double desiredAngle = atan2 (y, x); double leftSpeed = 0.0; double rightSpeed = 0.0; // turn left if (desiredAngle >= 0.0) { leftSpeed = cos(desiredAngle); rightSpeed = 1; } // turn right else { leftSpeed = 1; rightSpeed = cos(desiredAngle); } // rescale values leftSpeed *= obstacleAvoidanceSpeed; rightSpeed *= obstacleAvoidanceSpeed; // change movement direction e_set_speed_left(leftSpeed); e_set_speed_right(rightSpeed); // advertise obstacle avoidance in progress // return 1; }
/*! \breif Read the sensors proxymities * \param sensorTable Where the value must be stocked */ void followGetSensorValues(int *sensorTable) { unsigned int i; for (i=0; i < NB_SENSORS; i++) { sensorTable[i] = e_get_calibrated_prox(i); //e_get_prox(i) - follow_sensorzero[i]; } }
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);