![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
working version with calibration done
Fork of Eurobot2013 by
Diff: main.cpp
- Revision:
- 11:5ba926692210
- Parent:
- 10:2bd9f4e02b74
diff -r 2bd9f4e02b74 -r 5ba926692210 main.cpp --- a/main.cpp Sun Apr 07 16:30:49 2013 +0000 +++ b/main.cpp Tue Apr 09 15:32:47 2013 +0000 @@ -3,101 +3,88 @@ #include "math.h" #include "globals.h" #include "RFSRF05.h" +#include "IR_Turret.h" #include "PwmIn.h" #include "system.h" #include "geometryfuncs.h" -// Stepper motor pwm out -PwmOut stepper(STEPPER_PIN); -//InterruptIn stepper_irq(STEPPER_IRQ_PIN); - -PwmIn ir_sensor(IR_SENSOR_PIN); 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]; -// RTOS stuff -Semaphore serial_sema(1); -Semaphore IR_timeout_sema(1); - -// IR timeout timer -Timeout IR_Timeout; +void IR_Callback(int beaconnum, float angle, float aux) +{ + OLED1 = !OLED1; -// some bad globals -float my_dutycycle; -float sonar_dist[3]; -int IR_counter[3] = {0,0,0}; -int IR_step_counter[3]= {0,0,0}; -int sample_count = 0; -int step_counter; -float IR_angles[3]; -int step_count_old = 0; + measurmentdata* measured = (measurmentdata*)measureMQ.alloc(); + if (measured) { + measured->ID = (measurement_t)(beaconnum+3); + measured->value = angle; + measured->aux = IRvariance; -void IR_Timeout_isr() { - OLED4 = !OLED4; - IR_timeout_sema.wait(); - if ((IR_counter[0] > IR_counter[1]) && (IR_counter[0] > IR_counter[2])) { - IR_angles[0] = rectifyAng(((float)IR_step_counter[0]/IR_counter[0])*STEP_ANGLE); - } else if ((IR_counter[1] > IR_counter[0]) && (IR_counter[1] > IR_counter[2])) { - IR_angles[1] = rectifyAng(((float)IR_step_counter[1]/IR_counter[1])*STEP_ANGLE); - } else if ((IR_counter[2] > IR_counter[0]) && (IR_counter[2] > IR_counter[1])) { - IR_angles[2] = rectifyAng(((float)IR_step_counter[2]/IR_counter[2])*STEP_ANGLE); - } - IR_counter[0] = 0; - IR_step_counter[0] = 0; - IR_counter[1] = 0; - IR_step_counter[1] = 0; - IR_counter[2] = 0; - IR_step_counter[2] = 0; - step_count_old = 0; - - IR_timeout_sema.release(); - serial_sema.release(); + 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 IR_Callback(float pulsewidth) -{ - static int step_count_local; - - step_count_local = LPC_TIM2->TC; // save current counter value to a local - // resets timeout if pulse is captured - IR_Timeout.detach (); - - - if ( step_count_local < step_count_old ){ - step_count_local += STEPPER_DIV; - } - OLED1 = !OLED1; - IR_timeout_sema.wait(); - step_count_old = step_count_local; - if ((pulsewidth <= IR0_PULSEWIDTH + PULSEWIDTH_TOLERANCE) && (pulsewidth > IR0_PULSEWIDTH - PULSEWIDTH_TOLERANCE)) { - IR_counter[0]++; - IR_step_counter[0] += step_count_local; - } else if ((pulsewidth <= IR1_PULSEWIDTH + PULSEWIDTH_TOLERANCE) && (pulsewidth > IR1_PULSEWIDTH - PULSEWIDTH_TOLERANCE)) { - IR_counter[1]++; - IR_step_counter[1] += step_count_local; - } else if ((pulsewidth <= IR2_PULSEWIDTH + PULSEWIDTH_TOLERANCE) && (pulsewidth > IR2_PULSEWIDTH - PULSEWIDTH_TOLERANCE)) { - IR_counter[2]++; - IR_step_counter[2] += step_count_local; - } - IR_timeout_sema.release(); - - IR_Timeout.attach(&IR_Timeout_isr, IR_TIMEOUT); -} - - -void Sonar_Callback(int num, float dist) +void Sonar_Callback(int num, float dist, float sonaraux) { //Here is where you deal with your brand new reading ;D OLED2 = !OLED2; - sonar_dist[num] = dist; - //serial_sema.release(); + + 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"); + } } @@ -106,44 +93,61 @@ 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) { - serial_sema.wait(); - //printf("dutycycle: %0.4f, Sonar: %0.4f, %0.4f,%0.4f \n\r", my_dutycycle, sonar_dist[0],sonar_dist[1],sonar_dist[2]); - printf("IR0: %f, IR1: %f, IR2: %f \n\r",IR_angles[0],IR_angles[1],IR_angles[2]); - //printf("IR0: %d, IR1: %d, IR2: %d \n\r",IR_counter[0],IR_counter[1],IR_counter[2]); + 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(19200); + pc.baud(115200); pc.printf("Hello from mbed\n"); - IR_timeout_sema.release(); - - sonar.callbackfunc = Sonar_Callback; - ir_sensor.callbackfunc = IR_Callback; - - Thread thread_serial(serial_thread); - - stepper.period(STEPPER_PERIOD); - stepper.pulsewidth(STEPPER_PERIOD/2.0f); // servo position determined by a pulsewidth between 1-2ms + mbed_serial.baud(115200); - //some timer counters - //enable PCTIM2 - LPC_SC->PCONP|=(1<<22); - - //SET P30 - LPC_PINCON->PINSEL0|=((1<<8)|(1<<9)); + sonar.callbackfunc = Sonar_Callback; + ir.callbackfunc = IR_Callback; - //configure counter - LPC_TIM2->TCR =0x2;//counter disable - LPC_TIM2->CTCR =0x1;//counter mode,increments on rising edges - LPC_TIM2->PR =0x0;//set prescaler - LPC_TIM2->MR0 = STEPPER_DIV; // Match count for 3200 counts - LPC_TIM2->MCR = 2; // Reset on Match - LPC_TIM2->TCR =0x1; // counter enable + Thread thread_serial(serial_thread); while (true) { Thread::wait(osWaitForever);