Shuto Naruse / Mbed 2 deprecated Eurobot2012_Primary

Eurobot2012_Primary

Dependencies:   mbed Eurobot_2012_Primary

Kalman/IR/IR.cpp@5:7ac07bf30707, 2012-04-26 (annotated)

Committer:
narshu
Date:
Thu Apr 26 23:49:49 2012 +0000
Revision:
5:7ac07bf30707
Parent:
4:7b7334441da9
Child:
7:f9c59a3e4155
Fixed Sonar and most of the kalman filter except for the IR serial;

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 5:7ac07bf30707 35
narshu 4:7b7334441da9 36
narshu 5:7ac07bf30707 37
narshu 4:7b7334441da9 38 // A workaround for mbed UART ISR bug
narshu 4:7b7334441da9 39 // Clear the RBR flag to make sure the interrupt doesn't loop
narshu 4:7b7334441da9 40 // UART3 for the port on pins 9/10, UART2 for pins 28/27, and UART1 for pins 13/14.
narshu 4:7b7334441da9 41 // UART0 for USB UART
narshu 4:7b7334441da9 42 #ifdef ROBOT_PRIMARY
narshu 4:7b7334441da9 43 unsigned char RBR = LPC_UART3->RBR;
narshu 4:7b7334441da9 44 #else
narshu 4:7b7334441da9 45 unsigned char RBR = LPC_UART1->RBR;
narshu 4:7b7334441da9 46 #endif
narshu 4:7b7334441da9 47
narshu 5:7ac07bf30707 48 //printf("%#x/n", RBR);
narshu 5:7ac07bf30707 49
narshu 5:7ac07bf30707 50 OLED3 = !OLED3;
narshu 5:7ac07bf30707 51 return;
narshu 5:7ac07bf30707 52
narshu 5:7ac07bf30707 53 // bytes packing/unpacking for IR turret serial comm
narshu 5:7ac07bf30707 54 static union IRValue_t {
narshu 5:7ac07bf30707 55 float IR_floats[3];
narshu 5:7ac07bf30707 56 int IR_ints[3];
narshu 5:7ac07bf30707 57 unsigned char IR_chars[12];
narshu 5:7ac07bf30707 58 } IRValues;
narshu 5:7ac07bf30707 59
narshu 4:7b7334441da9 60 const char Alignment_char[4] = {0xFF,0xFE,0xFD,0xFC};
narshu 4:7b7334441da9 61 static int Alignment_ptr = 0;
narshu 4:7b7334441da9 62 static bool data_flag = false;
narshu 4:7b7334441da9 63 static int buff_pointer = 0;
narshu 4:7b7334441da9 64
narshu 4:7b7334441da9 65 if (!data_flag) { // look for alignment bytes
narshu 4:7b7334441da9 66 if (RBR == Alignment_char[Alignment_ptr]) {
narshu 4:7b7334441da9 67 Alignment_ptr ++;
narshu 4:7b7334441da9 68 }
narshu 4:7b7334441da9 69 if (Alignment_ptr >= 4) {
narshu 4:7b7334441da9 70 Alignment_ptr = 0;
narshu 4:7b7334441da9 71 data_flag = true; // set the dataflag
narshu 4:7b7334441da9 72 }
narshu 4:7b7334441da9 73 } else { // fetch data bytes
narshu 4:7b7334441da9 74 IRValues.IR_chars[buff_pointer] = RBR;
narshu 4:7b7334441da9 75 buff_pointer ++;
narshu 4:7b7334441da9 76 if (buff_pointer >= 12) {
narshu 4:7b7334441da9 77 buff_pointer = 0;
narshu 4:7b7334441da9 78 data_flag = false; // dessert the dataflag
narshu 5:7ac07bf30707 79
narshu 5:7ac07bf30707 80
narshu 5:7ac07bf30707 81
narshu 5:7ac07bf30707 82 /*
narshu 4:7b7334441da9 83 if (angleInit) {
narshu 4:7b7334441da9 84 kalman.runupdate(Kalman::measurement_t(IRValues.IR_ints[0]+3),rectifyAng(IRValues.IR_floats[1] - angleOffset),IRValues.IR_floats[2]);
narshu 4:7b7334441da9 85 } else {
narshu 4:7b7334441da9 86 //dont bother to update if we dont know the offset of the IR, as it messes up the P matrix
narshu 4:7b7334441da9 87 //kalman->runupdate(kalman.measurement_t(IRValues.IR_ints[0]+3),IRValues.IR_floats[1],IRValues.IR_floats[2]);
narshu 5:7ac07bf30707 88
narshu 4:7b7334441da9 89 //only update the IRMeasures used by kalman init
narshu 4:7b7334441da9 90 kalman.IRMeasures[IRValues.IR_ints[0]] = IRValues.IR_floats[1];
narshu 4:7b7334441da9 91 }
narshu 5:7ac07bf30707 92 */
narshu 5:7ac07bf30707 93
narshu 5:7ac07bf30707 94
narshu 4:7b7334441da9 95 }
narshu 4:7b7334441da9 96
narshu 4:7b7334441da9 97 }
narshu 4:7b7334441da9 98 }