Shuto Naruse / Mbed 2 deprecated Eurobot2012_Secondary

Eurobot2012_Secondary

Fork of Eurobot_2012_Secondary by Shuto Naruse

Eurobot_shared/Kalman/IR/IR.cpp@1:cc2a9eb0bd55, 2012-10-17 (annotated)

Committer:
narshu
Date:
Wed Oct 17 22:25:31 2012 +0000
Revision:
1:cc2a9eb0bd55
Commit before publishing

Who changed what in which revision?

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