/**
* 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
}
}
void robot_off(void)
{
while (1) {
e_led_clear();
wait(10000);
}
}
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");
}
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);
}
}
// *** 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
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);
}
}
/*! \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;
}
/*! \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;
}
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;
}
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;
}
/*! \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);
}
}
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);
}
}
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;
}
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
}
}
