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;
e_init_port();
e_init_motors();
//e_init_ad_scan(ALL_ADC);
//e_init_ad_scan();
follow_sensor_calibrate();
loopcount=0;
while (1) {
followGetSensorValues(distances); // read sensor values
gostraight=0;
if ((followgetSelectorValue() & 0x0001) == RIGHT_FOLLOW) {
e_set_led(6,0);
e_set_led(2,1);
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 {
e_set_led(6,1);
e_set_led(2,0);
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);
}
}
/*! \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);
}
}