File content as of revision 10:2bd9f4e02b74:

#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;

    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();

}




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