Diff: main.cpp

Revision:

12:0b40dc152fe2

Parent:

11:aaab65e884be

Child:

13:3f239fa868f2

--- a/main.cpp	Tue Mar 19 10:39:54 2019 +0000
+++ b/main.cpp	Tue Mar 19 14:46:27 2019 +0000
@@ -41,7 +41,7 @@
 
 DigitalOut solenoid(PTC3); //For the gate of the solenoid control MOSFET
 
-//For black line detection
+//For black line detection. The six sensors create a line across the track
 AnalogIn QTR3A_1(PTB3);
 AnalogIn QTR3A_2(PTC2);
 AnalogIn QTR3A_3(PTC1);
@@ -55,7 +55,7 @@
 
 
 //A lookup table of which direction to turn the robot based on the values of all 6 sensor readings
-//e.g. 0 = 000000 meaning no sensors detect a line and directionLookup[0] = STOP
+//e.g. 3 = 000011 meaning no sensors detect a line and directionLookup[3] = RIGHT
 //e.g. 12 = 001100 meaning the middle two sensors detect a line directionLookup[12] = FORWARD
 
 int directionLookup[64] = {LOST, RIGHTHARD, RIGHT, RIGHT, FORWARD, RIGHT, RIGHT, RIGHT, FORWARD, FORWARD, //0-9
@@ -64,19 +64,19 @@
                            FORWARD, FORWARD, LEFTHARD, CHOOSEPATH, CHOOSEPATH, CHOOSEPATH, CHOOSEPATH, CHOOSEPATH, RIGHT, CHOOSEPATH, //30-39
                            LEFT, CHOOSEPATH, CHOOSEPATH, CHOOSEPATH, FORWARD, CHOOSEPATH, FORWARD, RIGHT, LEFT, CHOOSEPATH, //40-49
                            LEFT, CHOOSEPATH, CHOOSEPATH, LEFT, CHOOSEPATH, CHOOSEPATH, LEFT, CHOOSEPATH, LEFT, CHOOSEPATH, //50-59
-                           FORWARD, FORWARD, FORWARD, FORWARD
-                          }; //60-63
+                           FORWARD, FORWARD, FORWARD, FORWARD //60-63
+                          }; 
 
 
 
-const int sensor_addr = 41 << 1; //this should just be equal to 82, haven't touched it though
+const int sensor_addr = 41 << 1; //this should just be equal to 82, haven't touched it though. It's for the Colour Sensor
 
 class MotorController
 {
 public:
     int state;
     int speed;
-    int lastTurn[4];
+    int lastTurn;
 
 
     void initialize();
@@ -88,10 +88,6 @@
     void turnRight();
     void turnLeftHard();
     void turnRightHard();
-    /*
-    void turnLeftHard();
-    void turnRightHard();
-    */
     void changeDirection(int);
     
 
@@ -155,10 +151,6 @@
 
     setLeftMotorSpeed(speed);
     setRightMotorSpeed(speed);
-
-    //wait(0.2);
-    //stopMotors();
-
 }
 
 void MotorController::goBackward()
@@ -170,18 +162,12 @@
 
     setLeftMotorSpeed(speed);
     setRightMotorSpeed(speed);
-
-    //wait(0.2);
-    //stopMotors();
 }
 
 void MotorController::turnLeft()
 {
     state = LEFT;
-    lastTurn[3] = lastTurn[2];
-    lastTurn[2] = lastTurn[1];
-    lastTurn[1] = lastTurn[0];
-    lastTurn[0] = LEFT;
+    lastTurn = LEFT;
 
     setLeftMotorMode(BACKWARD);
     setRightMotorMode(FORWARD);
@@ -194,8 +180,9 @@
     //}
     
     //else {
-    //wait(0.1);
-    //stopMotors();
+    wait(0.1);
+    speed = PWM_PERIOD_US*0.4;
+    goForward();
     //}
     
 
@@ -203,10 +190,7 @@
 void MotorController::turnLeftHard()
 {
     state = LEFT;
-    lastTurn[3] = lastTurn[2];
-    lastTurn[2] = lastTurn[1];
-    lastTurn[1] = lastTurn[0];
-    lastTurn[0] = LEFT;
+    lastTurn = LEFT;
 
     setLeftMotorMode(BACKWARD);
     setRightMotorMode(FORWARD);
@@ -214,7 +198,9 @@
     setLeftMotorSpeed(speed);
     setRightMotorSpeed(speed);
     
-    //wait(0.25);
+    wait(0.25);
+    speed = PWM_PERIOD_US*0.4;
+    goForward();
     //stopMotors();
     
 
@@ -223,33 +209,25 @@
 void MotorController::turnRight()
 {
     state = RIGHT;
-    lastTurn[3] = lastTurn[2];
-    lastTurn[2] = lastTurn[1];
-    lastTurn[1] = lastTurn[0];
-    lastTurn[0] = RIGHT;
+    lastTurn = RIGHT;
     
     setLeftMotorMode(FORWARD);
     setRightMotorMode(BACKWARD);
 
     setLeftMotorSpeed(speed);
     setRightMotorSpeed(speed);
-   // if(lastTurn[3] ==RIGHT and lastTurn[2] ==RIGHT and lastTurn[1] == RIGHT and lastTurn[0] == RIGHT) {     
-     //   turnRightHard();
-       // }
-    
-   // else {
-    //wait(0.1);
-    //stopMotors();
-   // }
+
+    //Turn for a short amount of time and then go straight
+    wait(0.1); 
+    speed = PWM_PERIOD_US*0.4;
+    goForward();
+
 }
 
 void MotorController::turnRightHard()
 {
     state = RIGHT;
-    lastTurn[3] = lastTurn[2];
-    lastTurn[2] = lastTurn[1];
-    lastTurn[1] = lastTurn[0];
-    lastTurn[0] = RIGHT;
+    lastTurn = RIGHT;
     
     
     setLeftMotorMode(FORWARD);
@@ -259,15 +237,15 @@
     setRightMotorSpeed(speed);
     
     
-    //wait(0.25);
-    //stopMotors();
-    
+    wait(0.25);
+    speed = PWM_PERIOD_US*0.4;
+    goForward();
 }
 
 void MotorController::changeDirection(int direction)
 {
     if(direction == LOST) {
-        direction = lastTurn[0];
+        direction = lastTurn;
     }
     switch(direction) {
 
@@ -277,32 +255,32 @@
 
         case FORWARD:
             goForward();
-            setSpeed(PWM_PERIOD_US * 0.45);
+            setSpeed(PWM_PERIOD_US * 0.40);
             break;
 
         case BACKWARD:
             goBackward();
-            setSpeed(PWM_PERIOD_US * 0.45);
+            setSpeed(PWM_PERIOD_US * 0.40);
             break;
 
         case LEFT:
             turnLeft();
-            setSpeed(PWM_PERIOD_US * 0.7);
+            setSpeed(PWM_PERIOD_US * 0.70);
             break;
 
         case RIGHT:
             turnRight();
-            setSpeed(PWM_PERIOD_US *0.7); 
+            setSpeed(PWM_PERIOD_US *0.70); 
             break;
             
         case LEFTHARD:
             turnLeftHard();
-            setSpeed(PWM_PERIOD_US*0.85);
+            setSpeed(PWM_PERIOD_US*0.70);
             break;
             
         case RIGHTHARD:
             turnRightHard();
-            setSpeed(PWM_PERIOD_US*0.85);
+            setSpeed(PWM_PERIOD_US*0.70);
             break;
     }
 }
@@ -352,7 +330,7 @@
         on();
         motorController->goForward();
         motorController->setSpeed(PWM_PERIOD_US * 0.45);
-        wait(2);
+        wait(1);
     }
 
     //Detect the first sheet of paper if red and pick up the disc
@@ -362,7 +340,7 @@
         on();
         motorController->goForward();
         motorController->setSpeed(PWM_PERIOD_US * 0.45);
-        wait(2);
+        wait(1);
     }
 
     //Detect the end of the first sheet of paper