Diff: main.cpp

Revision:

10:6b12d31b0fbf

Parent:

9:e144fd53f606

Child:

11:325a545a757e

--- a/main.cpp	Thu Oct 25 14:53:29 2018 +0000
+++ b/main.cpp	Fri Oct 26 08:16:39 2018 +0000
@@ -24,21 +24,21 @@
 volatile bool y_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)
-
 //values of PID controller
 const float Kp = 2;
 const float Ki = 0.2;
@@ -61,7 +61,7 @@
 float pwm2;
 float P_Last = 0;       // Starting position
       
-void xDirection() {
+void yDirection() {
     //direction of the motion
     if (emg0Bool && !emg1Bool) { //if a is pressed and not d, move to the left
 //        directionpin1 = true;
@@ -76,13 +76,16 @@
 
     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
     }
     else {
         pwmpin1 = 0;
@@ -94,28 +97,35 @@
     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) { 
-//            directionpin2 = true;
-            direction = 1;
+        if (x_direction) { 
+            directionpin2 = true;
+            direction = 1.0;
         }
-        else if (!y_direction) {
-//            directionpin2 = false;
-            direction = -1;
+        else if (!x_direction) {
+            directionpin2 = false;
+            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, only for demo purposes
+        //reset the boolean, for demo purposes
         emg2Bool = false; 
     }
     else {
-        pwmpin1 = false;
+        pwmpin1 = 0;
     }
 }
 
@@ -215,9 +225,9 @@
                     break;
             }
         }
-        yDirection(); //call the function to move in the y direction
+        xDirection(); //call the function to move in the y direction
         pc.printf("yDirection: motor2angle=%f)", motor2_angle);
-        xDirection(); //call the function to move in the x direction
+        yDirection(); //call the function to move in the x direction
         pc.printf("xDirection: motor1angle=%f, motor2angle=%f", motor1_angle);
         PIDController1();
         PIDController2();