Cooper Liu / Mbed 2 deprecated Eurobot2013_Co-Processor

working version with calibration done

Fork of Eurobot2013 by Oskar Weigl

Kalman/IR/IR.cpp@1:6799c07fe510, 2012-11-07 (annotated)

Committer:
sv
Date:
Wed Nov 07 14:37:35 2012 +0000
Revision:
1:6799c07fe510
Preliminary copy of 2012 code

Who changed what in which revision?

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