Diff: Kalman/IR/IR.cpp

Revision:

9:377560539b74

Parent:

8:ffc7d8af2d5a

Child:

10:294b9adbc9d3

--- a/Kalman/IR/IR.cpp	Fri Apr 27 18:36:54 2012 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,86 +0,0 @@
-#include "IR.h"
-#include "Kalman.h"
-#include "system.h"
-#include "geometryfuncs.h"
-#include "globals.h"
-#include "mbed.h"
-
-IR::IR(Kalman &kalmanin):
-#ifdef ROBOT_PRIMARY
-        IRserial(p9, p10),
-#else
-        IRserial(p13, p14),
-#endif
-        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() {
-    IRserial.attach(NULL,Serial::RxIrq);
-}
-
-void IR::attachisr() {
-    IRserial.attach(this, &IR::vIRValueISR, Serial::RxIrq);
-}
-
-void IR::vIRValueISR (void) {
-
-    // 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.
-    // UART0 for USB UART
-
-#ifdef ROBOT_PRIMARY
-    unsigned char RBR = LPC_UART3->RBR;
-#else
-    unsigned char RBR = LPC_UART1->RBR;
-#endif
-
-    // 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;
-    static int buff_pointer = 0;
-
-    if (!data_flag) { // look for alignment bytes
-        if (RBR == Alignment_char[Alignment_ptr]) {
-            Alignment_ptr ++;
-        }
-        if (Alignment_ptr >= 4) {
-            Alignment_ptr = 0;
-            data_flag = true; // set the dataflag
-        }
-    } else { // fetch data bytes
-        IRValues.IR_chars[buff_pointer] = RBR;
-        buff_pointer ++;
-        if (buff_pointer >= 12) {
-            buff_pointer = 0;
-            data_flag = false; // dessert the dataflag
-        
-            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];
-            }
-
-        }
-
-    }
-}
\ No newline at end of file