Diff: main.cpp

Revision:

6:4e4df2f6157e

Parent:

5:353ed56417a2

Child:

7:c7bdeee1bbac

--- a/main.cpp	Mon Oct 17 12:41:39 2016 +0000
+++ b/main.cpp	Fri Oct 21 08:56:33 2016 +0000
@@ -1,13 +1,21 @@
 #include "mbed.h"
 #include "MODSERIAL.h"
+#include "QEI.h"
 
-DigitalOut M1_rotate(D2); // voltage only base rotation
+
+DigitalOut M1_Rotate(D2); // voltage only base rotation
 PwmOut M1_Speed(D3);      // voltage only base rotation
 
-DigitalOut M2_rotate(D4);   // encoder side pot 2 translation
+MODSERIAL pc(USBTX, USBRX);
+
+//QEI wheel(PinName channelA, PinName channelB, PinName index, int pulsesPerRev, Encoding encoding=X2_ENCODING)
+QEI motor2(D10,D11,NC,8400,QEI::X4_ENCODING);
+QEI motor3(D12,D13,NC,8400,QEI::X4_ENCODING);
+
+DigitalOut M2_Rotate(D4);   // encoder side pot 2 translation
 PwmOut M2_Speed(D5);        // encoder side pot 2 translation
 
-DigitalOut M3_rotate(D7);   // encoder side pot 1 spatel rotation
+DigitalOut M3_Rotate(D7);   // encoder side pot 1 spatel rotation
 PwmOut M3_Speed(D6);        // encoder side pot 1 spatel rotation
 
 DigitalIn links(SW3);
@@ -18,45 +26,46 @@
 AnalogIn pot1(A4); // pot 1 motor 1
 AnalogIn pot2(A3); // pot 2 motor 3
 
-MODSERIAL pc(USBTX, USBRX);
 bool draairechts;
 bool draailinks;
-bool turnthedamthing = 0;
+bool turn = 0;
 float waiter = 0.1;
+float afstand = 150;
+float translation = 0;
 void GetDirections()
 {
     pc.baud(115200);
-    if ((rechts == 0) && (links == 0) && (turnthedamthing == 0)) {          
+    if ((rechts == 0) && (links == 0) && (turn == 0)) {
         draailinks = 0;
         draairechts = 0;
-        turnthedamthing = 1;
+        turn = 1;
         pc.printf("begin de actie \n \r ");
         wait(waiter);
 
-    } else if ((rechts == 0) && (links == 0) && (turnthedamthing == 1)) {
+    } else if ((rechts == 0) && (links == 0) && (turn == 1)) {
         draailinks = 0;
         draairechts = 0;
-        turnthedamthing = 0;
+        turn = 0;
         pc.printf("breek de actie af \n \r ");
         wait(waiter);
-    } else if ((rechts == 1) && (links == 1)) {
+    } else if ((rechts == 1) && (links == 1)&& (turn == 0)) {
 
-    } else if ((rechts == 1) && (draailinks == 0)) {
+    } else if ((rechts == 1) && (draailinks == 0)&& (turn == 0)) {
         /* if the right button is pressed and the motor isn't rotating to the left,
         then start rotating to the right etc*/
         draairechts = !draairechts;
         pc.printf("draai naar rechts \n \r ");
         wait(waiter);
-    } else if ((rechts == 1) && (draailinks == 1)) {
+    } else if ((rechts == 1) && (draailinks == 1)&& (turn == 0)) {
         draailinks = 0;
         draairechts = !draairechts;
         pc.printf("draai naar rechts na links \n \r ");
         wait(waiter);
-    } else if ((links == 1) && (draairechts == 0)) {
+    } else if ((links == 1) && (draairechts == 0)&& (turn == 0)) {
         draailinks = !draailinks;
         pc.printf("draai naar links \n \r ");
         wait(waiter);
-    } else if ((links == 1) && (draairechts == 1)) {
+    } else if ((links == 1) && (draairechts == 1) && (turn == 0)) {
         draairechts = 0;
         draailinks = !draailinks;
         pc.printf("draai naar links na rechts \n \r ");
@@ -64,11 +73,59 @@
     }
     wait(2*waiter);
 }
+
+void GetPosition()
+{
+    float pulses = motor2.getPulses();
+    float Puls_degree = (8400/360);
+    float degrees = (pulses/Puls_degree);
+    float wheel1 = 16;
+    float wheel2 = 31;
+    float wheel3 = 41;
+    float overbrenging = ((wheel2/wheel1)*(wheel3/wheel1));
+    float pi = 3.14159265359;
+    float radians = (degrees/360)*2*pi;
+    float translation = ((radians/overbrenging)*32.25);
+    pc.printf("translation %f , degrees %f, radians %f, pulses %f \n \r ",translation, degrees, radians, pulses);
+
+
+}
+void Burgerflip()
+{
+    M2_Speed = 0.5;
+    M2_Rotate = 1;
+    M3_Speed = 0.5;
+    M3_Rotate = 1;
+
+}
 int main()
 {
-
-
-    /*
+    while (true) {
+        GetDirections();
+        if (draairechts == true) {
+            M1_Speed = 0.5;
+            M1_Rotate = 0;
+        } else if (draailinks == true) {
+            M1_Speed = 0.5;
+            M1_Rotate = 1;
+        } else if (turn == 1) {
+            /*M2_Speed = 0.5;
+            M2_Rotate = 1;
+            M3_Speed = 0.5;
+            M3_Rotate = 1;*/
+            Burgerflip();
+        } else if (turn == 0) {
+            M2_Speed = 0;
+            M3_Speed = 0;
+        }
+        if ((draailinks == false) && (draairechts == false)) {
+            M1_Speed = 0;
+        }
+        GetPosition();
+        /* pulses = 8400 */
+    }
+}
+/*
         float GetReferenceVelocity() {
             // Returns reference velocity in rad/s.
             // Positive value means clockwise rotation.
@@ -135,44 +192,4 @@
                 motor3MagnitudePin = fabs(motorValue);
             }
         }
-        */
-
-    while (true) {
-        GetDirections();
-        if (draairechts == true) {
-            M1_Speed = 0.5;
-            M1_rotate = 0;
-        } else if (draailinks == true) {
-            M1_Speed = 0.5;
-            M1_rotate = 1;
-        } else if ((draailinks == false) && (draairechts == false)) {
-            M1_Speed = 0;
-        }
-
-
-
-        /*
-        if ((draailinks == true) && (draairechts == false)) {
-            M1_Speed = 0.5;
-            M1_rotate = 0;
-        } else if ((draairechts == true) && (draailinks == false)) {
-            M1_Speed = 0.5;
-            M1_rotate = 1;
-        } else {
-            M1_Speed = 0;
-        }
-        */
-
-        /*
-        float M2 = M2_rotate.read();
-        float potje1 = pot1.read();
-        float potje2 = pot2.read();
-        M1_Speed.write(potje1);
-        M3_Speed.write(potje2);
-        wait(0.1);
-        pc.printf("motor 1 %f, motor 2 %f, motor 3 %f \n \r ", potje1, M2, potje2);
-        */
-
-    }
-
-}
+        */
\ No newline at end of file