Cooper Liu
/
Eurobot2013_Co-Processor
working version with calibration done
Fork of Eurobot2013 by
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?
User | Revision | Line number | New 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 |