Diff: main.cpp

Revision:

11:5ba926692210

Parent:

10:2bd9f4e02b74

--- a/main.cpp	Sun Apr 07 16:30:49 2013 +0000
+++ b/main.cpp	Tue Apr 09 15:32:47 2013 +0000
@@ -3,101 +3,88 @@
 #include "math.h"
 #include "globals.h"
 #include "RFSRF05.h"
+#include "IR_Turret.h"
 #include "PwmIn.h"
 #include "system.h"
 #include "geometryfuncs.h"
 
 
-// Stepper motor pwm out
-PwmOut stepper(STEPPER_PIN);
-//InterruptIn stepper_irq(STEPPER_IRQ_PIN);
-
-PwmIn ir_sensor(IR_SENSOR_PIN);
 
 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];
 
 
 
-// RTOS stuff
-Semaphore serial_sema(1);
-Semaphore IR_timeout_sema(1);
-
-// IR timeout timer
-Timeout IR_Timeout;
+void IR_Callback(int beaconnum, float angle, float aux)
+{
+    OLED1 = !OLED1;
 
-// some bad globals
-float my_dutycycle;
-float sonar_dist[3];
-int IR_counter[3] = {0,0,0};
-int IR_step_counter[3]= {0,0,0};
-int sample_count = 0;
-int step_counter;
-float IR_angles[3];
-int step_count_old = 0;
+    measurmentdata* measured = (measurmentdata*)measureMQ.alloc();
+    if (measured) {
+        measured->ID = (measurement_t)(beaconnum+3);
+        measured->value = angle;
+        measured->aux = IRvariance;
 
-void IR_Timeout_isr() {
-    OLED4 = !OLED4;
-        IR_timeout_sema.wait();
-        if ((IR_counter[0] > IR_counter[1]) && (IR_counter[0] > IR_counter[2])) {
-        IR_angles[0] = rectifyAng(((float)IR_step_counter[0]/IR_counter[0])*STEP_ANGLE);
-        } else if ((IR_counter[1] > IR_counter[0]) && (IR_counter[1] > IR_counter[2])) {
-        IR_angles[1] = rectifyAng(((float)IR_step_counter[1]/IR_counter[1])*STEP_ANGLE);
-        } else if ((IR_counter[2] > IR_counter[0]) && (IR_counter[2] > IR_counter[1])) {
-        IR_angles[2] = rectifyAng(((float)IR_step_counter[2]/IR_counter[2])*STEP_ANGLE);
-        }    
-         IR_counter[0] = 0;
-         IR_step_counter[0] = 0;   
-         IR_counter[1] = 0;
-         IR_step_counter[1] = 0;  
-         IR_counter[2] = 0;
-         IR_step_counter[2] = 0;   
-         step_count_old = 0;
-         
-         IR_timeout_sema.release();                 
-         serial_sema.release();      
+        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 IR_Callback(float pulsewidth)
-{
-    static int step_count_local;
-
-    step_count_local = LPC_TIM2->TC;  // save current counter value to a local
-    // resets timeout if pulse is captured
-    IR_Timeout.detach ();
-    
-    
-    if ( step_count_local < step_count_old ){
-        step_count_local += STEPPER_DIV;  
-    }    
-        OLED1 = !OLED1;
-        IR_timeout_sema.wait();
-        step_count_old = step_count_local;
-        if ((pulsewidth <= IR0_PULSEWIDTH + PULSEWIDTH_TOLERANCE) && (pulsewidth > IR0_PULSEWIDTH - PULSEWIDTH_TOLERANCE)) {
-            IR_counter[0]++;
-            IR_step_counter[0] += step_count_local;
-        } else if ((pulsewidth <= IR1_PULSEWIDTH + PULSEWIDTH_TOLERANCE) && (pulsewidth > IR1_PULSEWIDTH - PULSEWIDTH_TOLERANCE)) {
-            IR_counter[1]++;
-            IR_step_counter[1] += step_count_local;
-        } else if ((pulsewidth <= IR2_PULSEWIDTH + PULSEWIDTH_TOLERANCE) && (pulsewidth > IR2_PULSEWIDTH - PULSEWIDTH_TOLERANCE)) {
-            IR_counter[2]++;
-            IR_step_counter[2] += step_count_local;
-        }
-        IR_timeout_sema.release();
-        
-        IR_Timeout.attach(&IR_Timeout_isr, IR_TIMEOUT);
-}
-
-
-void Sonar_Callback(int num, float dist)
+void Sonar_Callback(int num, float dist, float sonaraux)
 {
     //Here is where you deal with your brand new reading ;D
     OLED2 = !OLED2;
-    sonar_dist[num] = dist;
-    //serial_sema.release();
+
+    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");
+    }
 
 }
 
@@ -106,44 +93,61 @@
 
 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) {
-        serial_sema.wait();
-        //printf("dutycycle: %0.4f, Sonar: %0.4f, %0.4f,%0.4f \n\r", my_dutycycle, sonar_dist[0],sonar_dist[1],sonar_dist[2]);
-        printf("IR0: %f, IR1: %f, IR2: %f  \n\r",IR_angles[0],IR_angles[1],IR_angles[2]);
-        //printf("IR0: %d, IR1: %d, IR2: %d  \n\r",IR_counter[0],IR_counter[1],IR_counter[2]);
+        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(19200);
+    pc.baud(115200);
     pc.printf("Hello from mbed\n");
 
-    IR_timeout_sema.release();
-
-    sonar.callbackfunc = Sonar_Callback;
-    ir_sensor.callbackfunc = IR_Callback;
-
-    Thread thread_serial(serial_thread);
-
-    stepper.period(STEPPER_PERIOD);
-    stepper.pulsewidth(STEPPER_PERIOD/2.0f); // servo position determined by a pulsewidth between 1-2ms
+    mbed_serial.baud(115200);
 
 
-    //some timer counters
-    //enable PCTIM2
-    LPC_SC->PCONP|=(1<<22);
-
-    //SET P30
-    LPC_PINCON->PINSEL0|=((1<<8)|(1<<9));
+    sonar.callbackfunc = Sonar_Callback;
+    ir.callbackfunc = IR_Callback;
 
-    //configure counter
-    LPC_TIM2->TCR   =0x2;//counter disable
-    LPC_TIM2->CTCR  =0x1;//counter mode,increments on rising edges
-    LPC_TIM2->PR    =0x0;//set prescaler
-    LPC_TIM2->MR0   = STEPPER_DIV;        // Match count for 3200 counts
-    LPC_TIM2->MCR   = 2;           // Reset on Match
-    LPC_TIM2->TCR   =0x1;           // counter enable
+    Thread thread_serial(serial_thread);
 
     while (true) {
         Thread::wait(osWaitForever);