Shuto Naruse / Mbed 2 deprecated Eurobot2012_Primary

Eurobot2012_Primary

Dependencies:   mbed Eurobot_2012_Primary

Kalman/IR/IR.cpp@2:cffa347bb943, 2012-04-26 (annotated)

Committer:
narshu
Date:
Thu Apr 26 21:02:12 2012 +0000
Revision:
2:cffa347bb943
not working

Who changed what in which revision?

UserRevisionLine numberNew contents of line
narshu 2:cffa347bb943 1 #include "IR.h"
narshu 2:cffa347bb943 2 #include "Kalman.h"
narshu 2:cffa347bb943 3 #include "system.h"
narshu 2:cffa347bb943 4 #include "geometryfuncs.h"
narshu 2:cffa347bb943 5
narshu 2:cffa347bb943 6 IR::IR(Kalman &kalmanin, PinName TX, PinName RX):
narshu 2:cffa347bb943 7 IRserial(p9, p10),
narshu 2:cffa347bb943 8 kalman(kalmanin) {
narshu 2:cffa347bb943 9
narshu 2:cffa347bb943 10 //Starting values of IR calibration
narshu 2:cffa347bb943 11 angleInit = false;
narshu 2:cffa347bb943 12 angleOffset = 0;
narshu 2:cffa347bb943 13
narshu 2:cffa347bb943 14 //Setting up IR serial
narshu 2:cffa347bb943 15 IRserial.baud(115200);
narshu 2:cffa347bb943 16 IRserial.format(8,Serial::Odd,1);
narshu 2:cffa347bb943 17 IRserial.attach(this, &IR::vIRValueISR, Serial::RxIrq);
narshu 2:cffa347bb943 18
narshu 2:cffa347bb943 19 }
narshu 2:cffa347bb943 20
narshu 2:cffa347bb943 21 void IR::detachisr(){
narshu 2:cffa347bb943 22 IRserial.attach(NULL,Serial::RxIrq);
narshu 2:cffa347bb943 23 }
narshu 2:cffa347bb943 24
narshu 2:cffa347bb943 25 void IR::attachisr(){
narshu 2:cffa347bb943 26 IRserial.attach(this, &IR::vIRValueISR, Serial::RxIrq);
narshu 2:cffa347bb943 27 }
narshu 2:cffa347bb943 28
narshu 2:cffa347bb943 29 void IR::vIRValueISR (void) {
narshu 2:cffa347bb943 30
narshu 2:cffa347bb943 31 // bytes packing/unpacking for IR turret serial comm
narshu 2:cffa347bb943 32 union IRValue_t {
narshu 2:cffa347bb943 33 float IR_floats[3];
narshu 2:cffa347bb943 34 int IR_ints[3];
narshu 2:cffa347bb943 35 unsigned char IR_chars[12];
narshu 2:cffa347bb943 36 } IRValues;
narshu 2:cffa347bb943 37
narshu 2:cffa347bb943 38
narshu 2:cffa347bb943 39 // A workaround for mbed UART ISR bug
narshu 2:cffa347bb943 40 // Clear the RBR flag to make sure the interrupt doesn't loop
narshu 2:cffa347bb943 41 // UART3 for the port on pins 9/10, UART2 for pins 28/27, and UART1 for pins 13/14.
narshu 2:cffa347bb943 42 // UART0 for USB UART
narshu 2:cffa347bb943 43 unsigned char RBR = LPC_UART1->RBR;
narshu 2:cffa347bb943 44
narshu 2:cffa347bb943 45 const char Alignment_char[4] = {0xFF,0xFE,0xFD,0xFC};
narshu 2:cffa347bb943 46 static int Alignment_ptr = 0;
narshu 2:cffa347bb943 47 static bool data_flag = false;
narshu 2:cffa347bb943 48 static int buff_pointer = 0;
narshu 2:cffa347bb943 49
narshu 2:cffa347bb943 50 if (!data_flag) { // look for alignment bytes
narshu 2:cffa347bb943 51 if (RBR == Alignment_char[Alignment_ptr]) {
narshu 2:cffa347bb943 52 Alignment_ptr ++;
narshu 2:cffa347bb943 53 }
narshu 2:cffa347bb943 54 if (Alignment_ptr >= 4) {
narshu 2:cffa347bb943 55 Alignment_ptr = 0;
narshu 2:cffa347bb943 56 data_flag = true; // set the dataflag
narshu 2:cffa347bb943 57 }
narshu 2:cffa347bb943 58 } else { // fetch data bytes
narshu 2:cffa347bb943 59 IRValues.IR_chars[buff_pointer] = RBR;
narshu 2:cffa347bb943 60 buff_pointer ++;
narshu 2:cffa347bb943 61 if (buff_pointer >= 12) {
narshu 2:cffa347bb943 62 buff_pointer = 0;
narshu 2:cffa347bb943 63 data_flag = false; // dessert the dataflag
narshu 2:cffa347bb943 64 OLED3 = !OLED3;
narshu 2:cffa347bb943 65 if (angleInit) {
narshu 2:cffa347bb943 66 kalman.runupdate(Kalman::measurement_t(IRValues.IR_ints[0]+3),rectifyAng(IRValues.IR_floats[1] - angleOffset),IRValues.IR_floats[2]);
narshu 2:cffa347bb943 67 } else {
narshu 2:cffa347bb943 68 //dont bother to update if we dont know the offset of the IR, as it messes up the P matrix
narshu 2:cffa347bb943 69 //kalman->runupdate(kalman.measurement_t(IRValues.IR_ints[0]+3),IRValues.IR_floats[1],IRValues.IR_floats[2]);
narshu 2:cffa347bb943 70
narshu 2:cffa347bb943 71 //only update the IRMeasures used by kalman init
narshu 2:cffa347bb943 72 kalman.IRMeasures[IRValues.IR_ints[0]] = IRValues.IR_floats[1];
narshu 2:cffa347bb943 73 }
narshu 2:cffa347bb943 74 }
narshu 2:cffa347bb943 75
narshu 2:cffa347bb943 76 }
narshu 2:cffa347bb943 77 }