commit!
Embed:
(wiki syntax)
Show/hide line numbers
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 }
Generated on Wed Jul 13 2022 17:39:34 by 1.7.2