Diff: main.cpp

Revision:

1:8eb1c39a91be

Parent:

0:80b7dec1c50e

Child:

2:4720140f7953

--- a/main.cpp	Fri Oct 23 07:56:18 2015 +0000
+++ b/main.cpp	Fri Oct 23 08:55:02 2015 +0000
@@ -1,7 +1,7 @@
 #include "mbed.h"
 #include "math.h"
 #include "MODSERIAL.h"
-#include <string> 
+#include <string>
 
 //Define important constants in memory
 #define     PI              3.14159265
@@ -11,7 +11,7 @@
 #define     ENCODER2_CPR    4200    //gearbox 1:131.25 ->  4200 counts per revolution of the output shaft (X2), 
 #define     PWM_PERIOD      0.0001  //10k Hz pwm motor frequency. Higher -> too hot, lower -> motor doesnt respond correctly
 
-DigitalIn button(PTA4);
+DigitalIn button(SW2);
 MODSERIAL pc(USBTX,USBRX);      //serial communication
 
 
@@ -39,57 +39,61 @@
 
 void take_sample()
 {
-   double      emg_1 = emg_1 + 0.1;
-   double      emg_2 = emg_2 + 0.05; 
+    emg_1 = emg_1 + 0.1;
+    emg_2 = emg_2 + 0.05;
 }
 
-//Current position - Encoder counts -> current angle -> Forward Kinematics 
+//Current position - Encoder counts -> current angle -> Forward Kinematics
 int main()
 {
     pc.baud(115200);            //serial baudrate
-    if (button.read() == 1)
-    {
-    while(emg_1<10){
-    signal_emg.attach(take_sample,1/500);
-    
-    rpc=(2*PI)/ENCODER1_CPR;               //radians per count (resolution) - 2pi divided by 4200
-    theta1 = emg_1 * rpc;   //multiply resolution with number of counts
-    theta2 = emg_2 * rpc;
-    current_x = l1 * cos(theta1) + l2 * cos(theta1 + theta2);
-    current_y = l1 * sin(theta1) + l2 * sin(theta1 + theta2);
-    
-    pc.printf("Theta1 = %f, theta2 = %f \r\n",theta1,theta2);   
-        
-    //calculate error (refpos-currentpos) currentpos = forward kinematics
-    x_error = x - current_x;
-    y_error = y - current_y;
-    
-    
-    //inverse kinematics (refpos to refangle)
-    
-    costheta2 = (pow(x,2) + pow(y,2) - pow(l1,2) - pow(l2,2)) / (2*l1*l2) ;
-    sintheta2 = sqrt( abs( 1 - pow(costheta2,2) ) );
-     
-    
-    dtheta2 = atan2(sintheta2,costheta2);
-    
-    double k1 = l1 + l2*costheta2;
-    double k2 = l2*sintheta2;
-    
-    dtheta1 = atan2(y, x) - atan2(k2, k1);
-    
-    /* alternative:
-    costheta1 = ( x * (l1 + l2 * costheta2) + y * l2 * sintheta2 ) / ( pow(x,2) + pow(y,2) );
-    sintheta1 = sqrt( abs( 1 - pow(costheta1,2) ) );
-    
-    dtheta1 = atan2(sintheta1,costheta1);
-    */
-    
-    //Angle error
-    m1_error = dtheta1-theta1;
-    m2_error = dtheta2-theta2;
-    
-    pc.printf("Motor 1 error is %f, motor 2 error is %f /r/n",m1_error,m2_error);
-    }
+    while(true) {
+        if (button == 0) {
+            pc.printf("knop1\n");
+            while(emg_1<2) {
+                signal_emg.attach(&take_sample,0.5);
+                pc.printf("emg1 = %f\n",emg_1);
+
+                rpc=(2*PI)/ENCODER1_CPR;               //radians per count (resolution) - 2pi divided by 4200
+                theta1 = emg_1 * rpc;   //multiply resolution with number of counts
+                theta2 = emg_2 * rpc;
+                current_x = l1 * cos(theta1) + l2 * cos(theta1 + theta2);
+                current_y = l1 * sin(theta1) + l2 * sin(theta1 + theta2);
+
+                pc.printf("Theta1 = %f, theta2 = %f \r\n",theta1,theta2);
+
+                //calculate error (refpos-currentpos) currentpos = forward kinematics
+                x_error = x - current_x;
+                y_error = y - current_y;
+
+
+                //inverse kinematics (refpos to refangle)
+
+                costheta2 = (pow(x,2) + pow(y,2) - pow(l1,2) - pow(l2,2)) / (2*l1*l2) ;
+                sintheta2 = sqrt( abs( 1 - pow(costheta2,2) ) );
+
+
+                dtheta2 = atan2(sintheta2,costheta2);
+
+                double k1 = l1 + l2*costheta2;
+                double k2 = l2*sintheta2;
+
+                dtheta1 = atan2(y, x) - atan2(k2, k1);
+
+                /* alternative:
+                costheta1 = ( x * (l1 + l2 * costheta2) + y * l2 * sintheta2 ) / ( pow(x,2) + pow(y,2) );
+                sintheta1 = sqrt( abs( 1 - pow(costheta1,2) ) );
+
+                dtheta1 = atan2(sintheta1,costheta1);
+                */
+
+                //Angle error
+                m1_error = dtheta1-theta1;
+                m2_error = dtheta2-theta2;
+
+                pc.printf("Motor 1 error is %f, motor 2 error is %f \r\n",m1_error,m2_error);
+                wait(0.5);
+            }
+        }
     }
 }
\ No newline at end of file