File content as of revision 11:5ba926692210:

#include "mbed.h"
#include "rtos.h"
#include "math.h"
#include "globals.h"
#include "RFSRF05.h"
#include "IR_Turret.h"
#include "PwmIn.h"
#include "system.h"
#include "geometryfuncs.h"



Serial pc(USBTX, USBRX); // tx, rx

IR_TURRET ir(STEPPER_PIN,IR_SENSOR_PIN);
RFSRF05 sonar(p16,p10,p11,p12,p13,p14,p15,p5,p6,p7,p8,p9);
Serial mbed_serial(MBED_MAIN_SERIAL_TX,MBED_MAIN_SERIAL_RX);


enum measurement_t {SONAR0 = 0, SONAR1, SONAR2, IR0, IR1, IR2};
struct measurmentdata {
    measurement_t ID;
    float value;
    float aux;
} ;

Mail <measurmentdata, 16> measureMQ;

// bytes packing for peer to peer communication
typedef union {
    struct _data{
        unsigned char header[3];
        unsigned char ID;
        float value;
        float aux;
    }  data;  
    unsigned char type_char[sizeof(_data)];
} bytepack_t;


// some globals
float sonar_dist[3];
float IR_angles[3];
float IR_window_sizes[3];



void IR_Callback(int beaconnum, float angle, float aux)
{
    OLED1 = !OLED1;

    measurmentdata* measured = (measurmentdata*)measureMQ.alloc();
    if (measured) {
        measured->ID = (measurement_t)(beaconnum+3);
        measured->value = angle;
        measured->aux = IRvariance;

        osStatus putret = measureMQ.put(measured);
        if (putret)
            OLED4 = 1;
        //    printf("putting in MQ error code %#x\r\n", putret);
    } else {
        OLED4 = 1;
        //printf("MQalloc returned NULL ptr\r\n");
    }

}

void Sonar_Callback(int num, float dist, float sonaraux)
{
    //Here is where you deal with your brand new reading ;D
    OLED2 = !OLED2;

    measurmentdata* measured = (measurmentdata*)measureMQ.alloc();
    if (measured) {
        measured->ID = (measurement_t)num;
        measured->value = dist/1000.0f;
        measured->aux = sonarvariance;

        osStatus putret = measureMQ.put(measured);
        if (putret)
            OLED4 = 1;
        //    printf("putting in MQ error code %#x\r\n", putret);
    } else {
        OLED4 = 1;
        //printf("MQalloc returned NULL ptr\r\n");
    }

}




void serial_thread(void const *argument)
{
    measurement_t type;
    float value,aux;
    //bytepack_t header,pack_value,pack_aux;
    bytepack_t datapackage;

    // first 3 bytes of header is used for alignment
    datapackage.data.header[0] = 0xFF;
    datapackage.data.header[1] = 0xFF;
    datapackage.data.header[2] = 0xFF;
    while (true) {
        osEvent evt = measureMQ.get();

        if (evt.status == osEventMail) {
            OLED3 = !OLED3;

            measurmentdata &measured = *(measurmentdata*)evt.value.p;
            type = measured.ID; //Note, may support more measurment types than sonar in the future!
            value = measured.value;
            aux = measured.aux;

            // don't forget to free the memory
            measureMQ.free(&measured);
            datapackage.data.ID = (unsigned char)(type);
            
            //if (type <= SONAR0) {
            //     printf("SONAR   %d: %0.5f   +- %f \n",type,value*1000,aux);
           // } else if ((type<=IR2)&&(type>=IR1)) {
           if (type == IR0){
                 printf("IR      %d: %0.5f     +- %f \n",type-3,value,aux);
            }
            
            datapackage.data.value = value;
            datapackage.data.aux = aux;

            // output sample to main board
            for (int i = 0; i < sizeof(datapackage); i++) {
                mbed_serial.putc(datapackage.type_char[i]);
               // pc.putc(datapackage.type_char[i]);
            }          
        }
    }
}

int main()
{
    pc.baud(115200);
    pc.printf("Hello from mbed\n");

    mbed_serial.baud(115200);


    sonar.callbackfunc = Sonar_Callback;
    ir.callbackfunc = IR_Callback;

    Thread thread_serial(serial_thread);

    while (true) {
        Thread::wait(osWaitForever);
    }
}