main.cpp

00001 #include "mbed.h"
00002 #include "rtos.h"
00003 #include "math.h"
00004 #include "globals.h"
00005 #include "RFSRF05.h"
00006 #include "IR_Turret.h"
00007 #include "PwmIn.h"
00008 #include "system.h"
00009 #include "geometryfuncs.h"
00010 
00011 
00012 
00013 Serial pc(USBTX, USBRX); // tx, rx
00014 
00015 IR_TURRET ir(STEPPER_PIN,IR_SENSOR_PIN);
00016 RFSRF05 sonar(p16,p10,p11,p12,p13,p14,p15,p5,p6,p7,p8,p9);
00017 Serial mbed_serial(MBED_MAIN_SERIAL_TX,MBED_MAIN_SERIAL_RX);
00018 
00019 
00020 enum measurement_t {SONAR0 = 0, SONAR1, SONAR2, IR0, IR1, IR2};
00021 struct measurmentdata {
00022     measurement_t ID;
00023     float value;
00024     float aux;
00025 } ;
00026 
00027 Mail <measurmentdata, 16> measureMQ;
00028 
00029 // bytes packing for peer to peer communication
00030 typedef union {
00031     struct _data{
00032         unsigned char header[3];
00033         unsigned char ID;
00034         float value;
00035         float aux;
00036     }  data;  
00037     unsigned char type_char[sizeof(_data)];
00038 } bytepack_t;
00039 
00040 
00041 // some globals
00042 float sonar_dist[3];
00043 float IR_angles[3];
00044 float IR_window_sizes[3];
00045 
00046 
00047 
00048 void IR_Callback(int beaconnum, float angle, float aux)
00049 {
00050     OLED1 = !OLED1;
00051 
00052     measurmentdata* measured = (measurmentdata*)measureMQ.alloc();
00053     if (measured) {
00054         measured->ID = (measurement_t)(beaconnum+3);
00055         measured->value = angle;
00056         measured->aux = IRvariance;
00057 
00058         osStatus putret = measureMQ.put(measured);
00059         if (putret)
00060             OLED4 = 1;
00061         //    printf("putting in MQ error code %#x\r\n", putret);
00062     } else {
00063         OLED4 = 1;
00064         //printf("MQalloc returned NULL ptr\r\n");
00065     }
00066 
00067 }
00068 
00069 void Sonar_Callback(int num, float dist, float sonaraux)
00070 {
00071     //Here is where you deal with your brand new reading ;D
00072     OLED2 = !OLED2;
00073 
00074     measurmentdata* measured = (measurmentdata*)measureMQ.alloc();
00075     if (measured) {
00076         measured->ID = (measurement_t)num;
00077         measured->value = dist/1000.0f;
00078         measured->aux = sonarvariance;
00079 
00080         osStatus putret = measureMQ.put(measured);
00081         if (putret)
00082             OLED4 = 1;
00083         //    printf("putting in MQ error code %#x\r\n", putret);
00084     } else {
00085         OLED4 = 1;
00086         //printf("MQalloc returned NULL ptr\r\n");
00087     }
00088 
00089 }
00090 
00091 
00092 
00093 
00094 void serial_thread(void const *argument)
00095 {
00096     measurement_t type;
00097     float value,aux;
00098     //bytepack_t header,pack_value,pack_aux;
00099     bytepack_t datapackage;
00100 
00101     // first 3 bytes of header is used for alignment
00102     datapackage.data.header[0] = 0xFF;
00103     datapackage.data.header[1] = 0xFF;
00104     datapackage.data.header[2] = 0xFF;
00105     while (true) {
00106         osEvent evt = measureMQ.get();
00107 
00108         if (evt.status == osEventMail) {
00109             OLED3 = !OLED3;
00110 
00111             measurmentdata &measured = *(measurmentdata*)evt.value.p;
00112             type = measured.ID; //Note, may support more measurment types than sonar in the future!
00113             value = measured.value;
00114             aux = measured.aux;
00115 
00116             // don't forget to free the memory
00117             measureMQ.free(&measured);
00118             datapackage.data.ID = (unsigned char)(type);
00119             
00120             //if (type <= SONAR0) {
00121             //     printf("SONAR   %d: %0.5f   +- %f \n",type,value*1000,aux);
00122            // } else if ((type<=IR2)&&(type>=IR1)) {
00123            if (type == IR0){
00124                  printf("IR      %d: %0.5f     +- %f \n",type-3,value,aux);
00125             }
00126             
00127             datapackage.data.value = value;
00128             datapackage.data.aux = aux;
00129 
00130             // output sample to main board
00131             for (int i = 0; i < sizeof(datapackage); i++) {
00132                 mbed_serial.putc(datapackage.type_char[i]);
00133                // pc.putc(datapackage.type_char[i]);
00134             }          
00135         }
00136     }
00137 }
00138 
00139 int main()
00140 {
00141     pc.baud(115200);
00142     pc.printf("Hello from mbed\n");
00143 
00144     mbed_serial.baud(115200);
00145 
00146 
00147     sonar.callbackfunc = Sonar_Callback;
00148     ir.callbackfunc = IR_Callback;
00149 
00150     Thread thread_serial(serial_thread);
00151 
00152     while (true) {
00153         Thread::wait(osWaitForever);
00154     }
00155 }
00156