Diff: main.cpp

Revision:

3:01b5e80d842d

Parent:

1:813f4b17ae65

Child:

4:208f5279143a

--- a/main.cpp	Sun Mar 03 00:55:10 2019 +0000
+++ b/main.cpp	Sat Mar 09 14:27:48 2019 +0000
@@ -2,50 +2,54 @@
 
 #include "math.h"
 #include "mbed.h"
-#include "QEI.h"
+#include "PID.h"
 #include "Encoder.h"
+#include "lineSensor.h"
 #include "P.h"
 #include "Wheel.h"
 #include "Robot.h"
-#include "lineSensor.h"
 
 
 //blue D8
 //Green D9
 //Red D5
-
-    Ticker sampler;
-    DigitalOut enable(PA_13);
-    DigitalOut led(PA_5);
+int i = 0;
+int main() {
+    Serial pc(USBTX, USBRX);
     
-    Encoder* LE = new Encoder(PB_10,PB_4);
+    DigitalOut enable(PA_13);
+    enable.write(1);
+    
     Encoder* RE = new Encoder(PB_3,PB_5);
-    Wheel* leftWheel = new Wheel(LE,PC_8,PA_9, PA_14);
-    Wheel* rightWheel = new Wheel(RE,PC_6,PA_8, PA_7);
-    Robot rbt(leftWheel, rightWheel);
+    Encoder* LE = new Encoder(PB_10,PB_4);
+    Wheel* rightWheel = new Wheel(RE,PC_8,PA_9, PA_14);
+    Wheel* leftWheel = new Wheel(LE,PC_6,PA_8, PA_7);
+    //an array of lineSensor pointers params: lineSensor(PinName emitter Pin, PinName reciever Pin, make sure it is from LEFT TO RIGHT
     
     
-    //an array of lineSensor pointers params: lineSensor(PinName emitter Pin, PinName reciever Pin)
-    lineSensor* sensorArray[6] = {new lineSensor(D11,A0),new lineSensor(D7,A1),new lineSensor(D6,A2),new lineSensor(D5,A3),new lineSensor(D4,A4),new lineSensor(D3,A5)};
-    int i = 0;
+    pc.printf("Stage 1 speed %f\r\n", rightWheel->returnAngularVelocity());
     
-    void sensorReadings(void)
+    rightWheel->init(0);
+    while(rightWheel->returnAngularVelocity() != 0)
     {
-        sensorArray[i]->takeReading();
-        if (sensorArray[i]->returnLineVoltage() >= 1.5f)
-        {
-            led.write(0); 
-        } else { led.write(1);}
-        i++;
-        if (i == 6) {i=0;}     
+     pc.printf("Stage 2, speed %f\r\n",rightWheel->returnAngularVelocity()); 
+    }
+    leftWheel->init(0);
+    while(leftWheel->returnAngularVelocity() != 0)
+    {
+     pc.printf("Stage 2, speed %f\r\n",leftWheel->returnAngularVelocity()); 
     }
 
-int main() {
-    enable.write(1);
-    leftWheel->init(1);
-    rightWheel->init(0);
-    
-    sampler.attach(callback(sensorReadings),0.1);
-    while(1){}
+    while(1)
+    {
+        i++;
+        if (i >= 10)
+        {
+        i = 0;
+        pc.printf("speed %f\r\n", rightWheel->returnAngularVelocity()); 
+        pc.printf("speed %f\r\n", leftWheel->returnAngularVelocity());
+        }
+    }
+}
 
-}
+/* ineSensor* sensorArray[6] = {new lineSensor(D11,A0),new lineSensor(D7,A1),new lineSensor(D6,A2),new lineSensor(D5,A3),new lineSensor(D4,A4),new lineSensor(D3,A5)}; */
\ No newline at end of file