Diff: main.cpp

Revision:

18:8f5305cebad4

Parent:

17:45b31bf0c11e

diff -r 45b31bf0c11e -r 8f5305cebad4 main.cpp
--- a/main.cpp	Fri Nov 02 13:46:29 2018 +0000
+++ b/main.cpp	Tue Nov 06 15:35:54 2018 +0000
@@ -103,23 +103,23 @@
 
 // VARIABLES ROBOT KINEMATICS
 // constants
-const float la = 0.256;         // lengte actieve arm
-const float lp = 0.21;          // lengte passieve arm
-const float rp = 0.052;         // straal van midden end effector tot hoekpunt
-const float rm = 0.23;          // straal van global midden tot motor
-const float a = 0.09;           // zijde van de driehoek
-const float xas = 0.40;         // afstand van motor 1 tot motor 3
-const float yas = 0.346;        // afstand van xas tot motor 2
-const float thetap = 0;         // rotatiehoek van de end effector
+const float la = 0.256;         // length active arm
+const float lp = 0.21;          // length passive arm
+const float rp = 0.052;         // radius from midpoint end effector to cornerpoint
+const float rm = 0.23;          // radius from global midpoint to motor
+const float a = 0.09;           // side of a triangle
+const float xas = 0.40;         // distance from motor 1 to 3
+const float yas = 0.346;        // distance from motor 2 to x axis
+const float thetap = 0;         // rotation angle of end effector
 
 
-// motor locatie
-const int a1x = 0;              //x locatie motor 1
-const int a1y = 0;              //y locatie motor 1
-const float a2x = (0.5)*xas;    // x locatie motor 2
-const float a2y = yas;          // y locatie motor 2
-const float a3x = xas;          // x locatie motor 3
-const int a3y = 0;              // y locatie motor 3
+// motor location
+const int a1x = 0;              //x location motor 1
+const int a1y = 0;              //y location motor 1
+const float a2x = (0.5)*xas;    // x location motor 2
+const float a2y = yas;          // y location motor 2
+const float a3x = xas;          // x location motor 3
+const int a3y = 0;              // y location motor 3
 
 // script voor het bepalen van de desired position aan de hand van emg (1/0)
 
@@ -129,23 +129,22 @@
 int EMGyplus;
 int EMGymin ;
 
-// Dit moet experimenteel geperfectioneerd worden
-float tijdstap = 0.005;      //nu wss heel langzaam, kan miss omhoog KEER V GEEFT VERANDERING IN POSITIE
-float v = 0.1;                // snelheid kan wss ook hoger
+float tijdstap = 0.005;      // timestep (times velocity gives the size of the step)
+float v = 0.1;                // angle velocity
 
 float px = 0.2;     //starting x    // BOUNDARIES
 float py = 0.155;   // starting y   // BOUNDARIES
 
-// verschil horizontale as met de actieve arm
-float da1 = 1.619685; // verschil a1 hoek en motor
+// difference horizontal axis with active arm
+float da1 = 1.619685; // difference angle a1 with motor
 float da2 = -0.609780;
 float da3 = 3.372859;
 
 // limits (since no forward kinematics)
-float upperxlim = 0.25; //36, 0.04, 0.315, -0.085niet helemaal naar requierments ff kijken of ie groter kan
+float upperxlim = 0.25; 
 float lowerxlim = 0.15;
 float upperylim = 0.225;
-float lowerylim = 0.05; //0.03 is goed
+float lowerylim = 0.05; 
 
 // VARIABLES PID CONTROLLER
 double PI = M_PI;// CHANGE THIS INTO M_PI
@@ -368,7 +367,7 @@
 
 // ~~~~~~~~~~~~~~ROBOT KINEMATICS ~~~~~~~~~~~~~~~~~~
 
-// functie x positie
+// function x positition
 float positionx(int EMGxplus,int EMGxmin)
 {
     float EMGx = EMGxplus - EMGxmin;
@@ -385,12 +384,11 @@
             px = lowerxlim;
         }
     }
-//printf("X eindpunt (%f) en verplaatsing: (%f)\n\r",px,verplaatsingx);
     return px;
 }
 
 
-// functie y positie
+// function y position
 float positiony(int EMGyplus,int EMGymin)
 {
     float EMGy = EMGyplus - EMGymin;
@@ -408,7 +406,7 @@
             py = lowerylim;
         }
     }
-//printf("Y eindpunt (%f) en verplaatsing: (%f) \n\r",py,verplaatsingy);
+
     return (py);
 }
 
@@ -417,11 +415,11 @@
 // arm 1 --> reference angle motor 1
 float hoek1(float px, float py)   // input: ref x, ref y
 {
-    float c1x =  px - rp * cos(thetap +(M_PI/6));       // x locatie hoekpunt end-effector
-    float c1y = py - rp*sin(thetap+(M_PI/6));           // y locatie hoekpunt end-effector
+    float c1x =  px - rp * cos(thetap +(M_PI/6));       // x location angle point end-effector
+    float c1y = py - rp*sin(thetap+(M_PI/6));           // y location angle point 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 - da1;                          //hoek tussen horizontaal en actieve arm
+    float a1 = alpha1 + psi1 - da1;                          //angle between horizontal and active arm
     //printf("arm 1 = %f \n\r",a1);
     return a1;
 }