Diff: main.cpp

Revision:

10:8b0d8d4016a1

Parent:

6:0162a633768d

--- a/main.cpp	Mon Oct 22 09:18:30 2018 +0000
+++ b/main.cpp	Thu Oct 25 14:44:32 2018 +0000
@@ -1,8 +1,9 @@
 #include "mbed.h"
 #include "math.h"
 #include "MODSERIAL.h"
+#define PI 3.141592f //65358979323846  // pi 
 
-DigitalIn button(SW2);
+DigitalIn button(SW3);
 DigitalOut directionpin1(D7);
 DigitalOut directionpin2(D8);
 MODSERIAL pc(USBTX, USBRX);
@@ -12,46 +13,49 @@
 volatile bool emg0Bool = false;
 volatile bool emg1Bool = false;
 volatile bool emg2Bool = false;
-volatile bool y_direction = true;
+volatile bool x_direction = true;
 volatile bool a;
 
-volatile float x_position = 0.0;
+const float C1 = 3.0; //motor 1 gear ratio
+const float C2 = 0.013; //motor 2 gear ratio/radius of the circular gear in m
+const float length = 0.300; //length in m (placeholder)
+
+volatile float x_position = length;
 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; //circular gear motor
-volatile float motor2_angle = 0.0; //sawtooth gear motor
+volatile float old_y_position;
+volatile float old_x_position;
+volatile float old_motor1_angle;
+volatile float old_motor2_angle;
+volatile float motor1_angle = 0.0; //sawtooth gear motor
+volatile float motor2_angle = 0.0; //rotational 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
-const float C2 = 3; //motor 2 gear ratio/radius of the circular gear (placeholder)
       
-void xDirection() {
+void yDirection() {
     //direction of the motion
     if (emg0Bool && !emg1Bool) { //if a is pressed and not d, move to the left
         directionpin1 = true;
         directionpin2 = true;
-        direction = -1;
+        direction = -1.0;
     }
     else if (!emg0Bool && emg1Bool) { //if d is pressed and not a, move to the right
         directionpin1 = false;
         directionpin2 = false;
-        direction = 1;
+        direction = 1.0;
     }
 
     if (emg0Bool || emg1Bool){
         //calculating the motion
-        old_x = x_position;
-        x_position = old_x + (0.1f * direction);
-        motor1_angle = asin( x_position / (length * C1 ));     //rotational motor angle in rad
+        old_y_position = y_position;
+        y_position = old_y_position + (0.1f * direction);
+        old_motor2_angle = motor2_angle;
+        motor2_angle = asin( y_position / length );     //saw tooth motor angle in rad
         
-        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
+        //correction from motor 1 to keep x position the same
+        old_x_position = x_position;
+        x_position = old_x_position + cos( motor2_angle ) - cos( old_motor2_angle );
+        old_motor1_angle = motor1_angle;
+        motor1_angle = old_motor1_angle + ( x_position - old_x_position) / C1;     //rotational-gear motor angle in rad
     }
     
     //reset the booleans
@@ -59,24 +63,31 @@
     emg1Bool = false;
 }
 
-void yDirection () {      
+void xDirection () { 
+    //if the button is pressed, reverse the y direction
+    if (!button) {
+        x_direction = !x_direction;
+        wait(0.5f);
+        }     
+        
     if (emg2Bool) { //if w is pressed, move up/down
         //direction of the motion
-        if (y_direction) { 
+        if (x_direction) { 
             directionpin2 = true;
-            direction = 1;
+            direction = 1.0;
         }
-        else if (!y_direction) {
+        else if (!x_direction) {
             directionpin2 = false;
-            direction = -1;
+            direction = -1.0;
         }
         
         //calculating the motion
-        old_y = y_position;
-        y_position = old_y + (0.1f * direction);
-        motor2_angle = y_position / C2; // sawtooth-gear motor angle in rad
+        old_x_position = x_position;
+        x_position = old_x_position + (0.1f * direction);
+        old_motor1_angle = motor1_angle;
+        motor1_angle = old_motor1_angle + ( x_position - length ) / C2; // sawtooth-gear motor angle in degrees
         
-        //reset the boolean
+        //reset the boolean, for demo purposes
         emg2Bool = false; 
     }
 }
@@ -86,11 +97,6 @@
     pc.printf(" ** program reset **\n\r");
     
     while (true) {
-        //if the button is pressed, reverse the y direction
-        if (!button) {
-            y_direction = !y_direction;
-            wait(0.5);
-        }
         
         //testing the code from keyboard imputs: a-d: left to right, w: forward/backwards
         a = pc.readable();
@@ -108,13 +114,13 @@
                     break;
             }
         }
+        yDirection(); //call the function to move in the y direction
         xDirection(); //call the function to move in the x direction
-        yDirection(); //call the function to move in the y direction
         
         // 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);
+        pc.printf("motor1 angle: %f degrees\n\r", motor1_angle *180.0f /PI);
+        pc.printf("motor2 angle: %f degrees\n\r\n", motor2_angle *180.0f /PI);
 
         wait(0.5f); //can also be done with ticker, to be sure that it happens exactly every 0.5 seconds
     }