Пример #1
0
/**
 * light the led according to the orientation angle
 * @param  void
 * @return void
 */
void e_display_angle(void)
{
	float angle = 0.0;

// 	To avoid oscillation the limite of variation is limited at 
//  a fix value	wich is 1/9 of the LED resolution
	angle = e_read_orientation();
	if ( abs(angle_mem - angle) > 5.0)
	{
		e_led_clear();
			// table of selection
		if ( 	  (angle > (360.0 - 22.5)) |  (angle <= (0.0   + 22.5)) && angle != ANGLE_ERROR)
			LED0 = 1;
		else if ( angle > (45.0 - 22.5)  && angle <= (45.0  + 22.5))
			LED7 = 1;
		else if ( angle > (90.0 - 22.5)  && angle <= (90.0  + 22.5))
			LED6 = 1;
		else if ( angle > (135.0 - 22.5) && angle <= (135.0 + 22.5))
			LED5 = 1;
		else if ( angle > (180.0 - 22.5) && angle <= (180.0 + 22.5))
			LED4 = 1;
		else if ( angle > (225.0 - 22.5) && angle <= (225.0 + 22.5))
			LED3 = 1;
		else if ( angle > (270.0 - 22.5) && angle <= (270.0 + 22.5))
			LED2 = 1;
		else if ( angle > (315.0 - 22.5) && angle <= (315.0 + 22.5))
			LED1 = 1;
		else				// if angle undefined
			e_led_clear();
		angle_mem = angle;	// value to compare with the next one
	}
}
Пример #2
0
void robot_off(void)
{
	while (1) {
		e_led_clear();
		wait(10000);
	}
}
Пример #3
0
int main() {
 	e_led_clear();
 	int selector;

	//Reset if Power on (some problem for few robots)
	if (RCONbits.POR) {
		RCONbits.POR=0;
		__asm__ volatile ("reset");
	}
Пример #4
0
void run_SensDispl() {
	int cam_mode,cam_width,cam_heigth,cam_zoom,cam_size;
	int i;
	unsigned long cam_light;
	unsigned char *buf_ptr;
	/*Cam default parameter*/
	cam_mode=GREY_SCALE_MODE;
	cam_width=20;
	cam_heigth=20;
	cam_zoom=8;
	cam_size=cam_width*cam_heigth;
	e_poxxxx_init_cam();
	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();
	while (1) {
		e_poxxxx_launch_capture(&buffer[0]);	// start camera capture
		e_led_clear();
		e_set_body_led(0);
		e_set_front_led(0);
		if (e_get_micro_volume(0)>30 || e_get_micro_volume(1)>30 || e_get_micro_volume(2)>30)
			e_set_body_led(1);
		if (e_get_prox(0)>400) 
			e_set_led(0,1);
		if (e_get_prox(1)>400) 
			e_set_led(1,1);
		if (e_get_prox(2)>400) 
			e_set_led(2,1);
		if (e_get_prox(3)>400) 
			e_set_led(3,1);
		if (e_get_prox(4)>400) 
			e_set_led(4,1);
		if (e_get_prox(5)>400) 
			e_set_led(5,1);
		if (e_get_prox(6)>400) 
			e_set_led(6,1);
		if (e_get_prox(7)>400) 
			e_set_led(7,1);
		while(!e_poxxxx_is_img_ready());	// wait end of capture
		cam_light=0;
		buf_ptr=(unsigned char*)&buffer[0];
		for (i=0; i<cam_size; i++) {
			cam_light+=*buf_ptr;
			buf_ptr++;
		}
		sprintf(buffer, "Cam light %lu\r\n", cam_light);
		e_send_uart1_char(buffer, strlen(buffer));
		if (cam_light>48000)	// 20*20pixels*120grayValue
			e_set_front_led(1);
		wait(5000);
	}								
}
Пример #5
0
// *** behaviour insired by a dust cleaner
void run_DustCleaner() {	
	int i;
	int sensor;
	int leftwheel, rightwheel;
	int spiral=100;
	int weightleft[8] = {-10,-10,-5,0,0,5,10,10};
	int weightright[8] = {10,10,5,0,0,-5,-10,-10};

	while (1) {
		e_led_clear();
		e_set_body_led(0);
		e_set_front_led(0);
		if (e_get_prox(0)>600) 
			e_set_led(0,1);
		if (e_get_prox(1)>600) 
			e_set_led(1,1);
		if (e_get_prox(2)>600) 
			e_set_led(2,1);
		if (e_get_prox(3)>600) 
			e_set_led(3,1);
		if (e_get_prox(4)>600) 
			e_set_led(4,1);
		if (e_get_prox(5)>600) 
			e_set_led(5,1);
		if (e_get_prox(6)>600) 
			e_set_led(6,1);
		if (e_get_prox(7)>600) 
			e_set_led(7,1);

		if  (e_get_prox(0)>600 || e_get_prox(1)>600 || e_get_prox(2)>600 || 
 			e_get_prox(5)>600 || e_get_prox(6)>600 || e_get_prox(7)>600 ){  // obstacle
			e_set_body_led(1);
			// *** avoid
			leftwheel=200;
			rightwheel=200;
			for (i=0; i<8; i++) {
				sensor=e_get_prox(i); //-sensorzero[i];
				sprintf(buffer, "%d, ", sensor);
				e_send_uart1_char(buffer, strlen(buffer));
				leftwheel+=weightleft[i]*(sensor>>4);
				rightwheel+=weightright[i]*(sensor>>4);
			}
			sprintf(buffer, "setspeed %d %d\r\n", leftwheel, rightwheel);
			e_send_uart1_char(buffer, strlen(buffer));
			if (leftwheel>800) {leftwheel=800;}
			if (rightwheel>800) {rightwheel=800;}
			if (leftwheel<-800) {leftwheel=-800;}
			if (rightwheel<-800) {rightwheel=-800;}
			e_set_speed_left(leftwheel);
			e_set_speed_right(rightwheel);

		} else { // spiral
Пример #6
0
void robot_off(void)
{
	e_init_port();
	e_init_motors();
	e_init_ad_scan(ALL_ADC);
	
	e_calibrate_ir();

	e_start_agendas_processing();
	while (1) {
		e_led_clear();
		wait(10000);
	}
}
Пример #7
0
/*! \brief The left LED are indicating the left side */
void left_led(void)
{
	static unsigned char no_led = 0;
	switch(no_led)
	{
		case 0: e_set_led(0, 2); e_set_led(4, 2); break;
		case 1: e_set_led(7, 2); e_set_led(5, 2); break;
		case 2: e_set_led(6, 2); break;	
		case 3: e_led_clear();
	}
	if(no_led < 3)
		no_led++;
	else
		no_led = 0;
}
Пример #8
0
/*! \brief The right LED are indicating the right side */
void right_led(void)
{
	static unsigned char no_led = 0;
	switch(no_led)
	{
		case 0: e_set_led(0, 2); e_set_led(4, 2); break;
		case 1: e_set_led(1, 2); e_set_led(3, 2); break;
		case 2: e_set_led(2, 2); break;	
		case 3: e_led_clear();
	}
	if(no_led < 3)
		no_led++;
	else
		no_led = 0;
}
Пример #9
0
int main() {

    e_init_port();
    e_init_uart1();
    e_led_clear();

    e_activate_agenda(e_blink_led0, 2500);
    e_activate_agenda(radio, 100);
    e_start_agendas_processing();

    while(1){
        for ( delay = 0 ; delay < 15000; delay++){
            __asm__("nop");
        }
    }

    return 0;
}
Пример #10
0
int main()
{
    // init robot
    e_init_port();
    e_init_ad_scan();
    e_init_uart1();
    e_led_clear();	
    e_init_motors();
    e_start_agendas_processing();

    // wait for s to start
    btcomWaitForCommand('s');
    btcomSendString("==== READY - IR TESTING ====\n\n");

    e_calibrate_ir(); 

    // initialize ircom and start reading
    ircomStart();
    ircomEnableContinuousListening();
    ircomListen();

    // rely on selector to define the role
    int selector = getselector();

    // show selector choosen
    int i;
    long int j;
    for (i = 0; i < selector; i++)
    {
	e_led_clear();
	
	for(j = 0; j < 200000; j++)
	    asm("nop");
	
	e_set_led(i%8, 1);
	
	for(j = 0; j < 300000; j++)
	    asm("nop");

	e_led_clear();

	for(j = 0; j < 300000; j++)
	    asm("nop");
    }

    // activate obstacle avoidance
    e_activate_agenda(obstacleAvoidance, 10000);

    // acting as sender
    if (selector == 1)
    {    	
	btcomSendString("==== EMITTER ====\n\n");

	int i;
	for (i = 0; i < 10000; i++)
	{
	    // takes ~15knops for a 32window, avoid putting messages too close...
	    for(j = 0; j < 200000; j++)	asm("nop");

	    ircomSend(i % 256);	    
	    while (ircomSendDone() == 0);

	    btcomSendString(".");
	}
    }
    
    // acting as receiver
    else if (selector == 2)
    {
	btcomSendString("==== RECEIVER ====\n\n");

	int i = 0;
	while (i < 200)
	{
	    // ircomListen();
	    IrcomMessage imsg;
	    ircomPopMessage(&imsg);
	    if (imsg.error == 0)
	    {
		int val = (int) imsg.value;
	    
		/* Send Value*/		
		char tmp[128];
		sprintf(tmp, "Receive successful : %d  - distance=%f \t direction=%f \n", val, (double)imsg.distance, (double)imsg.direction);
		btcomSendString(tmp);
	    }
	    else if (imsg.error > 0)
	    {
		btcomSendString("Receive failed \n");		
	    }
	    // else imsg.error == -1 -> no message available in the queue

	    if (imsg.error != -1) i++;
	}
    }
    // no proper role defined...
    else 
    {
	int i = 0;
	long int j;
	while(1)
	{
	    e_led_clear();
	    
	    for(j = 0; j < 200000; j++)
		asm("nop");
	    
	    e_set_led(i, 1);
	    
	    for(j = 0; j < 300000; j++)
		asm("nop");
	    
	    i++;
	    i = i%8;
	}	
    }    
    
    ircomStop();
    return 0;
}
Пример #11
0
/*! \brief The "main" function of the program */
void run_wallfollow() {
	int leftwheel, rightwheel;		// motor speed left and right
	int distances[NB_SENSORS];		// array keeping the distance sensor readings
	int i;							// FOR-loop counters
	int gostraight;
	int loopcount;
	unsigned char selector_change;
	 
	e_init_port();
	e_init_ad_scan(ALL_ADC);
	e_init_motors();
	e_start_agendas_processing();

	//follow_sensor_calibrate();	
	
	e_activate_agenda(left_led, 2500);
	e_activate_agenda(right_led, 2500);
	e_pause_agenda(left_led);
	e_pause_agenda(right_led);
	
	e_calibrate_ir();
	loopcount=0;
	int selectionValue;
//	selector_change = !(followgetSelectorValue() & 0x0001);
	e_set_steps_right(0);
	selectionValue = followgetSelectorValue();
	while (doesWallExist(selectionValue)) {
		followGetSensorValues(distances); // read sensor values

		gostraight=0;
		if (selectionValue == 1) {
		////	if(selector_change == LEFT_FOLLOW) {
		//		selector_change = RIGHT_FOLLOW;
				e_led_clear();
				e_pause_agenda(left_led);
				e_restart_agenda(right_led);
	//		}  
			for (i=0; i<8; i++) {
				if (distances[i]>50) {break;}
			}
			if (i==8) {
				gostraight=1;
			} else {
				follow_weightleft[0]=-10;
				follow_weightleft[7]=-10;
				follow_weightright[0]=10;
				follow_weightright[7]=10;
				if (distances[2]>300) {
					distances[1]-=200;
					distances[2]-=600;
					distances[3]-=100;
				}
			}
		} else {
		//	if(selector_change == RIGHT_FOLLOW) {
			//	selector_change = LEFT_FOLLOW;
				e_led_clear();
				e_pause_agenda(right_led);
				e_restart_agenda(left_led);
		//	}
			for (i=0; i<8; i++) {
				if (distances[i]>50) {break;}
			}
			if (i==8) {
				gostraight=1;
			} else {
				follow_weightleft[0]=10;
				follow_weightleft[7]=10;
				follow_weightright[0]=-10;
				follow_weightright[7]=-10;
				if (distances[5]>300) {
					distances[4]-=100;
					distances[5]-=600;
					distances[6]-=200;
				}
			}
		}

		leftwheel=BIAS_SPEED;
		rightwheel=BIAS_SPEED;
		if (gostraight==0) {
			for (i=0; i<8; i++) {
				leftwheel+=follow_weightleft[i]*(distances[i]>>4);
				rightwheel+=follow_weightright[i]*(distances[i]>>4);
			}
		}

		// set robot speed
		followsetSpeed(leftwheel, rightwheel);

		wait(15000);
		
		//doesWallExist(selectionValue);
	}	
}
Пример #12
0
void run_CameraTurn() {
	int cam_mode,cam_width,cam_heigth,cam_zoom,cam_size;
	int i;
	unsigned char *buf_ptr, pixel, lightest;
	unsigned int left, right, lightPos;

	#include "DataEEPROM.h"
	/*read HW version from the eeprom (last word)*/
	int HWversion=0xFFFF;
	int temp = 0;
	temp = ReadEE(0x7F,0xFFFE,&HWversion, 1);
	temp = temp & 0x03;	// get the camera rotation from the HWversion byte

	/*Cam default parameter*/
	cam_mode=GREY_SCALE_MODE;
	if ((temp==3)||(temp==0)) { // 0' and 180' camera rotation
		cam_width=1;
		cam_heigth=60;
	} else {
		cam_width=60;
		cam_heigth=1;
	}
	cam_zoom=8;
	cam_size=cam_width*cam_heigth;
	e_poxxxx_init_cam();
	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();

	while (1) {
		e_poxxxx_launch_capture(&buffer[0]);	// start camera capture
		e_led_clear();
		e_set_body_led(0);
		e_set_front_led(0);

		while(!e_poxxxx_is_img_ready());	// wait end of capture
		buf_ptr=(unsigned char*)&buffer[0];
		left=0; right=0; lightPos=0; lightest=0;
		for (i=0; i<30; i++) {	//left
			pixel=*buf_ptr;
			buf_ptr++;
			left+=pixel;
			if (pixel>lightest) {
				lightest=pixel;
				lightPos=i;
			}
		}
		for (; i<cam_heigth; i++) {	//right
			pixel=*buf_ptr;
			buf_ptr++;
			right+=pixel;
			if (pixel>lightest) {
				lightest=pixel;
				lightPos=i;
			}
		}
		if (lightPos<20) {	//led on at lightest position
			e_set_led(7,1); }
		else if (lightPos<40) {
			e_set_led(0,1); }
		else {
			e_set_led(1,1); }

		if ((temp==3)||(temp==2)) { // 0' and 90' camera rotation
			e_set_speed_left(10*(lightPos-30));  // motor speed in steps/s
			e_set_speed_right(-10*(lightPos-30));
		} else {
			e_set_speed_left(-10*(lightPos-30));  // motor speed in steps/s
			e_set_speed_right(10*(lightPos-30));
		}

		sprintf(buffer, "left %u, right %u, lightest %u, lightPos %u\r\n", left, right, lightest, lightPos);
		e_send_uart1_char(buffer, strlen(buffer));
		wait(5000);
	}								
}
Пример #13
0
int main()
{
    // init robot
    e_init_port();
    e_init_ad_scan();
    e_init_uart1();
    e_led_clear();
    e_init_motors();
    e_start_agendas_processing();

    // initialise buffer
    int k;
    for (k = 0; k < NB_NEIGHBOURS; k++)
        neighbours[k].id = -1;


    // wait for s to start
    /*     btcomWaitForCommand('s'); */

    btcomSendString("-OK-\n");

    e_calibrate_ir();

    // initialize ircom, then rng and start listening
    ircomStart();
    //ircomEnableContinuousListening();
    //ircomEnableProximity();

    initRandomNumberGenerator();
    // after rng init we can disable prox sensors
    //ircomDisableProximity();
    ircomEnableContinuousListening();
    ircomListen();


    id = getselector();

    ircomResetTime();
    lastClock = ircomGetTime();

    // activate movement
    //e_activate_agenda(move, 2500);

    organiseInit();

    // advertise current direction
    e_led_clear();
    e_set_led(0,1);
    e_set_led(4,1);

    while(1)
    {
        int messageReceived = 0;
        while(((ircomGetTime() - lastClock < COM_CYCLE_SPEED) || (ircomIsReceiving() == 1)))
        {
            IrcomMessage msg;
            ircomPopMessage(&msg);

            if (msg.error == 0)
            {
                processNewMessage(&msg);
                messageReceived = 1;
                SetRobotSeen(msg);
            }
        }
        e_set_body_led(messageReceived);


        SendData();
        //sendId();
        //sendAngleToNeighbours();
        lastClock = ircomGetTime();

        e_set_body_led(0);
    }

    ircomStop();
    return 0;
}
Пример #14
0
void aggressive(void)
{
	long i;
	int left, right;

//	e_init_sound();
	

	e_start_agendas_processing();

	e_set_speed_left(500);
	e_set_speed_right(500);


	while(1) {
		e_set_led(1,1);	

		// If robot is stuck, use front 4 sensors to confirm in range, do a degree spin to try get unstuck
		//if front left sensors, turn right
		if (e_get_prox(0) > 800 || e_get_prox(1) > 800) {
			if (e_get_prox(0) > 800) {
				e_set_led(0,1);
			}
			if (e_get_prox(1) > 800) {
				e_set_led(1,1);
			}
				
			// Do a 90 degree spin, 
			left=-450;right=450;
			e_set_speed_right(right);
			e_set_speed_left(left);
			for(i=0;i<40000;i++) {asm("nop");}

			// set back to normal speed
			e_set_speed_left(500);
			e_set_speed_right(500);
			
			//clear all LED lights
			e_led_clear();
		}

		//if front right sensors, turn left
		if (e_get_prox(6) > 800 || e_get_prox(7) > 800) {
			if (e_get_prox(6) > 800) {
				e_set_led(7,1);
			}
			if (e_get_prox(7) > 800) {
				e_set_led(7,1);
			}
				
			left=450;right=-450;
			e_set_speed_right(right);
			e_set_speed_left(left);
			for(i=0;i<40000;i++) {asm("nop");}

			// set back to normal speed
			e_set_speed_left(500);
			e_set_speed_right(500);
			
			//clear all LED lights
			e_led_clear();
		}

		//if back sensors, turn to face
		if (e_get_prox(3) > 800 || e_get_prox(5) > 800) {
			
			/*// Run away fast with fear flash
			e_set_led(0,1);
			e_set_led(1,1);
			e_set_led(2,1);
			e_set_led(3,1);
			e_set_led(4,1);
			e_set_led(5,1);
			e_set_led(6,1);
			e_set_led(7,1);
			e_play_sound(11028, 8016);
           	*/

			e_set_speed_left(1500);
			e_set_speed_right(-1500);
			for(i=0;i<10000;i++) {asm("nop");}

			// Set back to normal speed
			//e_led_clear();
			e_set_speed_left(500);
			e_set_speed_right(500);
		}


		//camera code
	}
}