int main()
{
pc.baud(115200);
NUCLEO.baud(921600);
printf("HEXACOPTER MBED SOFTWARE\n");
init();
wait(1);
NUCLEO.putc('S');
wait(0.01);
while(1)
{
safeGuard.attach(&safeGuardOff, 0.2);
if (counter == 100)
{
counter = 0;
led1 = !led1;
if (Msg.err_state != 5 && !rec_msg)
{
led2 = !led1;
}
else if (Msg.err_state != 5)
{
led2 = 0;
}
rec_msg = false;
switch (Msg.err_state)
{
case 2:
led3 = !led1;
if (led3 == 1) buzz.beep_once();
led2 = 0;
led4 = 0;
break;
case 3:
led2 = !led1;
led3 = !led1;
if (led3 == 1) buzz.beep_twice();
led4 = 0;
break;
case 4:
led4 = !led1;
if (led4 == 1) buzz.beep_thrice();
led2 = 0;
led3 = 0;
break;
case 5:
led4 = !led1;
led3 = !led1;
led2 = !led1;
if (led4 == 1) buzz.long_beep();
if (res_err_cnt >= MAX_WAIT)
{
res_err_cnt = 0;
mbed_reset();
}
res_err_cnt++;
break;
default:
led2 = 0;
led3 = 0;
led4 = 0;
break;
}
}
Msg.batt = bl.GetLevel();
char * msg_array = (char *)&Msg;
for (int i = 0; i < msg_size; i++)
{
NUCLEO.getc();
NUCLEO.putc(msg_array[i]);
}
buffer = (char *) &Rec;
while (!NUCLEO.readable());
for (int i = 0; i < rec_size; i++)
{
buffer[i] = NUCLEO.getc();
}
for (int i = 0; i < 6; i++)
{
motors.pulsewidth(i,Rec.motor_speeds[i]);
}
switch (Rec.buzz_val)
{
case 0:
//do nothing.
break;
case 1:
buzz.beep_once();
break;
case 2:
buzz.beep_twice();
break;
case 3:
buzz.beep_thrice();
break;
case 4:
buzz.long_beep();
break;
default:
//do nothing.
break;
}
counter++;
safeGuard.detach();
}
return 0;
}
void line_following(void)
{
// local variables
const float speed = 0.2;
const float correction = 0.1;
const float threshold = 0.5;
m3pi.locate(0,1);
m3pi.printf("Line Flw"); // print on LCD display
wait(2.0);
m3pi.sensor_auto_calibrate();
t.attach(&isr_timeout, 5.0); // attach isr for timeout
while (1) {
// check for obstacles along the path
if (object_presence() > 0) {
//an obstacle has been detected
m3pi.right_motor(0.2); // turn right
m3pi.left_motor(0.1);
wait(1);
m3pi.left_motor(0.2); // then turn left
m3pi.right_motor(0.1);
wait(1);
m3pi.forward(0.1); // forward
wait(1);
}
else {
/* Line following algorithm */
// -1.0 is far left, 1.0 is far right, 0.0 in the middle
float position_of_line = m3pi.line_position();
// Line is more than the threshold to the right, slow the left motor
if (position_of_line > threshold) {
m3pi.right_motor(speed);
m3pi.left_motor(speed-correction);
}
// Line is more than 50% to the left, slow the right motor
else if (position_of_line < -threshold) {
m3pi.left_motor(speed);
m3pi.right_motor(speed-correction);
}
// Line is in the middle
else {
m3pi.forward(speed);
}
}
// check if timeout has expired
if(flags.flag_timeout_measurement) {
if(flags.flag_timeout_expired) {
t.detach();
m3pi.stop();
goto end_routine; //jump out of the loop
}
else {
//obtain measurement
flags.flag_timeout_expired = 0b1; //set the flag
flags.flag_timeout_measurement = 0b0; //reset flag
m3pi.stop(); //stop the 3pi
obtain_measurement(); // perform measurement
t.detach(); // reset the timer, must be reloaded
t.attach(&isr_timeout, 5.0);
}
}
}
end_routine:
flags.flag_timeout_expired = 0b0;
flags.flag_timeout_measurement = 0b0;
}
