Diff: main.cpp

Revision:

6:6545e197858a

Parent:

5:a54ea6514bc5

Child:

7:83a69ca630bc

--- a/main.cpp	Wed Oct 31 19:26:42 2018 +0000
+++ b/main.cpp	Wed Oct 31 19:52:34 2018 +0000
@@ -61,6 +61,7 @@
 const float yas = 0.346;        // afstand van xas tot motor 2
 const float thetap = 0;         // rotatiehoek van de end effector
 
+
 // motor locatie
 const int a1x = 0;              //x locatie motor 1
 const int a1y = 0;              //y locatie motor 1
@@ -84,6 +85,11 @@
 float px = 0.2;     //starting x    
 float py = 0.155;   // starting y
 
+// verschil horizontale as met de actieve arm
+float da1 = 1.619685; // verschil a1 hoek en motor
+float da2 = -0.609780;
+float da3 = 3.372859;
+
 // limits (since no forward kinematics)
 float upperxlim = 0.36; //niet helemaal naar requierments ff kijken of ie groter kan
 float lowerxlim = 0.04;
@@ -160,8 +166,8 @@
     float c1y = py - rp*sin(thetap+(M_PI/6));           // y locatie hoekpunt end-effector
     float alpha1 = atan2((c1y-a1y),(c1x-a1x));          // hoek tussen horizontaal en lijn van motor naar bijbehorende end-effector punt
     float psi1 = acos(( pow(la,2)-pow(lp,2)+pow((c1x-a1x),2)+pow((c1y-a1y),2))/(2*la*sqrt(pow ((c1x-a1x),2)+pow((c1y-a1y),2) ))); //Hoek tussen lijn van motor naar bijbehorende end=effector punt en actieve arm
-    float a1 = alpha1 + psi1;                          //hoek tussen horizontaal en actieve arm
-    //printf("arm 1 = %f \n\r",a1);
+    float a1 = alpha1 + psi1 - da1;                          //hoek tussen horizontaal en actieve arm
+    printf("arm 1 = %f \n\r",a1);
     return a1;
 }
 
@@ -172,8 +178,8 @@
     float c2y = py - rp*sin(thetap-(M_PI/2)); 
     float alpha2 = atan2((c2y-a2y),(c2x-a2x)); 
     float psi2 = acos(( pow(la,2)-pow(lp,2)+pow((c2x-a2x),2)+pow((c2y-a2y),2))/(2*la*sqrt(pow ((c2x-a2x),2)+pow((c2y-a2y),2) )));
-    float a2 = alpha2 + psi2;   
-    //printf("arm 2 = %f \n\r",a2);
+    float a2 = alpha2 + psi2 - da2;   
+    printf("arm 2 = %f \n\r",a2);
     return a2;
 }
 
@@ -184,8 +190,8 @@
     float c3y = py - rp*sin(thetap+(5*M_PI/6)); 
     float alpha3 = atan2((c3y-a3y),(c3x-a3x)); 
     float psi3 = acos(( pow(la,2)-pow(lp,2)+pow((c3x-a3x),2)+pow((c3y-a3y),2))/(2*la*sqrt(pow ((c3x-a3x),2)+pow((c3y-a3y),2) ))); 
-    float a3 = alpha3 + psi3;   
-    //printf("arm 3 = %f \n\r",a3);
+    float a3 = alpha3 + psi3 - da3;   
+    printf("arm 3 = %f \n\r",a3);
     return a3;
 }