This is some awesome robot code
Dependencies: mbed-rtos mbed QEI
Fork of ICRSEurobot13 by
Communication/coprocserial.cpp@26:7cb3a21d9a2e, 2013-04-10 (annotated)
- Committer:
- madcowswe
- Date:
- Wed Apr 10 03:46:23 2013 +0000
- Revision:
- 26:7cb3a21d9a2e
- Parent:
- 20:70d651156779
- Child:
- 48:254b124cef02
Update loop rewritten, not tested not set to run
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
madcowswe | 20:70d651156779 | 1 | #include "Kalman.h" |
madcowswe | 20:70d651156779 | 2 | #include "mbed.h" |
madcowswe | 20:70d651156779 | 3 | #include "globals.h" |
madcowswe | 20:70d651156779 | 4 | |
madcowswe | 20:70d651156779 | 5 | Serial coprocserial(NC, P_SERIAL_RX); |
madcowswe | 20:70d651156779 | 6 | |
madcowswe | 20:70d651156779 | 7 | //DigitalOut OLED1(LED1); |
madcowswe | 26:7cb3a21d9a2e | 8 | DigitalOut OLED3(LED3); |
madcowswe | 20:70d651156779 | 9 | |
madcowswe | 20:70d651156779 | 10 | // bytes packing |
madcowswe | 20:70d651156779 | 11 | typedef union { |
madcowswe | 20:70d651156779 | 12 | |
madcowswe | 20:70d651156779 | 13 | struct _data{ |
madcowswe | 20:70d651156779 | 14 | unsigned char sync[3]; |
madcowswe | 20:70d651156779 | 15 | unsigned char ID; |
madcowswe | 20:70d651156779 | 16 | float value; |
madcowswe | 20:70d651156779 | 17 | float variance; |
madcowswe | 20:70d651156779 | 18 | } data; |
madcowswe | 20:70d651156779 | 19 | |
madcowswe | 20:70d651156779 | 20 | unsigned char type_char[12]; |
madcowswe | 20:70d651156779 | 21 | } bytepack_t; |
madcowswe | 20:70d651156779 | 22 | |
madcowswe | 20:70d651156779 | 23 | bytepack_t incbuff; |
madcowswe | 20:70d651156779 | 24 | |
madcowswe | 20:70d651156779 | 25 | volatile int buffprintflag = 0; |
madcowswe | 20:70d651156779 | 26 | void printbuff(){ |
madcowswe | 20:70d651156779 | 27 | while(!buffprintflag); |
madcowswe | 20:70d651156779 | 28 | buffprintflag = 0; |
madcowswe | 20:70d651156779 | 29 | for(int i = 0; i < 9; i++){ |
madcowswe | 20:70d651156779 | 30 | printf("%x ", incbuff.type_char[i]); |
madcowswe | 20:70d651156779 | 31 | } |
madcowswe | 20:70d651156779 | 32 | printf("\r\n"); |
madcowswe | 20:70d651156779 | 33 | } |
madcowswe | 20:70d651156779 | 34 | |
madcowswe | 20:70d651156779 | 35 | void procserial(){ |
madcowswe | 20:70d651156779 | 36 | |
madcowswe | 20:70d651156779 | 37 | //Fetch the byte in a "clear interrupt" sense |
madcowswe | 20:70d651156779 | 38 | unsigned char c = LPC_UART1->RBR; |
madcowswe | 20:70d651156779 | 39 | |
madcowswe | 20:70d651156779 | 40 | //OLED1 = !OLED1; |
madcowswe | 20:70d651156779 | 41 | |
madcowswe | 20:70d651156779 | 42 | static int ctr = 0; |
madcowswe | 20:70d651156779 | 43 | |
madcowswe | 20:70d651156779 | 44 | |
madcowswe | 20:70d651156779 | 45 | if (ctr < 3){ |
madcowswe | 20:70d651156779 | 46 | if (c == 0xFF) |
madcowswe | 20:70d651156779 | 47 | ctr++; |
madcowswe | 20:70d651156779 | 48 | else |
madcowswe | 20:70d651156779 | 49 | ctr = 0; |
madcowswe | 20:70d651156779 | 50 | } else { |
madcowswe | 20:70d651156779 | 51 | incbuff.type_char[ctr] = c; |
madcowswe | 20:70d651156779 | 52 | if (++ctr == 12){ |
madcowswe | 20:70d651156779 | 53 | ctr = 0; |
madcowswe | 20:70d651156779 | 54 | |
madcowswe | 26:7cb3a21d9a2e | 55 | OLED3 = !OLED3; |
madcowswe | 20:70d651156779 | 56 | buffprintflag = 1; |
madcowswe | 20:70d651156779 | 57 | |
madcowswe | 20:70d651156779 | 58 | //runupdate |
madcowswe | 20:70d651156779 | 59 | Kalman::runupdate((Kalman::measurement_t)incbuff.data.ID, incbuff.data.value, incbuff.data.variance); |
madcowswe | 20:70d651156779 | 60 | } |
madcowswe | 20:70d651156779 | 61 | } |
madcowswe | 20:70d651156779 | 62 | |
madcowswe | 20:70d651156779 | 63 | } |
madcowswe | 20:70d651156779 | 64 | |
madcowswe | 20:70d651156779 | 65 | void InitSerial(){ |
madcowswe | 20:70d651156779 | 66 | |
madcowswe | 20:70d651156779 | 67 | coprocserial.baud(115200); |
madcowswe | 20:70d651156779 | 68 | |
madcowswe | 20:70d651156779 | 69 | printf("attachserial\r\n"); |
madcowswe | 20:70d651156779 | 70 | coprocserial.attach(procserial); |
madcowswe | 20:70d651156779 | 71 | } |