Diff: main.cpp

Revision:

0:4bb3dfde1219

diff -r 000000000000 -r 4bb3dfde1219 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat Oct 30 18:14:38 2021 +0000
@@ -0,0 +1,171 @@
+#include "mbed.h"
+#include "variables.h"
+#include "SRF05.h"
+
+DigitalOut myled(LED1);
+#include <vector> //Tracker data
+
+
+#define TESTING 1  //for TDD
+
+
+// Forwards and backwards
+void driveWheels(int direction, float pause)
+{
+    
+#if TESTING
+    pc.printf("Driving\n");
+#endif
+    
+    if(direction == forward)
+    {
+        drive   = 1;
+        turnRight     = 0;
+        wait(pause);        
+    }
+    else
+    {
+        drive   = 0;
+        turnRight     = 1;
+        wait(pause);  
+    }
+    drive   = 0;
+    turnRight     = 0;
+    
+#if TESTING
+    wait(0.01);
+#endif
+}
+
+// Steering function
+void turnWheels(int direction)
+{
+#if TESTING
+    pc.printf("Turning\n");
+#endif
+    switch(direction)
+    {
+        case left:
+            turnLeft = 0;
+            turnRight = 1;
+            break;
+        case right:
+            turnLeft = 1;
+            turnRight = 0;
+            break;  
+        case center:
+        defauturnLeft:
+            turnLeft = 0;
+            turnRight = 0;
+            break;
+    }
+}
+
+//Robot location
+std::vector <float> readPos()
+{
+#if TESTING
+    pc.printf("Reading\n");
+#endif
+    std::vector<float> pos;
+    pos.push_back(ir1.read());
+    pos.push_back(ir2.read());
+    pos.push_back(ir3.read());
+    pos.push_back(ir4.read());
+    pos.push_back(ir5.read());
+    return pos;
+}
+
+int wheelDir(std::vector<float> line)
+{
+#if TESTING
+    pc.printf("Locating\n");
+#endif
+    int ret = 100;
+    float low = 1;
+    for (unsigned i=0; i<line.size(); ++i)
+    {
+#if TESTING
+    pc.printf("%.1f", line[i]);
+    pc.printf("\n");
+#endif
+        if(line[i] < low && line[i] < 0.7)
+        {
+            ret = i; 
+            low = line[i];
+        }
+       
+    }
+        
+#if TESTING
+    pc.printf("%u", ret);
+    pc.printf("\n");
+#endif
+    return ret;
+}
+//Collision handling
+float collision(float col){
+    col = srf.read();
+#if TESTING    
+    pc.printf("Distance = %.1f\n", col);
+#endif    
+    return col;
+}
+
+//Main Loop
+int main()
+{
+#if TESTING
+    pc.printf("Set_up_variables\n");
+#endif
+    std::vector <float>line;
+    std::vector <float> pturnRightLine = readPos();
+    int dir;
+    int pos;
+    float col;
+    while(1)
+    {
+#if TESTING
+        pc.printf("Main\n");
+#endif
+        line = readPos();
+        pos = wheelDir(line);
+#if TESTING
+        pc.printf("%u",pos);
+#endif
+        
+        if(pos == 100) 
+        { 
+            driveWheels(backward, fulldrive);
+        }
+        else 
+        {
+            if(pturnRightLine != line)
+            {
+                switch(pos) 
+                {
+                    case 0:
+                    case 1:
+                        dir = left;
+                        break; 
+                    case 2: 
+                        dir = center;
+                        break;
+                    case 3:
+                    case 4:
+                        dir = right;
+                        break;   
+                }
+                pturnRightLine.clear();
+                std::vector<float> pturnRightLine(line);     
+                turnWheels(dir);
+                collision(col);    
+            }
+            driveWheels(forward, fulldrive);
+            //Collision handling
+            if(col <= 200){
+                driveWheels(backward, fulldrive);
+            }      
+        } 
+    }
+}