File content as of revision 1:82463f25fd3c:

#include "mbed.h"
#include "HCSR04.h"
#include "DRV8833.h"
#include "QEI.h"
#include "PID.h"
#include "stdio.h"
#include "LPC17xx.h"

 
#define PIN_TRIGGER    (p12)
#define PIN_ECHO       (p11)
#define PULSE_PER_REV  (1192)
#define WHEEL_CIRCUM   (12.56637)
#define DIST_PER_PULSE (0.01054225722682)
#define MTRS_TO_INCH   (39.3701)
#define MAX_SPEED      (0.8)


//Hardware Initilization
Serial bt(p28,p27);
Serial pc(USBTX,USBRX);
HCSR04 rangeFinder( PIN_TRIGGER, PIN_ECHO );
DRV8833 leftMotors(p21,p22);
DRV8833 rightMotors(p23,p24);
//QEI leftEncoder(p5,p6,NC,1192,QEI::X4_ENCODING);
//QEI rightEncoder(p7,p8,NC,1192,QEI::X4_ENCODING);
PID pid1(1.0,0.0,0.0,0.1);
InterruptIn encoder(p29);

//Prototypes
float getUltraSonic(void);
void UARTIntHandler(void);
void start(void);
void stop(void);
void forward(void);
void reverse(void);
void left(void);
void right(void);
void us_distance(void);
void wall_follow(void);
void bad_command(void);
void encoderIntHandler(void);
void encoderReset(void);
float encoderDistance(void);
int command_int(char *command);


//Variables
const int num_commands = 9;
float range, echo;
char inputString[12];
int commandNumber = 0;
unsigned long temp1;
unsigned long temp2;
float pid_return;
int pulses=0;

typedef struct
{
        char command[12];
        void (*fnctPtr)(void);
}CommandListType;

CommandListType CommandTable[num_commands]={
        {"g", &start},
        {"s", &stop},
        {"f", &forward},
        {"b", &reverse},
        {"l", &left},
        {"r", &right},
        {"u", &us_distance},
        {"w", &wall_follow},
        {"error", &bad_command}
};

int main(void)
{
     
        bt.baud(115200);

        bt.attach(&UARTIntHandler,Serial::RxIrq);
        NVIC_SetPriority(UART2_IRQn, 2);
        NVIC_SetPriority(EINT3_IRQn, 0);
     
        bt.printf("Start Main\n");        
     
        encoder.rise(&encoderIntHandler);
        
     
        while(1)
        {
              
        }
     
}

float encoderDistance(void)
{
    return ((float)pulses*DIST_PER_PULSE);
}

void encoderReset(void)
{
    pulses = 0;
    return;
}

void encoderIntHandler(void)
{
    pulses++;
    return;
    
}

float getUltraSonic(void)
{ 
    return 0;
}


void UARTIntHandler(void)
{  
    bt.scanf("%s",inputString);
    while(bt.readable()){
        bt.getc();
    }
    
    bt.printf("%s\n\r", inputString);

    commandNumber = command_int(inputString); 
    CommandTable[commandNumber].fnctPtr();
    return;
}

void start(void)
{
    bt.printf("Move a foot.\n\r");
    encoderReset();
    forward();
    while(encoderDistance()<12.0){
    //bt.printf("%f\n\r",encoderDistance());
    }
    //bt.printf("%d\n\r",pulses);
    stop();
    return;
}

void stop(void)
{
    bt.printf("Stop\n");
    leftMotors.speed(0);
    rightMotors.speed(0);
    bt.printf("%d\n\r",pulses);
    return;
}

void forward(void)
{
    //bt.printf("Move Forward\n");
    leftMotors.speed(0.25);
    rightMotors.speed(0.25);
}

void reverse(void)
{
    bt.printf("Move Backward\n");
    leftMotors.speed(-0.25);
    rightMotors.speed(-0.25);
}

void left(void)
{
    bt.printf("Move Left\n");
    leftMotors.speed(0.5);
    rightMotors.speed(-0.5);
}

void right(void)
{
    bt.printf("Move Right\n");
    leftMotors.speed(-0.7);
    rightMotors.speed(0.7);
}

void us_distance(void)
{
        bt.printf("Ultra Sonic\n\r");
        rangeFinder.startMeas();
        wait_ms(10);
        if ( (rangeFinder.getMeas(range) == RANGE_MEAS_VALID) && (range < 100.0) && (range > 3.0))
        {
            bt.printf("Range = %f\n", range);
        }  

}

void wall_follow(void)
{
    while(1){
        
        pid1.setInputLimits(4.0, 20.0);
        pid1.setOutputLimits( -0.6, 0.6);
        pid1.setSetPoint(20.0);
        
        rangeFinder.startMeas();
        wait_ms(100);
        if ( (rangeFinder.getMeas(range) == RANGE_MEAS_VALID) && (range < 100.0) && (range > 3.0))
        {
            //bt.printf("Range = %f\n", range);
        } 
        pid1.setProcessValue(range);
        pid_return = pid1.compute(); 
        bt.printf("Range: %f\n      PID:   %f", range, pid_return);
        
        if(pid_return > 0){
            leftMotors.speed(MAX_SPEED - pid_return);
            rightMotors.speed(MAX_SPEED);
        }else if(pid_return < 0){
            leftMotors.speed(MAX_SPEED);
            rightMotors.speed(MAX_SPEED + pid_return);
        }else{
            leftMotors.speed(MAX_SPEED);
            rightMotors.speed(MAX_SPEED);
        }
     }
}


void bad_command(void)
{


}

int command_int(char *command)
{
    int i = 0;

    for(i=0;i<num_commands-1;i++)
    {
        if(strncmp(command,CommandTable[i].command,strlen(CommandTable[i].command))==0)
        {
            return i;
        }
    }
    return num_commands-1;
}