Diff: main.cpp

Revision:

1:fc216448bb57

Parent:

0:dc2c63f663f8

Child:

2:ffd0553701d3

--- a/main.cpp	Mon Oct 15 13:28:55 2018 +0000
+++ b/main.cpp	Wed Oct 17 10:09:28 2018 +0000
@@ -1,4 +1,5 @@
 #include "mbed.h"
+#include "math.h"
 #include "MODSERIAL.h"
 
 DigitalIn button(SW2);
@@ -10,40 +11,64 @@
 volatile bool emg1;
 volatile bool emg2;
 volatile bool y_direction;
-volatile int motor1_angle;
-volatile int motor2_angle;
+
+volatile double x_position = 0;
+volatile double y_position = 0;
+volatile double motor1_angle;
+volatile double motor2_angle;
+volatile double direction;
+
+const float length = 0.300; //length in m (placeholder)
+const int C1 = 3; //motor 1 gear ratio (placeholder)
+const int C2 = 3; //motor 2 gear ratio
     
+void xDirection(double &motor1_angle, double &motor2_angle) {
+        //direction of the motion
+        if (emg0 && !emg1) {
+            directionpin1 = true;
+            directionpin2 = true;
+            direction = 1;
+        }
+        else if (!emg0 && emg1) {
+            directionpin1 = false;
+            directionpin2 = false;
+            direction = -1;
+        }
+
+    if (emg0 || emg1){
+        //calculating the motion
+        x_position = x_position + direction;
+        motor1_angle = pow(sin( x_position ), -1) / length;
+        y_position = y_position + direction;
+        motor2_angle = pow(cos( x_position ), -1) / length;
+    }
+}
+
+void yDirection (int &motor2_angle) {      
+    if (emg2) {
+        //calculating the motion
+        y_position++;
+        motor2_angle = C2 * y_position;
+        
+        //direction of the motion
+        if (y_direction) {
+            directionpin2 = true;
+        }
+        else if (!y_direction) {
+            directionpin2 = false;
+        }
+    }
+}
+
 int main() {
     pc.printf(" ** program reset **\n\r");
     while (true) {
-        
-        //x direction
-        if (emg0 || emg1){
-            motor1_angle++;
-            //cos function of motor1_angle
-            motor2_angle++;
-            //sin function of motor2_angle
-            if (emg0 && !emg1) {
-                directionpin1 = true;
-            }
-            else if (!emg0 && emg1) {
-                directionpin1 = false;
-            }
-        }
-              
-        //y direction
-        if (emg2) {
-            motor2_angle++;
-            if (y_direction) {
-                directionpin2 = true;
-            }
-            else if (!y_direction) {
-                directionpin2 = false;
-            }
-        }
+        xDirection(motor1_angle, motor2_angle);
+        yDirection(motor2_angle);
         
         pc.printf("motor1 angle: %i\n\r", motor1_angle);
         pc.printf("motor2 angle: %i\n\r\n", motor2_angle);
-        wait(0.5f);
+        
+        wait(0.5f); //can also be done with ticker, to be sure that it happens exactly every 0.5 seconds
     }
 }
\ No newline at end of file