Diff: main.cpp

Revision:

0:8a066434c28e

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Dec 12 17:25:08 2013 +0000
@@ -0,0 +1,247 @@
+#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;
+}
\ No newline at end of file