Diff: main.cpp

Revision:

3:905643e72bcd

Parent:

2:f4d6c9ba05d0

Child:

4:48f440b9a787

--- a/main.cpp	Wed Mar 16 04:46:09 2016 +0000
+++ b/main.cpp	Wed Mar 16 06:38:03 2016 +0000
@@ -47,15 +47,15 @@
     else return 99;    
 }
 
-Motor leftMotor(p22, p5, p6, 1); // pwm, fwd, rev, can brake 
-Motor rightMotor(p21, p8, p7, 1); // pwm, fwd, rev, can brake
+Motor leftMotor(p22, p6, p5, 1); // pwm, fwd, rev, can brake 
+Motor rightMotor(p21, p7, p8, 1); // pwm, fwd, rev, can brake
 Counter leftPulses(p9), rightPulses (p10);
 //Tuning parameters calculated from step tests;
 //see http://mbed.org/cookbook/PID for examples.
 //PID leftPid(0.4312, 0.1, 0.0, 0.01);  //Kc, Ti, Td old
-PID leftPid(0.4310, 0.1, 0.0, 0.01);  //Kc, Ti, Td
+PID leftPid(0.4620, 0.1, 0.0, 0.01);  //Kc, Ti, Td
 PID rightPid(0.4620, 0.1, 0.0, 0.01); //Kc, Ti, Td
-DigitalOut led(LED1);
+DigitalOut led(LED1), led2(LED2);
 AnalogIn ain(p15);
 uLCD_4DGL uLCD(p28,p27,p29); // serial tx, serial rx, reset pin;
 
@@ -71,53 +71,85 @@
     leftPid.setInputLimits(0, 3000);
     leftPid.setOutputLimits(0.0, 0.8);
     leftPid.setMode(AUTO_MODE);
-    rightPid.setInputLimits(0, 3200);
+    rightPid.setInputLimits(0, 3000);
     rightPid.setOutputLimits(0.0, 0.8);
     rightPid.setMode(AUTO_MODE);
     Serial pc(USBTX, USBRX);
     
-    int leftPrevPulses  = 0; //The previous reading of how far the left wheel
-    //has travelled.
+    int leftPrevPulses  = 0, leftActPulses=0; //The previous reading of how far the left wheel
     float leftVelocity  = 0.0; //The velocity of the left wheel in pulses per
-    //second.
-    int rightPrevPulses = 0; //The previous reading of how far the right wheel
-    //has travelled.
+    int rightPrevPulses = 0, rightActPulses=0; //The previous reading of how far the right wheel
     float rightVelocity = 0.0; //The velocity of the right wheel in pulses per
-    //second.
     
-    int distance     = 1000; //Number of pulses to travel.
+    int distance     = 0; //Number of pulses to travel.
     led=0;
+    led2=0;
 
     wait(0.5); //Wait a few seconds before we start moving.
     uLCD.cls();
+    uLCD.locate(1,2);
+    uLCD.printf("I go straight!");
 
     //Velocity to mantain in pulses per second.
     leftPid.setSetPoint(1000);
-    rightPid.setSetPoint(900);
-
-    while ((leftPulses.read() < distance) || (rightPulses.read() < distance)) {
-        
+    rightPid.setSetPoint(1000);
 
-        leftVelocity = (leftPulses.read() - leftPrevPulses) / 0.01;
-        leftPrevPulses = leftPulses.read();
-        rightVelocity = (rightPulses.read() - rightPrevPulses) / 0.01;
-        rightPrevPulses = rightPulses.read();
+    while (1) {
+        
+        if (distTransform(ain)>50) {    //going straight line
+            leftActPulses=leftPulses.read();
+            leftVelocity = (leftActPulses - leftPrevPulses) / 0.01;
+            leftPrevPulses = leftActPulses;
+            rightActPulses=rightPulses.read();
+            rightVelocity = (rightActPulses - rightPrevPulses) / 0.01;
+            rightPrevPulses = rightActPulses;
+    
+            leftPid.setProcessValue(fabs(leftVelocity));
+            leftMotor.speed(leftPid.compute());
+            rightPid.setProcessValue(fabs(rightVelocity));
+            rightMotor.speed(rightPid.compute());
 
-        leftPid.setProcessValue(fabs(leftVelocity));
-        leftMotor.speed(leftPid.compute());
-        rightPid.setProcessValue(fabs(rightVelocity));
-        rightMotor.speed(rightPid.compute());
+        } else { //Don't go straight, turn!
+            leftMotor.stop(0.5);
+            rightMotor.stop(0.5);
+            led2=1;
+            uLCD.locate(1,2);
+            uLCD.printf("I'm turning!");
+            wait(0.5);
+            leftPid.setSetPoint(-500);
+            rightPid.setSetPoint(500);
+            
+            leftActPulses=leftPulses.read();
+            rightActPulses=rightPulses.read();
+            distance=leftActPulses+100;
+            while (leftActPulses<distance) { //I'm turning!
+                leftMotor.speed(-0.5);
+                rightMotor.speed(0.5);
+                leftActPulses=leftPulses.read();
+                rightActPulses=rightPulses.read();
+                
+                wait(0.005);
+                
+            }//Turning while end
+            leftMotor.stop(0.5);
+            rightMotor.stop(0.5);          
+            wait(0.5);
+            led2=0;
+            uLCD.cls();
+            uLCD.locate(1,2);
+            uLCD.printf("I go straight!");
+            
+            leftPid.setSetPoint(1000);
+            rightPid.setSetPoint(1000);
         
-        pc.printf("\n%i", distTransform(ain));
+        } //Going straight/turning if end 
         
-        uLCD.locate(1,2);
-        uLCD.printf("%i", distTransform(ain));
-        wait(0.1);
+        //pc.printf("\n%i", distTransform(ain));
+        //uLCD.locate(1,1);
+        //uLCD.printf("Distance: %i cm", distTransform(ain));
+        wait(0.01);
         led=!led;
         
-    }
-
-    leftMotor.stop(0.5);
-    rightMotor.stop(0.5);
+    }   //end of big while loop
 
 }