Diff: main.cpp

Revision:

5:14a68d0ee71a

Parent:

4:f36406c9e42f

Child:

6:0162a633768d

--- a/main.cpp	Fri Oct 19 12:45:17 2018 +0000
+++ b/main.cpp	Sun Oct 21 12:20:59 2018 +0000
@@ -19,14 +19,14 @@
 volatile float y_position = 0.0;
 volatile float old_y;
 volatile float old_x;
-volatile int motor1_angle = 0;
-volatile int motor2_angle = 0;
-volatile int direction;
+volatile float motor1_angle = 0.0;
+volatile float motor2_angle = 0.0;
+volatile float direction;
 volatile char c;
 
 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
+const float C1 = 3; //motor 1 gear ratio (placeholder)
+const float C2 = 3; //motor 2 gear ratio
       
 void xDirection() {
     //direction of the motion
@@ -45,11 +45,11 @@
         //calculating the motion
         old_x = x_position;
         x_position = old_x + direction;
-        motor1_angle = asin( x_position ) / length;     //theta = arcsin(x) / L
+        motor1_angle = asin( x_position / length);     //x = L*sin(theta) -> theta = arcsin(x/L)
         
         old_y = y_position;
         y_position = old_y + direction;
-        motor2_angle = acos( x_position ) / length;     //theta = arcsin(x) / L
+        motor2_angle = acos( x_position / length );     //theta = arccos(x/L)
     }
     
     //reset the booleans
@@ -109,10 +109,11 @@
         xDirection(); //call the function to move in the x direction
         yDirection(); //call the function to move in the y direction
         
-        // print the motor angles
-        pc.printf("motor1 angle: %i\n\r", motor1_angle);
-        pc.printf("motor2 angle: %i\n\r\n", motor2_angle);
+        // print the motor angles and coordinates
+        pc.printf("position: (%f, %f)\n\r", x_position, y_position);
+        pc.printf("motor1 angle: %f\n\r", motor1_angle);
+        pc.printf("motor2 angle: %f\n\r\n", motor2_angle);
 
-        wait(0.25f); //can also be done with ticker, to be sure that it happens exactly every 0.5 seconds
+        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