Diff: main.cpp

Revision:

12:126ae33919b3

Parent:

11:d5369a546201

--- a/main.cpp	Thu Nov 02 11:22:08 2017 +0000
+++ b/main.cpp	Thu Nov 02 12:46:35 2017 +0000
@@ -22,7 +22,7 @@
 float e_int = 0;
 
 //tweede motor
-AnalogIn potmeter1(A2);
+AnalogIn potMeter1(A2);
 PwmOut M2E(D5);
 DigitalOut M2D(D4);
 Encoder motor2(D9,D8,true);
@@ -32,8 +32,10 @@
 float e_prev2 = 0; 
 float e_int2 = 0;
 
-float Potmeterwaarde;
-float potmeterwaarde2;
+double Potmeterwaarde1;
+double Potmeterwaarde2;
+double Potmeterwaarde;
+double potmeterwaarde2;
 int maxwaarde, maxwaarde2;
 float refP, refP2;
 float kp, Proportional, kd, VelocityError, Derivative, ki, Integrator, motorValue;
@@ -44,6 +46,68 @@
 float Aerror2;
 float motorVal, motorVal2;
 
+double pi = 3.14159265359;
+double SetPx = 38;     //Setpoint position x-coordinate from changePosition (EMG dependent)
+double SetPy = 30;     //Setpoint position y-coordinate from changePosition (EMG dependent)
+volatile double q1 = 0;          //Reference position q1 from calibration (only the first time)
+volatile double q2 = (pi/2);       //Reference position q2 from calibration (only the first time)
+const double L1 = 30;  //Length arm 1
+const double L2 = 38;  //Length arm 2
+double K = 1;           //Spring constant for movement end-joint to setpoint
+double B1 = 1;          //Friction coefficient for motor 1
+double B2 = 1;          //Friction coefficient for motot 2
+double T = 0.02;       //Desired time step
+double Motor1Set;       //Motor1 angle
+double Motor2Set;       //Motor2 angle
+double p;
+double pp;
+double bb;
+double cc;
+double a;
+double aa;
+
+void RKI()
+{
+    p=sin(q1)*L1;
+    pp=sin(q2)*L2;
+    a=cos(q1)*L1;
+    aa=cos(q2)*L2;
+    bb=SetPy;
+    cc=SetPx;
+    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
+    
+    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);
+    //pc.printf("Setpointx = %f, Setpointy = %f \r\n", SetPx, SetPy);
+}
+
+void SetpointRobot()
+{   
+     Potmeterwaarde2 = potMeter2.read();
+     Potmeterwaarde1 = potMeter1.read();
+
+    if (Potmeterwaarde2>0.6) {
+        SetPx++;    // hoe veel verder gaat hij? 1 cm? 10 cm?
+    }
+    if (Potmeterwaarde2<0.4) {
+        SetPx--;
+    }
+    if (Potmeterwaarde1>0.6) {
+        SetPy++;
+    }
+    if (Potmeterwaarde1<0.4) {
+        SetPy--;
+    }
+    //pc.printf("Setpointx = %f, Setpointy = %f \r\n", SetPx, SetPy);
+}
+
 float GetReferencePosition() 
 {
      Potmeterwaarde = potMeter2.read();
@@ -54,7 +118,7 @@
 
 float GetReferencePosition2() 
 {
-     potmeterwaarde2 = potmeter1.read();
+     potmeterwaarde2 = potMeter1.read();
      maxwaarde2 = 4096;                   // = 64x64
      refP2 = potmeterwaarde2*maxwaarde2;
     return refP2;                            // value between 0 and 4096 
@@ -155,22 +219,25 @@
 
 void MeasureAndControl(void)
 {
+    // RKI aanroepen
+    SetpointRobot();
+    RKI();
     // hier the control of the control system
-     refP = GetReferencePosition(); 
+     //refP = GetReferencePosition(); 
      Huidigepositie = Encoder(); 
-     Aerror = (refP - Huidigepositie);// make an error
+     Aerror = (Motor1Set - Huidigepositie);// make an error
      motorValue = FeedBackControl(Aerror, e_prev, e_int);
     SetMotor1(motorValue);
 
     // hier the control of the control system
-     refP2 = GetReferencePosition2(); 
+     //refP2 = GetReferencePosition2(); 
      Huidigepositie2 = Encoder2(); 
-     Aerror2 = (refP2 - Huidigepositie2);// make an error
+     Aerror2 = (Motor2Set - Huidigepositie2);// make an error
      motorValue2 = FeedBackControl2(Aerror2, e_prev2, e_int2);
     SetMotor2(motorValue2);
     
     pc.baud(115200);
-    pc.printf("potmeter = %f, refP = %f, huidigepositie = %f, error = %f, motorvalue = %f \r\n, potmeter2 = %f, refP2 = %f, huidigepositie2 = %f, error2 = %f, motorvalue2 = %f \r\n", Potmeterwaarde, refP, Huidigepositie, Aerror, motorValue, potmeterwaarde2, refP2, Huidigepositie2, Aerror2, motorValue2);
+    pc.printf("SetPx = %f, SetPy = %f\r\n potmeter = %f, refP = %f, huidigepositie = %f, error = %f, motorvalue = %f \r\npotmeter2 = %f, refP2 = %f, huidigepositie2 = %f, error2 = %f, motorvalue2 = %f \r\n", SetPx, SetPy, Potmeterwaarde1, Motor1Set, Huidigepositie, Aerror, motorValue, Potmeterwaarde2, Motor2Set, Huidigepositie2, Aerror2, motorValue2);
 }