Cooper Liu
/
Eurobot2013_Co-Processor
working version with calibration done
Fork of Eurobot2013 by
main.cpp
- Committer:
- xiaxia686
- Date:
- 2013-04-09
- Revision:
- 11:5ba926692210
- Parent:
- 10:2bd9f4e02b74
File content as of revision 11:5ba926692210:
#include "mbed.h" #include "rtos.h" #include "math.h" #include "globals.h" #include "RFSRF05.h" #include "IR_Turret.h" #include "PwmIn.h" #include "system.h" #include "geometryfuncs.h" Serial pc(USBTX, USBRX); // tx, rx IR_TURRET ir(STEPPER_PIN,IR_SENSOR_PIN); RFSRF05 sonar(p16,p10,p11,p12,p13,p14,p15,p5,p6,p7,p8,p9); Serial mbed_serial(MBED_MAIN_SERIAL_TX,MBED_MAIN_SERIAL_RX); enum measurement_t {SONAR0 = 0, SONAR1, SONAR2, IR0, IR1, IR2}; struct measurmentdata { measurement_t ID; float value; float aux; } ; Mail <measurmentdata, 16> measureMQ; // bytes packing for peer to peer communication typedef union { struct _data{ unsigned char header[3]; unsigned char ID; float value; float aux; } data; unsigned char type_char[sizeof(_data)]; } bytepack_t; // some globals float sonar_dist[3]; float IR_angles[3]; float IR_window_sizes[3]; void IR_Callback(int beaconnum, float angle, float aux) { OLED1 = !OLED1; measurmentdata* measured = (measurmentdata*)measureMQ.alloc(); if (measured) { measured->ID = (measurement_t)(beaconnum+3); measured->value = angle; measured->aux = IRvariance; osStatus putret = measureMQ.put(measured); if (putret) OLED4 = 1; // printf("putting in MQ error code %#x\r\n", putret); } else { OLED4 = 1; //printf("MQalloc returned NULL ptr\r\n"); } } void Sonar_Callback(int num, float dist, float sonaraux) { //Here is where you deal with your brand new reading ;D OLED2 = !OLED2; measurmentdata* measured = (measurmentdata*)measureMQ.alloc(); if (measured) { measured->ID = (measurement_t)num; measured->value = dist/1000.0f; measured->aux = sonarvariance; osStatus putret = measureMQ.put(measured); if (putret) OLED4 = 1; // printf("putting in MQ error code %#x\r\n", putret); } else { OLED4 = 1; //printf("MQalloc returned NULL ptr\r\n"); } } void serial_thread(void const *argument) { measurement_t type; float value,aux; //bytepack_t header,pack_value,pack_aux; bytepack_t datapackage; // first 3 bytes of header is used for alignment datapackage.data.header[0] = 0xFF; datapackage.data.header[1] = 0xFF; datapackage.data.header[2] = 0xFF; while (true) { osEvent evt = measureMQ.get(); if (evt.status == osEventMail) { OLED3 = !OLED3; measurmentdata &measured = *(measurmentdata*)evt.value.p; type = measured.ID; //Note, may support more measurment types than sonar in the future! value = measured.value; aux = measured.aux; // don't forget to free the memory measureMQ.free(&measured); datapackage.data.ID = (unsigned char)(type); //if (type <= SONAR0) { // printf("SONAR %d: %0.5f +- %f \n",type,value*1000,aux); // } else if ((type<=IR2)&&(type>=IR1)) { if (type == IR0){ printf("IR %d: %0.5f +- %f \n",type-3,value,aux); } datapackage.data.value = value; datapackage.data.aux = aux; // output sample to main board for (int i = 0; i < sizeof(datapackage); i++) { mbed_serial.putc(datapackage.type_char[i]); // pc.putc(datapackage.type_char[i]); } } } } int main() { pc.baud(115200); pc.printf("Hello from mbed\n"); mbed_serial.baud(115200); sonar.callbackfunc = Sonar_Callback; ir.callbackfunc = IR_Callback; Thread thread_serial(serial_thread); while (true) { Thread::wait(osWaitForever); } }