Cooper Liu
/
Eurobot2013_Co-Processor
working version with calibration done
Fork of Eurobot2013 by
Diff: main.cpp
- Revision:
- 10:2bd9f4e02b74
- Parent:
- 9:08552997b544
- Child:
- 11:5ba926692210
diff -r 08552997b544 -r 2bd9f4e02b74 main.cpp --- a/main.cpp Wed Nov 14 17:15:53 2012 +0000 +++ b/main.cpp Sun Apr 07 16:30:49 2013 +0000 @@ -1,10 +1,152 @@ #include "mbed.h" +#include "rtos.h" +#include "math.h" #include "globals.h" +#include "RFSRF05.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 + +RFSRF05 sonar(p16,p10,p11,p12,p13,p14,p15,p5,p6,p7,p8,p9); + + + +// RTOS stuff +Semaphore serial_sema(1); +Semaphore IR_timeout_sema(1); + +// IR timeout timer +Timeout IR_Timeout; + +// 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; + +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(); +} + + + +void IR_Callback(float pulsewidth) +{ + static int step_count_local; -bool Colour = 1; // 1 for red, 0 for blue -pos beaconpos[] = {{3000, 1000},{0,0}, {0,2000}}; //predefined red start + 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) +{ + //Here is where you deal with your brand new reading ;D + OLED2 = !OLED2; + sonar_dist[num] = dist; + //serial_sema.release(); + +} + + + -//note that nothing is running as of now. -int main() { - while(1); +void serial_thread(void const *argument) +{ + 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]); + } } + +int main() +{ + pc.baud(19200); + 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 + + + //some timer counters + //enable PCTIM2 + LPC_SC->PCONP|=(1<<22); + + //SET P30 + LPC_PINCON->PINSEL0|=((1<<8)|(1<<9)); + + //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 + + while (true) { + Thread::wait(osWaitForever); + } +} +