Eurobot2012_Primary
Dependencies: mbed Eurobot_2012_Primary
Diff: Kalman/IR/IR.cpp
- Revision:
- 3:429829612cf9
- Parent:
- 2:cffa347bb943
- Child:
- 4:7b7334441da9
--- a/Kalman/IR/IR.cpp Thu Apr 26 21:02:12 2012 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,77 +0,0 @@ -#include "IR.h" -#include "Kalman.h" -#include "system.h" -#include "geometryfuncs.h" - -IR::IR(Kalman &kalmanin, PinName TX, PinName RX): - IRserial(p9, p10), - kalman(kalmanin) { - - //Starting values of IR calibration - angleInit = false; - angleOffset = 0; - - //Setting up IR serial - IRserial.baud(115200); - IRserial.format(8,Serial::Odd,1); - IRserial.attach(this, &IR::vIRValueISR, Serial::RxIrq); - - } - -void IR::detachisr(){ - IRserial.attach(NULL,Serial::RxIrq); -} - -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. - // UART0 for USB UART - unsigned char RBR = LPC_UART1->RBR; - - 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 - 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]; - } - } - - } -} \ No newline at end of file