Пример #1
0
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++;
    }
}
Пример #2
0
/*! \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]);
}
Пример #3
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;
}
Пример #4
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);
    }
}
Пример #5
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);
}
Пример #6
0
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;
}
Пример #7
0
/*! \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];
	}		
}
Пример #8
0
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);