Diff: main.cpp

Revision:

2:9f343567723c

Parent:

1:e3db171abbb2

Child:

3:120fbef23c17

--- a/main.cpp	Wed Nov 01 21:13:47 2017 +0000
+++ b/main.cpp	Thu Nov 02 09:43:40 2017 +0000
@@ -58,11 +58,14 @@
     aa=cos(q2)*L2;
     bb=SetPy;
     cc=SetPx;
-    q1 += ((p + pp)*bb - (a + aa)*cc)*(K*T)/B1;     //Calculate desired joint 1 position
-    q2 += ((bb - a)*pp + (p - cc)*aa)*(K*T)/B2;     //Calculate desired joint 2 position
+    q1 = q1 + ((p + pp)*bb - (a + aa)*cc)*(K*T)/B1;     //Calculate desired joint 1 position
+    q2 = q2 + ((bb - a)*pp + (p - cc)*aa)*(K*T)/B2;     //Calculate desired joint 2 position
     
-    Motor1Set = q1/(2*pi);           //Calculate the desired motor1 angle from the desired joint positions
-    Motor2Set = (pi-q2-q1)/(2*pi);   //Calculate the desired motor2 angle from the desired joint positions
+    int maxwaarde = 4096;                   // = 64x64
+    
+    
+    Motor1Set = (q1/(2*pi))*maxwaarde;           //Calculate the desired motor1 angle from the desired joint positions
+    Motor2Set = ((pi-q2-q1)/(2*pi))*maxwaarde;   //Calculate the desired motor2 angle from the desired joint positions
     
     pc.printf("waarde p = %f, waarde pp = %f, a= %f, aa = %f, bb = %f, cc = %f \r\n",p,pp,a,aa,bb,cc);
     //pc.printf("q1 = %f, q2 = %f, Motor1Set = %f, Motor2Set = %f \r\n", q1, q2, Motor1Set, Motor2Set);
@@ -74,16 +77,16 @@
     double Potmeterwaarde2 = potMeter2.read();
     double Potmeterwaarde1 = potMeter1.read();
 
-    if (Potmeterwaarde2>0.4) {
+    if (Potmeterwaarde2>0.6) {
         SetPx++;    // hoe veel verder gaat hij? 1 cm? 10 cm?
     }
-    if (Potmeterwaarde2<0.6) {
+    if (Potmeterwaarde2<0.4) {
         SetPx--;
     }
-    if (Potmeterwaarde1>0.4) {
+    if (Potmeterwaarde1>0.6) {
         SetPy++;
     }
-    if (Potmeterwaarde1<0.6) {
+    if (Potmeterwaarde1<0.4) {
         SetPy--;
     }
     //pc.printf("Setpointx = %f, Setpointy = %f \r\n", SetPx, SetPy);
@@ -107,7 +110,7 @@
     
 float FeedBackControl(float error, float &e_prev, float &e_int)   // schaalt de snelheid naar de snelheid zodat onze chip het begrijpt (is nog niet in werking)
 {
-    float kp = 0.001;                             // kind of scaled.
+    float kp = 0.0005;                             // kind of scaled.
     float Proportional= kp*error;
     
     float kd = 0.0004;                           // kind of scaled. 
@@ -126,7 +129,7 @@
 
 float FeedBackControl2(float error2, float &e_prev2, float &e_int2)   // schaalt de snelheid naar de snelheid zodat onze chip het begrijpt (is nog niet in werking)
 {
-    float kp2 = 0.001;                             // kind of scaled.
+    float kp2 = 0.0005;                             // kind of scaled.
     float Proportional2= kp2*error2;
     
     float kd2 = 0.0004;                           // kind of scaled.