Diff: main.cpp

Revision:

10:2bd9f4e02b74

Parent:

9:08552997b544

Child:

11:5ba926692210

--- 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);
+    }
+}
+