Diff: main.cpp

Revision:

2:0351277ee5dd

Parent:

1:801f0b9a862a

Child:

6:109f46d3cb96

--- a/main.cpp	Thu Mar 13 20:45:56 2014 +0000
+++ b/main.cpp	Wed Mar 19 22:10:43 2014 +0000
@@ -6,236 +6,450 @@
 #include "HCSR04.h"
 #include "stdio.h"
 #include "LPC17xx.h"
-
-#define PIN_TRIGGER    (p12)
-#define PIN_ECHO       (p11)
+ 
+#define PIN_TRIGGERL    (p12)
+#define PIN_ECHOL       (p11)
+#define PIN_TRIGGERR    (p29)
+#define PIN_ECHOR       (p30)
 #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.3*127)
-
+#define PPR            (4331/4)
+#define LEFT           (1)
+#define RIGHT          (0)
+#define FORWARD        (1)
+#define BACKWARD       (0) 
+#define TOOLS          (0)
+#define MID            (1)
+#define RIGS           (2)  
+ 
 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(2.0,2.0,0.0,0.02);
+HCSR04 rangeFinderLeft( PIN_TRIGGERL, PIN_ECHOL );
+HCSR04 rangeFinderRight( PIN_TRIGGERR, PIN_ECHOR );
+PID pid1(15.0,0.0,4.0,0.02);
 PololuQik2 motors(p9, p10, p8, p15, errFunction, cRc);
-QEI leftEncoder(p17,p18,NC,1192,QEI::X4_ENCODING);
-//QEI rightEncoder(p19,p20,NC,1192,QEI::X4_ENCODING);
+QEI rightEncoder(p17,p18,NC,PPR,QEI::X4_ENCODING);
+QEI leftEncoder(p16,p15,NC,PPR,QEI::X4_ENCODING);
 //InterruptIn encoder(p29);
-
-
+ 
+ 
 //Functions
-
-void wall_follow(void);
-void wall_follow2(int *currentLocation);
+ 
+float wall_follow(int side, int direction, int section);
+void wall_follow2(int side, int direction, int section);
 void wall_follow3(int &currentLocation, int &WaveOpening);
 void leftTurn(void);
 void rightTurn(void);
 void us_distance(void);
-
+void tools_section(float *location, float &current);
+void overBump(void);
+ 
 //Variables
-
-int main(void){
-    int location=0;
-
+ 
+int main(void)
+{
+    float location[3], current=0;
+    int direction[3];
+ 
     pc.baud(115200);
     bt.baud(115200);
-    motors.begin();  
-        
-    /*motors.setMotor0Speed(MAX_SPEED); //left
-    motors.setMotor1Speed(MAX_SPEED); //right
-    wait_ms(350);
+    motors.begin();
+    /*
+    //  Very Consistent Turn
+    leftEncoder.reset();
+    rightEncoder.reset();
+    
+    motors.setMotor0Speed(-MAX_SPEED); //right
+    motors.setMotor1Speed(MAX_SPEED); //left
+    float usValue = 0;
+    while(1){
+        rangeFinderLeft.startMeas();
+        wait_ms(100);
+        rangeFinderLeft.getMeas(range);
+        bt.printf("Range %f \t OldValue %f\n\r",range, usValue);
+        if(range > usValue && usValue != 0 && range < 10){
+            break;
+        } else {
+            usValue = range;    
+        }
+    }
+    */
+    motors.stopBothMotors();
+    /*
+    
+    leftEncoder.reset();
+    rightEncoder.reset();
+    motors.setMotor0Speed(MAX_SPEED); //right
+    motors.setMotor1Speed(MAX_SPEED); //left
+
+    while(leftEncoder.getPulses()/(PPR) < 3);
     */
-    //wall_follow();
-    //wall_follow2(&location);
-    pc.printf("%d\n\r",location);
+    
+    //leftEncoder.reset();
+    //rightEncoder.reset();
+    //motors.setMotor0Speed(MAX_SPEED); //right
+    //motors.setMotor1Speed(MAX_SPEED); //left
+    
+    //while((abs(leftEncoder.getPulses())/(PPR) + abs(rightEncoder.getPulses())/(PPR))/2 < 3)
+ 
+    
+    
+    //Go to tools
+    tools_section(location, current);
+    bt.printf("Location 0 = %f", location[0]);
     
-    motors.stopBothMotors();
+ /*   
+    //////////////////////////////// without predefined wavegaps//////////////////////////////////////////////
+    current=0;
+    if(location[0]< 75){
+        turnRight();
+        current=wall_follow(LEFT,FORWARD);
+        if(current == 0)turnLeft();
+        else{
+            direction[0]= RIGHT;
+            turnLeft();
+            overBump();
+        }
+    }
+    else if(location[0]>=75 || current == 0){  
+        turnLeft();
+        wall_follow2(RIGHT,FORWARD);  
+    }
+        
+    
+    
+    
+    
+    
+    // left or right
+    
+    
+    location[1]=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
+    
+    
+    
+    
+     
+    
+    
     leftTurn();
-    wait(1);
+    //wall_follow2(RIGHT);
     rightTurn();
     
+ 
+ 
+    bt.printf("LOCATION %f\n\r",location);
+ 
+    motors.stopBothMotors();
+//   leftTurn();
+//   wait(1);
+//   rightTurn();
+*/
+ 
 }
-
-void errFunction(void){
-    //Nothing    
+ 
+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);
-        }  
+    pc.printf("Ultra Sonic\n\r");
+    rangeFinderLeft.startMeas();
+    wait_us(20);
+    if ( (rangeFinderLeft.getMeas(range) == RANGE_MEAS_VALID)) {
+        pc.printf("Range = %f\n\r", range);
+    }
 }
-
-void wall_follow(void)
+ 
+float wall_follow(int side, int direction, int section)
 {
-    while(1){
+    float location, wavegap;
+    int dir=1, set=5;
+    
+    pid1.reset();
+    
+    if(direction == BACKWARD) dir=-1;
+    if(section == TOOLS)set= 5;
+    
+    leftEncoder.reset();
+    rightEncoder.reset();
+    
+    location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
+    
+    while(location< 75) {
+        location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
+        
+        pid1.setInputLimits(0, set);
+        pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
+        pid1.setSetPoint(set);
+        if(side){   //Side = 1 for Left
+            rangeFinderLeft.startMeas();
+            wait_ms(20);
+            rangeFinderLeft.getMeas(range);
+        }
+        else{       //Use right Ultrasonic
+            rangeFinderRight.startMeas();
+            wait_ms(20);
+            rangeFinderRight.getMeas(range);
+            pc.printf("%d\r\n",range);
+        }
+        
+        if(range > 20) {
+            wavegap=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
+            bt.printf("wavegap %f\r\n",wavegap);
+            // AT WAVE OPENING!!!!
+            motors.setMotor0Speed(dir*MAX_SPEED);//right
+            motors.setMotor1Speed(dir*MAX_SPEED);//left
+            
+        } else {
         
-        pid1.setInputLimits(5.75, 6);
+        pid1.setProcessValue(range);
+        pid_return = pid1.compute();
+ 
+        if(pid_return > 0) {
+            if(side){
+                motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right
+                motors.setMotor1Speed(dir*MAX_SPEED);//left
+            }
+            else{
+                motors.setMotor1Speed(dir*MAX_SPEED - dir*pid_return);//left
+                motors.setMotor0Speed(dir*MAX_SPEED);//right
+            }
+        }else if(pid_return < 0) {
+            if(side){
+                motors.setMotor0Speed(dir*MAX_SPEED);//right
+                motors.setMotor1Speed(dir*MAX_SPEED + dir*pid_return);//left
+            }
+            else{
+                motors.setMotor1Speed(dir*MAX_SPEED);//left
+                motors.setMotor0Speed(dir*MAX_SPEED + dir*pid_return);//right
+            }
+        }else {
+            motors.setMotor0Speed(dir*MAX_SPEED);//right
+            motors.setMotor1Speed(dir*MAX_SPEED);//left
+        }
+    }}
+    return wavegap;
+}
+ 
+/* MODIFIED WALL_FOLLOW FOR NAVIGATION */
+ 
+void wall_follow2(int side, int direction, int section)
+{
+    int SeeWaveGap = false, count=0, dir=1, set=5;
+    
+    pid1.reset();
+    
+    if(direction == BACKWARD) dir=-1;
+    if(section == TOOLS)set= 5;
+    
+    leftEncoder.reset();
+    rightEncoder.reset();
+ 
+    while(1) {
+ 
+        pid1.setInputLimits(0.0, set);
         pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
-        pid1.setSetPoint(6.0);
-        
-        rangeFinder.startMeas();
-        wait_ms(20);
-        if ( (rangeFinder.getMeas(range) == RANGE_MEAS_VALID) && (range < 100.0) && (range > 3.0))
-        {
-            pc.printf("Range = %f\n\r", range);
-        } 
+        pid1.setSetPoint(set);
+ 
+        if(side){
+            rangeFinderLeft.startMeas();
+            wait_ms(20);
+            rangeFinderLeft.getMeas(range);
+        }
+        else{
+            rangeFinderRight.startMeas();
+            wait_ms(20);
+            rangeFinderRight.getMeas(range);
+        }
+ 
+ 
+        /*************CHECK FOR WAVE OPENING*****************/
+        /* If after 20 ms the ultrasonic still sees 20+ cm */
+        /*      then robot is at wave opening               */
+ 
+        pc.printf("range %f\r\n",range);
+        if(range > 20) {
+            motors.stopBothMotors();
+            bt.printf("wavegap\r\n");
+            // AT WAVE OPENING!!!!
+            break;
+        }
+ 
         pid1.setProcessValue(range);
-        pid_return = pid1.compute(); 
-        pc.printf("Range: %f\n      PID:   %f\r\n", range, pid_return);
-        
-        if(pid_return > 0){
-            motors.setMotor0Speed(MAX_SPEED - pid_return);//left
+        pid_return = pid1.compute();
+        //bt.printf("Range: %f\n      PID:   %f\r\n", range, pid_return);
+ 
+               if(pid_return > 0) {
+            if(side){
+                motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right
+                motors.setMotor1Speed(dir*MAX_SPEED);//left
+            }
+            else{
+                motors.setMotor1Speed(dir*MAX_SPEED - dir*pid_return);//left
+                motors.setMotor0Speed(dir*MAX_SPEED);//right
+            }
+        }else if(pid_return < 0) {
+            if(side){
+                motors.setMotor0Speed(dir*MAX_SPEED);//right
+                motors.setMotor1Speed(dir*MAX_SPEED + dir*pid_return);//left
+            }
+            else{
+                motors.setMotor1Speed(dir*MAX_SPEED);//left
+                motors.setMotor0Speed(dir*MAX_SPEED + dir*pid_return);//right
+            }
+        } else {
+            motors.setMotor0Speed(dir*MAX_SPEED);
+            motors.setMotor1Speed(dir*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(0, 5);
+        pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
+        pid1.setSetPoint(5.0);
+ 
+        rangeFinderLeft.startMeas();
+        wait_ms(100);
+        if ( (rangeFinderLeft.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){
+        } else if(pid_return < 0) {
             motors.setMotor0Speed(MAX_SPEED);
             motors.setMotor1Speed(MAX_SPEED + pid_return);
-        }else{
+        } else {
             motors.setMotor0Speed(MAX_SPEED);
             motors.setMotor1Speed(MAX_SPEED);
         }
-     }
+    }
 }
-
-/* MODIFIED WALL_FOLLOW FOR NAVIGATION */
-
-void wall_follow2(int *currentLocation)
-{
-    int SeeWaveGap = false;
-    int count=0;
-    
-    while(1){
-        
-        pid1.setInputLimits(0.0, 6.0);
-        pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
-        pid1.setSetPoint(6.0);
-        
-        rangeFinder.startMeas();
-        wait_ms(20);
-        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 60 ms the ultrasonic still sees 20+ cm */
-        /*      then robot is at wave opening               */
-        
-        pc.printf("range %f\r\n",range);
-        if(range > 20){
-            currentLocation++;
-            bt.printf("saw gap \r\n");
-            
-            if(SeeWaveGap){
-                motors.stopBothMotors();
-                bt.printf("wavegap\r\n");
-                // AT WAVE OPENING!!!!
-            
-                break;
-            }
-            SeeWaveGap = true;
-        }
-           
-        
-            
-        pid1.setProcessValue(range);
-        pid_return = pid1.compute(); 
-        bt.printf("Range: %f\n      PID:   %f\r\n", 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(5.75, 6);
-        pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
-        pid1.setSetPoint(6.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);
-        }
-     }
-}
-
+ 
 void rightTurn(void)
 {
+    float speedL, speedR;
+ 
+    speedL=speedR= 0.4;
+ 
     leftEncoder.reset();
-    //rightEncoder.reset();
-    motors.setMotor0Speed(-0.4*127);
-    motors.setMotor1Speed(0.4*127);
-    while(leftEncoder.getPulses()<1400);
+    rightEncoder.reset();
+    motors.setMotor0Speed(-speedR*127);//right
+    motors.setMotor1Speed(speedL*127);//left
+    while(leftEncoder.getPulses()<1050 || rightEncoder.getPulses()>-1050);
     motors.stopBothMotors();
 }
-
+ 
 void leftTurn(void)
 {
     leftEncoder.reset();
-    //rightEncoder.reset();
-    motors.setMotor0Speed(0.4*127);
-    motors.setMotor1Speed(-0.4*127);
-    while(leftEncoder.getPulses()>-1500);
+    rightEncoder.reset();
+    motors.setMotor0Speed(0.4*127);// right
+    motors.setMotor1Speed(-0.4*127);// left
+    while(leftEncoder.getPulses()>-1200 || rightEncoder.getPulses()<1200);
+    motors.stopBothMotors();
+}
+ 
+void overBump(void){
+    
+    leftEncoder.reset();
+    rightEncoder.reset();
+    motors.setMotor0Speed(MAX_SPEED); //right
+    motors.setMotor1Speed(MAX_SPEED); //left
+    while(abs(leftEncoder.getPulses()/PPR) + abs(rightEncoder.getPulses()/PPR)/2 < 1);
+    /*
+    leftEncoder.reset();
+    rightEncoder.reset();
+    motors.setMotor0Speed(MAX_SPEED); //right
+    motors.setMotor1Speed(MAX_SPEED); //left
+    while(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR)/2 <3);
+    */
     motors.stopBothMotors();
 }
+ 
+void tools_section(float *location, float &current){
+    
+    location[0]=wall_follow(LEFT,FORWARD, TOOLS); //location from the left edge of the field
+    current=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
+    bt.printf("left Encoder : %d \t right Encoder %d \r\n",leftEncoder.getPulses(),rightEncoder.getPulses());
+    
+    bt.printf("wavegap %f \t current %f \r\n",location[0],current);
+    
+     motors.stopBothMotors();
+    ////////////////////////////////////////// determine shape and pick up tool ///////////////////////////////////////////////////////
+    
+    if(current >location[0]){
+        wall_follow2(LEFT,BACKWARD,TOOLS);
+        wait_ms(100);
+        // forward 
+        
+ /*       leftEncoder.reset();
+        rightEncoder.reset();
+        motors.setMotor0Speed(-MAX_SPEED); //right
+        motors.setMotor1Speed(-MAX_SPEED); //left
+        while(abs(leftEncoder.getPulses())<300 || abs(rightEncoder.getPulses())<300);
+        motors.stopBothMotors();
+        wait_ms(500);*/
+        current = (abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2; 
+        // REMOVED += because current should just be current pulses?
+    }
+    else{/*
+        wall_follow2(LEFT,FORWARD);   
+        // backward 
+        leftEncoder.reset();
+        rightEncoder.reset();
+        motors.setMotor0Speed(MAX_SPEED); //right
+        motors.setMotor1Speed(MAX_SPEED); //left
+        while(abs(leftEncoder.getPulses())<300 || abs(rightEncoder.getPulses())<300);
+        motors.stopBothMotors();
+        current-=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;*/
+    }
+    
+    leftTurn();
+    
+    //Go over 
+    overBump();
+    
+}
+ 
\ No newline at end of file