Diff: Kalman/IR/IR.cpp

Revision:

5:7ac07bf30707

Parent:

4:7b7334441da9

Child:

7:f9c59a3e4155

--- a/Kalman/IR/IR.cpp	Thu Apr 26 22:05:59 2012 +0000
+++ b/Kalman/IR/IR.cpp	Thu Apr 26 23:49:49 2012 +0000
@@ -3,42 +3,38 @@
 #include "system.h"
 #include "geometryfuncs.h"
 #include "globals.h"
+#include "mbed.h"
 
 IR::IR(Kalman &kalmanin):
 #ifdef ROBOT_PRIMARY
-    IRserial(p9, p10),
+        IRserial(p9, p10),
 #else
-    IRserial(p13, p14),
+        IRserial(p13, p14),
 #endif
-    kalman(kalmanin) {
-    
+        kalman(kalmanin) {
+
     //Starting values of IR calibration
     angleInit = false;
     angleOffset = 0;
-    
+
     //Setting up IR serial
     IRserial.baud(115200);
     IRserial.format(8,Serial::Odd,1);
-    }
-    
-void IR::detachisr(){
+}
+
+void IR::detachisr() {
     IRserial.attach(NULL,Serial::RxIrq);
 }
 
-void IR::attachisr(){
+void IR::attachisr() {
     IRserial.attach(this, &IR::vIRValueISR, Serial::RxIrq);
 }
 
 void IR::vIRValueISR (void) {
 
-    // bytes packing/unpacking for IR turret serial comm
-    union IRValue_t {
-        float IR_floats[3];
-        int IR_ints[3];
-        unsigned char IR_chars[12];
-    } IRValues;
+
 
-    
+
     // A workaround for mbed UART ISR bug
     // Clear the RBR flag to make sure the interrupt doesn't loop
     // UART3 for the port on pins 9/10, UART2 for pins 28/27, and UART1 for pins 13/14.
@@ -49,6 +45,18 @@
     unsigned char RBR = LPC_UART1->RBR;
 #endif
 
+    //printf("%#x/n", RBR);
+
+    OLED3 = !OLED3;
+    return;
+
+    // bytes packing/unpacking for IR turret serial comm
+    static union IRValue_t {
+        float IR_floats[3];
+        int IR_ints[3];
+        unsigned char IR_chars[12];
+    } IRValues;
+
     const char Alignment_char[4] = {0xFF,0xFE,0xFD,0xFC};
     static int Alignment_ptr = 0;
     static bool data_flag = false;
@@ -68,16 +76,22 @@
         if (buff_pointer >= 12) {
             buff_pointer = 0;
             data_flag = false; // dessert the dataflag
-            OLED3 = !OLED3;
+
+
+
+            /*
             if (angleInit) {
                 kalman.runupdate(Kalman::measurement_t(IRValues.IR_ints[0]+3),rectifyAng(IRValues.IR_floats[1] - angleOffset),IRValues.IR_floats[2]);
             } else {
                 //dont bother to update if we dont know the offset of the IR, as it messes up the P matrix
                 //kalman->runupdate(kalman.measurement_t(IRValues.IR_ints[0]+3),IRValues.IR_floats[1],IRValues.IR_floats[2]);
-                
+
                 //only update the IRMeasures used by kalman init
                 kalman.IRMeasures[IRValues.IR_ints[0]] = IRValues.IR_floats[1];
             }
+            */
+
+
         }
 
     }