Diff: main.cpp

Revision:

9:e144fd53f606

Parent:

8:fd00916552e0

Child:

10:6b12d31b0fbf

--- a/main.cpp	Tue Oct 23 13:15:22 2018 +0000
+++ b/main.cpp	Thu Oct 25 14:53:29 2018 +0000
@@ -15,7 +15,7 @@
 DigitalIn KillSwitch(SW2);
 DigitalIn button(SW3);
 MODSERIAL pc(USBTX, USBRX);
-HIDScope scope(2);
+//HIDScope scope(2);
 
 //values of inverse kinematics
 volatile bool emg0Bool = false;
@@ -84,6 +84,10 @@
         y_position2 = old_y2 + (0.1f * direction);
         motor2_angle = ( y_position / C2) + acos( x_position / length );     //sawtooth-gear motor angle in rad
     }
+    else {
+        pwmpin1 = 0;
+        pwmpin2 = 0;
+    }
     
     //reset the booleans, only for demo purposes
     emg0Bool = false;
@@ -110,10 +114,13 @@
         //reset the boolean, only for demo purposes
         emg2Bool = false; 
     }
+    else {
+        pwmpin1 = false;
+    }
 }
 
 void PIDController1() {   
-    P1 = motor1.getPosition() / 8400 * 360;        
+    P1 = motor1.getPosition() / 8400 * 360; // <-- needs to be in radians, or motor1_angle in degrees!!!        
     e1 = e2;
     e2 = e3;
     e3 = motor1_angle - P1;
@@ -128,14 +135,14 @@
         Y1 = -1;
     }    
 
-    scope.set(0,Output1);
+/*    scope.set(0,Output1);
     scope.set(1,P1);
     scope.send();
-    pc.printf("motor1 encoder: %f\r\n", P1);
+*/    pc.printf("motor1 encoder: %f\r\n", P1);
 }
 
 void PIDController2() {   
-    P2 = motor2.getPosition() / 8400 * 360;        
+    P2 = motor2.getPosition() / 8400 * 360; // <-- needs to be in radians, or motor2_angle in degrees!!!        
     f1 = f2;
     f2 = f3;
     f3 = motor2_angle - P2;
@@ -150,28 +157,26 @@
         Y2 = -1;
     }    
 
-    scope.set(0,Output2);
+/*    scope.set(0,Output2);
     scope.set(1,P2);
     scope.send();
-    pc.printf("motor2 encoder: %f\r\n", P2);
+*/    pc.printf("motor2 encoder: %f\r\n", P2);
 }
 
 void ControlMotor1() {
-    if (Y1 >= 0) {
+    if (Y1 > 0) {
         Y1 = 0.6f * Y1 + 0.4f;
         directionpin1 = true;
     } 
     else if(Y1 < 0){
         Y1 = 0.6f - 0.4f * Y1;
         directionpin1 = false;
-    }
-    
-    pwm1 = abs(Y1);
-    pwmpin1 = pwm1;
+    }    
+    pwmpin1 = abs(Y1);
 }
 
 void ControlMotor2() {
-    if (Y1 >= 0) {
+    if (Y1 > 0) {
         Y1 = 0.5f * Y1 + 0.5f;
         directionpin2 = true;
     } 
@@ -179,8 +184,7 @@
         Y1 = 0.5f - 0.5f * Y1;
         directionpin2 = false;
     }
-    pwm2 = abs(Y2);
-    pwmpin2 = pwm2;
+    pwmpin2 = abs(Y2);
 }
 
 int main() {
@@ -212,13 +216,15 @@
             }
         }
         yDirection(); //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
+        pc.printf("xDirection: motor1angle=%f, motor2angle=%f", motor1_angle);
         PIDController1();
         PIDController2();
         ControlMotor1();
         ControlMotor2();
         
-        if (!KillSwitch) {
+        if (!KillSwitch) { //even in mekaar gebeund voor het hebben van een stop knop
             ledred = false;
             pwmpin1 = 0;
             pwmpin2 = 0;
@@ -240,6 +246,8 @@
         pc.printf("motor1 angle: %f\n\r", motor1_angle);
         pc.printf("motor2 angle: %f\n\r\n", motor2_angle);
 
-        wait(0.5f); //can also be done with ticker, to be sure that it happens exactly every 0.5 seconds
+        wait(0.25f); //can also be done with ticker, to be sure that it happens exactly every 0.5 seconds
+        pwmpin1 = 0;
+        pwmpin2 = 0;
     }
 }
\ No newline at end of file