Cooper Liu / Mbed 2 deprecated Eurobot2013_Co-Processor

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

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