Eurobot2012_Primary
Dependencies: mbed Eurobot_2012_Primary
Diff: Eurobot_shared/Kalman/IR/IR.cpp
- Revision:
- 16:b3dd4e0b3100
- Parent:
- 15:acae5c0e9ca8
- Child:
- 17:bafcef1c3579
--- a/Eurobot_shared/Kalman/IR/IR.cpp Sat Apr 28 22:21:03 2012 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,74 +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) { - - //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 - kalman.runupdate(Kalman::measurement_t(IRValues.IR_ints[0]+3),IRValues.IR_floats[1],IRvariance); - - - } - - } -} \ No newline at end of file