Diff: main.cpp

Revision:

23:fdde3e4b9e69

Parent:

22:ad85b8acf8b5

Child:

24:bdd74b91abbb

--- a/main.cpp	Mon Oct 24 14:45:10 2016 +0000
+++ b/main.cpp	Tue Oct 25 10:47:41 2016 +0000
@@ -1,12 +1,52 @@
 #include "mbed.h"
 #include "HIDScope.h"
 #include "MODSERIAL.h"
+#include "QEI.h"
+
+DigitalOut led_g(LED_GREEN);
+DigitalOut led_b(LED_BLUE);
+DigitalOut led_r(LED_RED);
+
+DigitalOut M1_Rotate(D2); // voltage only base rotation
+PwmOut M1_Speed(D3);      // voltage only base rotation
+
+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
+PwmOut M3_Speed(D6);        // encoder side pot 1 spatel rotation
+
+DigitalIn links(SW3);
+DigitalIn rechts(SW2);
+
+AnalogIn pot1(A4); // pot 1 motor 1
+AnalogIn pot2(A3); // pot 2 motor 3
 
 //Define objects
 AnalogIn    emg0( A0 );
 AnalogIn    emg1( A1 );
-DigitalIn   buttonCalibrate(SW2);
-MODSERIAL pc(USBTX, USBRX);
+DigitalIn   buttonCalibrate(D9);
+
+bool draairechts;
+bool draailinks;
+bool turn = 0;
+float waiter = 0.1;
+float afstand = 200;
+float translation = 0;
+float degrees3 = 0;
+
+float Puls_degree = (8400/360);
+float wheel1 = 16;
+float wheel2 = 31;
+float wheel3 = 41;
+float overbrenging = ((wheel2/wheel1)*(wheel3/wheel1));
+float pi = 3.14159265359;
 
 volatile float x;
 volatile float x_prev =0;
@@ -16,6 +56,7 @@
 double threshold_Left = 0;
 double threshold_Right= 0;
 Ticker      sample_timer;
+Ticker      sample_timer2;
 HIDScope    scope( 2 );
 DigitalOut  led(LED1);
 const double a1 = -1.6475;
@@ -94,14 +135,117 @@
     led = !led;
     pc.printf("%f, %f \n \r ", x, b);
 }
+void GetDirections()
+{
+    pc.baud(115200);
+    if ((rechts == 0) && (links == 0) && (turn == 0)) {
+        draailinks = 0;
+        draairechts = 0;
+        turn = 1;
+        pc.printf("begin de actie \n \r ");
+        wait(waiter);
 
+    } else if ((rechts == 0) && (links == 0) && (turn == 1)) {
+        draailinks = 0;
+        draairechts = 0;
+        turn = 0;
+        pc.printf("breek de actie af \n \r ");
+        wait(waiter);
+    } else if ((rechts == 1) && (links == 1)&& (turn == 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)&& (turn == 0)) {
+        draailinks = 0;
+        draairechts = !draairechts;
+        pc.printf("draai naar rechts na links \n \r ");
+        wait(waiter);
+    } 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) && (turn == 0)) {
+        draairechts = 0;
+        draailinks = !draailinks;
+        pc.printf("draai naar links na rechts \n \r ");
+        wait(waiter);
+    }
+    wait(2*waiter);
+}
+
+float GetPositionM2()
+{
+    float pulses2 = motor2.getPulses();
+    float degrees2 = (pulses2/Puls_degree);
+    float radians2 = (degrees2/360)*2*pi;
+    float translation = ((radians2/overbrenging)*32.25);
+
+    return translation;
+}
+float GetRotationM3()
+{
+    float pulses3 = motor3.getPulses();
+    float degrees3 = (pulses3/Puls_degree);
+    float radians3 = (degrees3/360)*2*pi;
+
+    return degrees3;
+}
+void GoBack()
+{
+    while (GetPositionM2() > 0) {
+        M3_Speed = 0;
+        M2_Speed = 1;
+        M2_Rotate = 0;
+        pc.printf("rotation %f translation %f \n \r ", GetRotationM3(), GetPositionM2());
+        led_r = 0;
+    }
+    M2_Speed = 0;
+
+
+    while (GetRotationM3() > 0) {
+        M3_Rotate = 1;
+        M3_Speed = 0.2;
+        led_r = 1;
+        led_b = 0;
+        pc.printf("rotation %f translation %f \n \r ", GetRotationM3(), GetPositionM2());
+
+    }
+    M3_Speed = 0;
+
+    turn = 0;
+
+
+}
+void Burgerflip()
+{
+    if (GetPositionM2() > afstand) {
+        M3_Speed = 0.2;
+        M3_Rotate = 0;
+        M2_Speed = 0;
+    } else if (GetPositionM2() < afstand) {
+        M2_Speed = 1;
+        M2_Rotate = 1;
+
+    }
+    if (GetRotationM3() > 150) {
+        GoBack();
+    }
+}
 int main()
 {
+    led_g = 1;
+    led_b = 1;
+    led_r = 1;
+
     /**Attach the 'sample' function to the timer 'sample_timer'.
     * this ensures that 'sample' is executed every... 0.002 seconds = 500 Hz
     */
     //sample_timer.attach(&sample, 0.001953125);
-    sample_timer.attach(&filterSampleLeft, 0.001953125);        //512 Hz
+    sample_timer2.attach(&filterSampleLeft, 0.001953125);        //512 Hz
     sample_timer.attach(&filterSampleRight, 0.001953125);
     pc.baud(115200);
     pc.printf("please push the button to calibrate \n \r");
@@ -110,9 +254,32 @@
             calibrate = true;
             threshold_Left = lowpassFilterLeft*0.7;
             threshold_Right = lowpassFilterRight*0.7;
+            
         }
         if (calibrate == true) {
             pc.printf("calibration complete, left = %f, right = %f \n \r", threshold_Left, threshold_Right);
+            GetDirections();
+            if (draairechts == true) {
+                M1_Speed = 0.2;
+                M1_Rotate = 0;
+            } else if (draailinks == true) {
+                M1_Speed = 0.2;
+                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;
+            }
+            pc.printf("rotation %f translation %f \n \r ", GetRotationM3(), GetPositionM2());
+            /* pulses = 8400 */
             /*empty loop, sample() is executed periodically*/
         }
     }