Shuto Naruse / Mbed 2 deprecated Eurobot2012_Primary

Eurobot2012_Primary

Dependencies:   mbed Eurobot_2012_Primary

Kalman/IR/IR.cpp@7:f9c59a3e4155, 2012-04-27 (annotated)

Committer:
narshu
Date:
Fri Apr 27 16:37:26 2012 +0000
Revision:
7:f9c59a3e4155
Parent:
5:7ac07bf30707 
Serial problem fixed, working on UI

Who changed what in which revision?

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