IR.cpp

00001 #include "IR.h"
00002 #include "Kalman.h"
00003 #include "system.h"
00004 #include "geometryfuncs.h"
00005 #include "globals.h"
00006 #include "mbed.h"
00007 
00008 IR::IR(Kalman &kalmanin):
00009 #ifdef ROBOT_PRIMARY
00010         IRserial(p9, p10),
00011 #else
00012         IRserial(p13, p14),
00013 #endif
00014         kalman(kalmanin) {
00015 
00016     //Setting up IR serial
00017     IRserial.baud(115200);
00018     IRserial.format(8,Serial::Odd,1);
00019 }
00020 
00021 void IR::detachisr() {
00022     IRserial.attach(NULL,Serial::RxIrq);
00023 }
00024 
00025 void IR::attachisr() {
00026     IRserial.attach(this, &IR::vIRValueISR, Serial::RxIrq);
00027 }
00028 
00029 void IR::vIRValueISR (void) {
00030 
00031     // A workaround for mbed UART ISR bug
00032     // Clear the RBR flag to make sure the interrupt doesn't loop
00033     // UART3 for the port on pins 9/10, UART2 for pins 28/27, and UART1 for pins 13/14.
00034     // UART0 for USB UART
00035 
00036 #ifdef ROBOT_PRIMARY
00037     unsigned char RBR = LPC_UART3->RBR;
00038 #else
00039     unsigned char RBR = LPC_UART1->RBR;
00040 #endif
00041 
00042     // bytes packing/unpacking for IR turret serial comm
00043     static union IRValue_t {
00044         float IR_floats[3];
00045         int IR_ints[3];
00046         unsigned char IR_chars[12];
00047     } IRValues;
00048 
00049     const char Alignment_char[4] = {0xFF,0xFE,0xFD,0xFC};
00050     static int Alignment_ptr = 0;
00051     static bool data_flag = false;
00052     static int buff_pointer = 0;
00053 
00054     if (!data_flag) { // look for alignment bytes
00055         if (RBR == Alignment_char[Alignment_ptr]) {
00056             Alignment_ptr ++;
00057         }
00058         if (Alignment_ptr >= 4) {
00059             Alignment_ptr = 0;
00060             data_flag = true; // set the dataflag
00061         }
00062     } else { // fetch data bytes
00063         IRValues.IR_chars[buff_pointer] = RBR;
00064         buff_pointer ++;
00065         if (buff_pointer >= 12) {
00066             buff_pointer = 0;
00067             data_flag = false; // dessert the dataflag
00068                 kalman.runupdate(Kalman::measurement_t(IRValues.IR_ints[0]+3),IRValues.IR_floats[1],IRvariance);
00069 
00070 
00071         }
00072 
00073     }
00074 }