Diff: main.cpp

Revision:

6:0162a633768d

Parent:

5:14a68d0ee71a

Child:

7:9e0ded88fe60

--- a/main.cpp	Sun Oct 21 12:20:59 2018 +0000
+++ b/main.cpp	Mon Oct 22 09:18:30 2018 +0000
@@ -17,16 +17,18 @@
 
 volatile float x_position = 0.0;
 volatile float y_position = 0.0;
+volatile float y_position2 = 0.0;
 volatile float old_y;
+volatile float old_y2;
 volatile float old_x;
-volatile float motor1_angle = 0.0;
-volatile float motor2_angle = 0.0;
+volatile float motor1_angle = 0.0; //circular gear motor
+volatile float motor2_angle = 0.0; //sawtooth gear motor
 volatile float direction;
 volatile char c;
 
 const float length = 0.300; //length in m (placeholder)
-const float C1 = 3; //motor 1 gear ratio (placeholder)
-const float C2 = 3; //motor 2 gear ratio
+const float C1 = 3; //motor 1 gear ratio
+const float C2 = 3; //motor 2 gear ratio/radius of the circular gear (placeholder)
       
 void xDirection() {
     //direction of the motion
@@ -44,12 +46,12 @@
     if (emg0Bool || emg1Bool){
         //calculating the motion
         old_x = x_position;
-        x_position = old_x + direction;
-        motor1_angle = asin( x_position / length);     //x = L*sin(theta) -> theta = arcsin(x/L)
+        x_position = old_x + (0.1f * direction);
+        motor1_angle = asin( x_position / (length * C1 ));     //rotational motor angle in rad
         
-        old_y = y_position;
-        y_position = old_y + direction;
-        motor2_angle = acos( x_position / length );     //theta = arccos(x/L)
+        old_y2 = y_position2;
+        y_position2 = old_y2 + (0.1f * direction);
+        motor2_angle = ( y_position / C2) + acos( x_position / length );     //sawtooth-gear motor angle in rad
     }
     
     //reset the booleans
@@ -71,8 +73,8 @@
         
         //calculating the motion
         old_y = y_position;
-        y_position = old_y + direction;
-        motor2_angle = C2 * y_position;
+        y_position = old_y + (0.1f * direction);
+        motor2_angle = y_position / C2; // sawtooth-gear motor angle in rad
         
         //reset the boolean
         emg2Bool = false;