Diff: main.cpp

Revision:

7:1e5fa5952695

Parent:

6:1056ed1d6d97

Child:

8:88e72c6deac9

--- a/main.cpp	Mon Mar 04 10:52:03 2019 +0000
+++ b/main.cpp	Tue Mar 05 13:58:37 2019 +0000
@@ -5,13 +5,17 @@
 #define ON 1
 
 //For motor control
-#define PWM_PERIOD_US 30000 //For setting PWM periods to 1 milliseconds. I made this number up
+#define PWM_PERIOD_US 1000 //For setting PWM periods to 1 milliseconds. I made this number up
 #define STOP 0
 #define FORWARD 1
 #define BACKWARD 2
 //For line following, use the previous defines and the follwoing
 #define LEFT 3
 #define RIGHT 4
+#define LEFTHARD 5
+#define RIGHTHARD 6
+
+int  count = 0;
 
 DigitalOut redled(LED_RED); //Debiug LED
 DigitalOut blueled(LED_BLUE);
@@ -64,6 +68,7 @@
     public:
     int state;
     int speed;
+    int lastTurn;
     
     
     void initialize();
@@ -75,6 +80,8 @@
     void goBackward();
     void turnLeft();
     void turnRight();  
+    void turnLeftHard();
+    void turnRightHard();
     void changeDirection(int direction);
     
     private:
@@ -127,9 +134,13 @@
     
     setLeftMotorMode(FORWARD);
     setRightMotorMode(FORWARD);
+    
 
     setLeftMotorSpeed(speed);
     setRightMotorSpeed(speed);
+    
+    wait(0.2);
+    stopMotors();
 
 }
 
@@ -143,11 +154,14 @@
     setLeftMotorSpeed(speed);
     setRightMotorSpeed(speed);
 
+    wait(0.2);
+    stopMotors();
 }
 
 void MotorController::turnLeft()
 {   
     state = LEFT;
+    lastTurn = LEFT;
     
     setLeftMotorMode(BACKWARD);
     setRightMotorMode(FORWARD);
@@ -155,20 +169,57 @@
     setLeftMotorSpeed(speed);
     setRightMotorSpeed(speed);
 
+    wait(0.05);
+    stopMotors();
+}
+
+void MotorController::turnLeftHard()
+{   
+    state = LEFTHARD;
+    lastTurn = LEFTHARD;
+    setLeftMotorMode(BACKWARD);
+    setRightMotorMode(FORWARD);
+
+    setLeftMotorSpeed(speed);
+    setRightMotorSpeed(speed);
+
+    wait(0.2);
+    stopMotors();
 }
 
 
 void MotorController::turnRight()
 {
     state = RIGHT;
-    
+    lastTurn = RIGHT;
     setLeftMotorMode(FORWARD);
     setRightMotorMode(BACKWARD);
 
     setLeftMotorSpeed(speed);
     setRightMotorSpeed(speed);
+    
+    
+    wait(0.05);
+    stopMotors();
+    
 }
 
+void MotorController::turnRightHard()
+{
+    state = RIGHTHARD;
+    lastTurn= RIGHTHARD;
+    setLeftMotorMode(FORWARD);
+    setRightMotorMode(BACKWARD);
+
+    setLeftMotorSpeed(speed);
+    setRightMotorSpeed(speed);
+    
+    
+    wait(0.2);
+    stopMotors();
+}
+    
+
 void MotorController::changeDirection(int direction) {
     
     switch(direction) {
@@ -179,24 +230,35 @@
          
         case FORWARD:
             goForward();
-            setSpeed(PWM_PERIOD_US * 0.3);
+            setSpeed(PWM_PERIOD_US * 0.5);
             break;   
         
         case BACKWARD:
             goBackward();
-            setSpeed(PWM_PERIOD_US * 0.3);
+            setSpeed(PWM_PERIOD_US * 0.5);
             break;
         
         case LEFT:
             turnLeft();
-            setSpeed(PWM_PERIOD_US * 0.3);
+            setSpeed(PWM_PERIOD_US * 0.7);
             break;
             
         case RIGHT:
             turnRight();
-            setSpeed(PWM_PERIOD_US * 0.3);
+            setSpeed(PWM_PERIOD_US *0.7);
+            break;  
+            
+        case LEFTHARD:    
+            turnLeftHard();
+            setSpeed(PWM_PERIOD_US * 0.8);
+             break;
+            
+        case RIGHTHARD:
+            turnRightHard();
+            setSpeed(PWM_PERIOD_US *0.8);
             break;  
     }
+    
 }
 
 void MotorController::setSpeed(int pulsewidth_us) {
@@ -311,7 +373,7 @@
     
     void initialize();
     void readSensors();
-    int chooseDirection();
+    int chooseDirection(int);
     
     private:
     
@@ -429,12 +491,26 @@
     return direction;
 }*/
 
-int LineFollower::chooseDirection() {
+int LineFollower::chooseDirection(int lastTurn) {
     
         
         //000
         if(!lineDetected1 && !lineDetected2 && !lineDetected3) {
-            direction = STOP;
+
+            if (count>3 and lastTurn==RIGHT) {
+                direction = RIGHTHARD;
+                count = 0;
+                }
+            else if(count>3 and lastTurn==LEFT) {
+                direction = LEFTHARD;
+                count =0;
+                }
+            
+            else {
+                direction = lastTurn;
+                }
+                
+            count++;
         }
         
         //001
@@ -471,7 +547,7 @@
             
         //110    
         else if(lineDetected1 && lineDetected2 && !lineDetected3) {
-            direction = RIGHT;
+            direction = LEFT;
         }
             
         //111
@@ -602,7 +678,7 @@
     
     
     //Start off going straight
-    motorController.setSpeed(PWM_PERIOD_US * 0.2);
+    motorController.setSpeed(PWM_PERIOD_US * 0.4);
     motorController.goForward();
 
 
@@ -617,7 +693,7 @@
     
         
         lineFollower.readSensors();
-        motorController.changeDirection(lineFollower.chooseDirection());
+        motorController.changeDirection(lineFollower.chooseDirection(motorController.lastTurn));
         
         colourSensor.read();