Cooper Liu / Mbed 2 deprecated Eurobot2013_Co-Processor

working version with calibration done

Fork of Eurobot2013 by Oskar Weigl

main.cpp@11:5ba926692210, 2013-04-09 (annotated)

Committer:
xiaxia686
Date:
Tue Apr 09 15:32:47 2013 +0000
Revision:
11:5ba926692210
Parent:
10:2bd9f4e02b74 
woking version (calibrated)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
madcowswe 0:92019d8564a7 1 #include "mbed.h"
xiaxia686 10:2bd9f4e02b74 2 #include "rtos.h"
xiaxia686 10:2bd9f4e02b74 3 #include "math.h"
madcowswe 6:5a52c046d8f7 4 #include "globals.h"
xiaxia686 10:2bd9f4e02b74 5 #include "RFSRF05.h"
xiaxia686 11:5ba926692210 6 #include "IR_Turret.h"
xiaxia686 10:2bd9f4e02b74 7 #include "PwmIn.h"
xiaxia686 10:2bd9f4e02b74 8 #include "system.h"
xiaxia686 10:2bd9f4e02b74 9 #include "geometryfuncs.h"
xiaxia686 10:2bd9f4e02b74 10
xiaxia686 10:2bd9f4e02b74 11
xiaxia686 10:2bd9f4e02b74 12
xiaxia686 10:2bd9f4e02b74 13 Serial pc(USBTX, USBRX); // tx, rx
xiaxia686 10:2bd9f4e02b74 14
xiaxia686 11:5ba926692210 15 IR_TURRET ir(STEPPER_PIN,IR_SENSOR_PIN);
xiaxia686 10:2bd9f4e02b74 16 RFSRF05 sonar(p16,p10,p11,p12,p13,p14,p15,p5,p6,p7,p8,p9);
xiaxia686 11:5ba926692210 17 Serial mbed_serial(MBED_MAIN_SERIAL_TX,MBED_MAIN_SERIAL_RX);
xiaxia686 11:5ba926692210 18
xiaxia686 11:5ba926692210 19
xiaxia686 11:5ba926692210 20 enum measurement_t {SONAR0 = 0, SONAR1, SONAR2, IR0, IR1, IR2};
xiaxia686 11:5ba926692210 21 struct measurmentdata {
xiaxia686 11:5ba926692210 22 measurement_t ID;
xiaxia686 11:5ba926692210 23 float value;
xiaxia686 11:5ba926692210 24 float aux;
xiaxia686 11:5ba926692210 25 } ;
xiaxia686 11:5ba926692210 26
xiaxia686 11:5ba926692210 27 Mail <measurmentdata, 16> measureMQ;
xiaxia686 11:5ba926692210 28
xiaxia686 11:5ba926692210 29 // bytes packing for peer to peer communication
xiaxia686 11:5ba926692210 30 typedef union {
xiaxia686 11:5ba926692210 31 struct _data{
xiaxia686 11:5ba926692210 32 unsigned char header[3];
xiaxia686 11:5ba926692210 33 unsigned char ID;
xiaxia686 11:5ba926692210 34 float value;
xiaxia686 11:5ba926692210 35 float aux;
xiaxia686 11:5ba926692210 36 } data;
xiaxia686 11:5ba926692210 37 unsigned char type_char[sizeof(_data)];
xiaxia686 11:5ba926692210 38 } bytepack_t;
xiaxia686 11:5ba926692210 39
xiaxia686 11:5ba926692210 40
xiaxia686 11:5ba926692210 41 // some globals
xiaxia686 11:5ba926692210 42 float sonar_dist[3];
xiaxia686 11:5ba926692210 43 float IR_angles[3];
xiaxia686 11:5ba926692210 44 float IR_window_sizes[3];
xiaxia686 10:2bd9f4e02b74 45
xiaxia686 10:2bd9f4e02b74 46
xiaxia686 10:2bd9f4e02b74 47
xiaxia686 11:5ba926692210 48 void IR_Callback(int beaconnum, float angle, float aux)
xiaxia686 11:5ba926692210 49 {
xiaxia686 11:5ba926692210 50 OLED1 = !OLED1;
xiaxia686 10:2bd9f4e02b74 51
xiaxia686 11:5ba926692210 52 measurmentdata* measured = (measurmentdata*)measureMQ.alloc();
xiaxia686 11:5ba926692210 53 if (measured) {
xiaxia686 11:5ba926692210 54 measured->ID = (measurement_t)(beaconnum+3);
xiaxia686 11:5ba926692210 55 measured->value = angle;
xiaxia686 11:5ba926692210 56 measured->aux = IRvariance;
xiaxia686 10:2bd9f4e02b74 57
xiaxia686 11:5ba926692210 58 osStatus putret = measureMQ.put(measured);
xiaxia686 11:5ba926692210 59 if (putret)
xiaxia686 11:5ba926692210 60 OLED4 = 1;
xiaxia686 11:5ba926692210 61 // printf("putting in MQ error code %#x\r\n", putret);
xiaxia686 11:5ba926692210 62 } else {
xiaxia686 11:5ba926692210 63 OLED4 = 1;
xiaxia686 11:5ba926692210 64 //printf("MQalloc returned NULL ptr\r\n");
xiaxia686 11:5ba926692210 65 }
xiaxia686 11:5ba926692210 66
xiaxia686 10:2bd9f4e02b74 67 }
xiaxia686 10:2bd9f4e02b74 68
xiaxia686 11:5ba926692210 69 void Sonar_Callback(int num, float dist, float sonaraux)
xiaxia686 10:2bd9f4e02b74 70 {
xiaxia686 10:2bd9f4e02b74 71 //Here is where you deal with your brand new reading ;D
xiaxia686 10:2bd9f4e02b74 72 OLED2 = !OLED2;
xiaxia686 11:5ba926692210 73
xiaxia686 11:5ba926692210 74 measurmentdata* measured = (measurmentdata*)measureMQ.alloc();
xiaxia686 11:5ba926692210 75 if (measured) {
xiaxia686 11:5ba926692210 76 measured->ID = (measurement_t)num;
xiaxia686 11:5ba926692210 77 measured->value = dist/1000.0f;
xiaxia686 11:5ba926692210 78 measured->aux = sonarvariance;
xiaxia686 11:5ba926692210 79
xiaxia686 11:5ba926692210 80 osStatus putret = measureMQ.put(measured);
xiaxia686 11:5ba926692210 81 if (putret)
xiaxia686 11:5ba926692210 82 OLED4 = 1;
xiaxia686 11:5ba926692210 83 // printf("putting in MQ error code %#x\r\n", putret);
xiaxia686 11:5ba926692210 84 } else {
xiaxia686 11:5ba926692210 85 OLED4 = 1;
xiaxia686 11:5ba926692210 86 //printf("MQalloc returned NULL ptr\r\n");
xiaxia686 11:5ba926692210 87 }
xiaxia686 10:2bd9f4e02b74 88
xiaxia686 10:2bd9f4e02b74 89 }
xiaxia686 10:2bd9f4e02b74 90
xiaxia686 10:2bd9f4e02b74 91
xiaxia686 10:2bd9f4e02b74 92
madcowswe 0:92019d8564a7 93
xiaxia686 10:2bd9f4e02b74 94 void serial_thread(void const *argument)
xiaxia686 10:2bd9f4e02b74 95 {
xiaxia686 11:5ba926692210 96 measurement_t type;
xiaxia686 11:5ba926692210 97 float value,aux;
xiaxia686 11:5ba926692210 98 //bytepack_t header,pack_value,pack_aux;
xiaxia686 11:5ba926692210 99 bytepack_t datapackage;
xiaxia686 11:5ba926692210 100
xiaxia686 11:5ba926692210 101 // first 3 bytes of header is used for alignment
xiaxia686 11:5ba926692210 102 datapackage.data.header[0] = 0xFF;
xiaxia686 11:5ba926692210 103 datapackage.data.header[1] = 0xFF;
xiaxia686 11:5ba926692210 104 datapackage.data.header[2] = 0xFF;
xiaxia686 10:2bd9f4e02b74 105 while (true) {
xiaxia686 11:5ba926692210 106 osEvent evt = measureMQ.get();
xiaxia686 11:5ba926692210 107
xiaxia686 11:5ba926692210 108 if (evt.status == osEventMail) {
xiaxia686 11:5ba926692210 109 OLED3 = !OLED3;
xiaxia686 11:5ba926692210 110
xiaxia686 11:5ba926692210 111 measurmentdata &measured = *(measurmentdata*)evt.value.p;
xiaxia686 11:5ba926692210 112 type = measured.ID; //Note, may support more measurment types than sonar in the future!
xiaxia686 11:5ba926692210 113 value = measured.value;
xiaxia686 11:5ba926692210 114 aux = measured.aux;
xiaxia686 11:5ba926692210 115
xiaxia686 11:5ba926692210 116 // don't forget to free the memory
xiaxia686 11:5ba926692210 117 measureMQ.free(&measured);
xiaxia686 11:5ba926692210 118 datapackage.data.ID = (unsigned char)(type);
xiaxia686 11:5ba926692210 119
xiaxia686 11:5ba926692210 120 //if (type <= SONAR0) {
xiaxia686 11:5ba926692210 121 // printf("SONAR %d: %0.5f +- %f \n",type,value*1000,aux);
xiaxia686 11:5ba926692210 122 // } else if ((type<=IR2)&&(type>=IR1)) {
xiaxia686 11:5ba926692210 123 if (type == IR0){
xiaxia686 11:5ba926692210 124 printf("IR %d: %0.5f +- %f \n",type-3,value,aux);
xiaxia686 11:5ba926692210 125 }
xiaxia686 11:5ba926692210 126
xiaxia686 11:5ba926692210 127 datapackage.data.value = value;
xiaxia686 11:5ba926692210 128 datapackage.data.aux = aux;
xiaxia686 11:5ba926692210 129
xiaxia686 11:5ba926692210 130 // output sample to main board
xiaxia686 11:5ba926692210 131 for (int i = 0; i < sizeof(datapackage); i++) {
xiaxia686 11:5ba926692210 132 mbed_serial.putc(datapackage.type_char[i]);
xiaxia686 11:5ba926692210 133 // pc.putc(datapackage.type_char[i]);
xiaxia686 11:5ba926692210 134 }
xiaxia686 11:5ba926692210 135 }
xiaxia686 10:2bd9f4e02b74 136 }
madcowswe 0:92019d8564a7 137 }
xiaxia686 10:2bd9f4e02b74 138
xiaxia686 10:2bd9f4e02b74 139 int main()
xiaxia686 10:2bd9f4e02b74 140 {
xiaxia686 11:5ba926692210 141 pc.baud(115200);
xiaxia686 10:2bd9f4e02b74 142 pc.printf("Hello from mbed\n");
xiaxia686 10:2bd9f4e02b74 143
xiaxia686 11:5ba926692210 144 mbed_serial.baud(115200);
xiaxia686 10:2bd9f4e02b74 145
xiaxia686 10:2bd9f4e02b74 146
xiaxia686 11:5ba926692210 147 sonar.callbackfunc = Sonar_Callback;
xiaxia686 11:5ba926692210 148 ir.callbackfunc = IR_Callback;
xiaxia686 10:2bd9f4e02b74 149
xiaxia686 11:5ba926692210 150 Thread thread_serial(serial_thread);
xiaxia686 10:2bd9f4e02b74 151
xiaxia686 10:2bd9f4e02b74 152 while (true) {
xiaxia686 10:2bd9f4e02b74 153 Thread::wait(osWaitForever);
xiaxia686 10:2bd9f4e02b74 154 }
xiaxia686 10:2bd9f4e02b74 155 }
xiaxia686 10:2bd9f4e02b74 156