Diff: main.cpp

Revision:

11:325a545a757e

Parent:

10:6b12d31b0fbf

Child:

12:9a2d3d544426

--- a/main.cpp	Fri Oct 26 08:16:39 2018 +0000
+++ b/main.cpp	Fri Oct 26 08:34:50 2018 +0000
@@ -3,6 +3,7 @@
 #include "MODSERIAL.h"
 #include "HIDScope.h"
 #include "encoder.h"
+#define PI 3.141592f //65358979323846  // pi 
 
 PwmOut pwmpin1(D6);
 PwmOut pwmpin2(D5);
@@ -87,10 +88,6 @@
         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;
-        pwmpin2 = 0;
-    }
     
     //reset the booleans, only for demo purposes
     emg0Bool = false;
@@ -107,11 +104,11 @@
     if (emg2Bool) { //if w is pressed, move up/down
         //direction of the motion
         if (x_direction) { 
-            directionpin2 = true;
+//            directionpin2 = true;
             direction = 1.0;
         }
         else if (!x_direction) {
-            directionpin2 = false;
+//            directionpin2 = false;
             direction = -1.0;
         }
         
@@ -119,18 +116,15 @@
         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
+        motor1_angle = old_motor1_angle + ( x_position - length ) / C2; // sawtooth-gear motor angle in rad
         
         //reset the boolean, for demo purposes
         emg2Bool = false; 
     }
-    else {
-        pwmpin1 = 0;
-    }
 }
 
 void PIDController1() {   
-    P1 = motor1.getPosition() / 8400 * 360; // <-- needs to be in radians, or motor1_angle in degrees!!!        
+    P1 = motor1.getPosition() / 8400 * 2*PI;    //actual motor angle in rad  
     e1 = e2;
     e2 = e3;
     e3 = motor1_angle - P1;
@@ -152,7 +146,7 @@
 }
 
 void PIDController2() {   
-    P2 = motor2.getPosition() / 8400 * 360; // <-- needs to be in radians, or motor2_angle in degrees!!!        
+    P2 = motor2.getPosition() / 8400 * 2*PI; // actual motor angle in rad
     f1 = f2;
     f2 = f3;
     f3 = motor2_angle - P2;
@@ -226,9 +220,7 @@
             }
         }
         xDirection(); //call the function to move in the y direction
-        pc.printf("yDirection: motor2angle=%f)", motor2_angle);
         yDirection(); //call the function to move in the x direction
-        pc.printf("xDirection: motor1angle=%f, motor2angle=%f", motor1_angle);
         PIDController1();
         PIDController2();
         ControlMotor1();