Fabio Faria / Mbed OS ISR_Mini-explorer_Rangefinder

Preliminary sample code for using the rangefinder.

Dependencies:   ISR_Mini-explorer_tmp2

main.cpp

Committer:
fabiofaria
Date:
2018-05-16
Revision:
4:0312c52bd9ed
Parent:
2:d4d7d6d4a827

File content as of revision 4:0312c52bd9ed:

#include "mbed.h"
#include "robot.h" // Inicializa o robô. Este include deverá ser usado em todos os main.cpp!
#include "Radio.h"
#include "Rangefinder.h"

int main()
{
    initRobot(124);
    X=20;
    Y=20;
    THETA=0;

    while(1) {
        if( read_rangefinder() == 0 ) {
            for(int i = 0; i < 16; i++) {
                pc.printf("%d=%u ", i, rgfMeasurements[i]);
            }
            pc.printf("\n");
        }
        else
        {
            pc.printf("Rangefinder reading failed!\n");
            q_led_red_rea = 0; // Turns ON rear red LED.
        }

        wait(0.5);
    }
}