Diff: main.cpp

Revision:

0:ff94cc47fef7

Child:

1:801f0b9a862a

diff -r 000000000000 -r ff94cc47fef7 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Mar 12 21:55:44 2014 +0000
@@ -0,0 +1,200 @@
+#include "rtos.h"
+#include "PID.h"
+#include "PololuQik2.h"
+#include "QEI.h"
+#include "mbed.h"
+#include "HCSR04.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.2*127)
+
+float range, pid_return;
+void errFunction(void);
+bool cRc;
+
+//Hardware Initialization
+Serial bt(p13,p14);
+Serial pc(USBTX,USBRX);
+HCSR04 rangeFinder( PIN_TRIGGER, PIN_ECHO );
+PID pid1(1.0,0.0,0.0,0.1);
+PololuQik2 motors(p9, p10, p8, p15, errFunction, cRc);
+
+//Functions
+
+void wall_follow(void);
+void wall_follow2(int &currentLocation);
+void wall_follow3(int &currentLocation, int &WaveOpening);
+
+void us_distance(void);
+
+
+int main(void){
+
+    pc.baud(115200);
+    motors.begin();
+        
+    //motors.setMotor0Speed(MAX_SPEED); //left
+    //motors.setMotor1Speed(MAX_SPEED); //right
+    //wait(10);
+    
+    wall_follow();
+    
+    motors.stopBothMotors();
+    
+    
+}
+
+void errFunction(void){
+    //Nothing    
+}
+
+void us_distance(void)
+{
+        pc.printf("Ultra Sonic\n\r");
+        rangeFinder.startMeas();
+        wait_us(20);
+        if ( (rangeFinder.getMeas(range) == RANGE_MEAS_VALID))
+        {
+            pc.printf("Range = %f\n\r", range);
+        }  
+}
+
+void wall_follow(void)
+{
+    while(1){
+        
+        pid1.setInputLimits(1.0, 20.0);
+        pid1.setOutputLimits( -0.3*127, 0.3*127);
+        pid1.setSetPoint(10.0);
+        
+        rangeFinder.startMeas();
+        wait_ms(100);
+        if ( (rangeFinder.getMeas(range) == RANGE_MEAS_VALID) && (range < 100.0) && (range > 3.0))
+        {
+            //pc.printf("Range = %f\n", range);
+        } 
+        pid1.setProcessValue(range);
+        pid_return = pid1.compute(); 
+        pc.printf("Range: %f\n      PID:   %f", range, pid_return);
+        
+        if(pid_return > 0){
+            motors.setMotor0Speed(MAX_SPEED - pid_return);//left
+            motors.setMotor1Speed(MAX_SPEED);
+        }else if(pid_return < 0){
+            motors.setMotor0Speed(MAX_SPEED);
+            motors.setMotor1Speed(MAX_SPEED + pid_return);
+        }else{
+            motors.setMotor0Speed(MAX_SPEED);
+            motors.setMotor1Speed(MAX_SPEED);
+        }
+     }
+}
+
+/* MODIFIED WALL_FOLLOW FOR NAVIGATION */
+
+void wall_follow2(int &currentLocation)
+{
+    int SeeWaveGap = false;
+    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);
+        } 
+        
+        /*************CHECK FOR WAVE OPENING*****************/
+        /* If after 100 ms the ultrasonic still sees 20+ cm */
+        /*      then robot is at wave opening               */
+        
+        
+        if(range > 20 && !SeeWaveGap){
+            currentLocation++;
+            SeeWaveGap = true;
+        } else {
+            // AT WAVE OPENING!!!!
+            break;
+        }
+        
+            
+        pid1.setProcessValue(range);
+        pid_return = pid1.compute(); 
+        bt.printf("Range: %f\n      PID:   %f", range, pid_return);
+        
+        if(pid_return > 0){
+            motors.setMotor0Speed(MAX_SPEED - pid_return);
+            motors.setMotor1Speed(MAX_SPEED);
+        }else if(pid_return < 0){
+            motors.setMotor0Speed(MAX_SPEED);
+            motors.setMotor1Speed(MAX_SPEED + pid_return);
+        }else{
+            motors.setMotor0Speed(MAX_SPEED);
+            motors.setMotor1Speed(MAX_SPEED);
+        }
+     }
+}
+
+
+/* MODIFIED WALL_FOLLOW FOR NAVIGATION WITH WAVE OPENINGS PASSED IN */
+/* MEANT FOR RETURNING FROM OIL RIGS */
+
+void wall_follow3(int &currentLocation, int &WaveOpening)
+{
+    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);
+        } 
+        
+        /*************CHECK FOR WAVE OPENING*****************/
+        /* If after 100 ms the ultrasonic still sees 20+ cm */
+        /*      then robot is at wave opening               */
+        
+        
+        if(range > 20 ){
+            currentLocation--;
+        }
+        
+        if( currentLocation == WaveOpening){
+            // AT WAVE OPENING!!!!
+            
+            break;
+        }
+        
+            
+        pid1.setProcessValue(range);
+        pid_return = pid1.compute(); 
+        bt.printf("Range: %f\n      PID:   %f", range, pid_return);
+        
+        if(pid_return > 0){
+            motors.setMotor0Speed(MAX_SPEED - pid_return);
+            motors.setMotor1Speed(MAX_SPEED);
+        }else if(pid_return < 0){
+            motors.setMotor0Speed(MAX_SPEED);
+            motors.setMotor1Speed(MAX_SPEED + pid_return);
+        }else{
+            motors.setMotor0Speed(MAX_SPEED);
+            motors.setMotor1Speed(MAX_SPEED);
+        }
+     }
+}