Diff: main.cpp

Revision:

15:0cbbf84528bd

Parent:

14:5534b8282a06

Child:

16:4bfcc154bba0

--- a/main.cpp	Thu Nov 02 22:25:35 2017 +0000
+++ b/main.cpp	Fri Nov 03 01:04:34 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);
@@ -36,7 +36,76 @@
 float e_prev2 = 0; 
 float e_int2 = 0;
 
+// RKI
+double pi = 3.14159265359;
+double q1 = (pi/2);    //Reference position hoek 1 in radiance
+double q2 = -(pi/2);   //Reference position hoek 2 in radiance
+const double L1 = 0.30;        //Length arm 1 in mm
+const double L2 = 0.38;        //Length arm 2 in mm
+double B1 = 1;          //Friction constant motor 1
+double B2 = 1;          //Friction constant motor 2
+double K = 1;           //Spring constant movement from end-effector position to setpoint position
+double Tijd = 1;           //Timestep value
+double Rsx = 0.38;       //Reference x-component of the setpoint radius
+double Rsy = 0.30;       //Reference y-component of the setpoint radius
+double refP = 0;      //Reference position motor 1
+double refP2 = 0.5*pi; //Reference position motor 2
+double Rex = cos(q1)*L1 - sin(q2)*L2;   //The x-component of the end-effector radius 
+double Rey = sin(q1)*L1 + cos(q2)*L2;   //The y-component of the end-effector radius
+double R1x = 0;                         //The x-component of the joint 1 radius
+double R1y = 0;                         //The y-component of the joint 1 radius
+double R2x = cos(q1)*L1;                //The x-component of the joint 2 radius
+double R2y = sin(q1)*L1;                //The y-component of the joint 1 radius   
+double Fx = 0;
+double Fy = 0;
+double Tor1 = 0;
+double Tor2 = 0;
+double w1= 0;
+double w2= 0;
 
+void RKI()
+{
+    Rex = cos(q1)*L1 - sin(q2)*L2;
+    Rey = sin(q1)*L1 + cos(q2)*L2;
+    R2x = cos(q1)*L1;
+    R2y = sin(q1)*L1;
+    Fx = (Rsx-Rex)*K;
+    Fy = (Rsy-Rey)*K;
+    Tor1 = (Rex-R1x)*Fy + (R1y-Rey)*Fx;
+    Tor2 = (Rex-R2x)*Fy + (R2y-Rey)*Fx;
+    w1 = Tor1/B1;
+    w2 = Tor2/B2;
+    q1 = q1 + w1*Tijd;
+    q2 = q2 + w2*Tijd;
+    
+    int maxwaarde = 4096;                   // = 64x64
+    refP = (((0.5*pi) - q1)/(2*pi))*maxwaarde;
+    refP2 = (((0.5*pi) + q1 - q2)/(2*pi))*maxwaarde;    //Get reference positions
+}
+
+void SetpointRobot()
+{    
+    double Potmeterwaarde2 = potMeter2.read();
+    double Potmeterwaarde1 = potMeter1.read();
+    
+    if  (Potmeterwaarde2>0.6) {
+        Rsx += 0.001;                    //het gaat telkens 1 mm verder wanneer de potmeter boven de 0.6 staat
+    }
+    else if  (Potmeterwaarde2<0.4) {
+        Rsx -= 0.001;                    //het gaat telkens 1 mm terug wanneer de potmeter onder de 0.4 staat
+    }
+    else {                               //de x-waarde van de setpoint verandert niet
+        }
+
+    if (Potmeterwaarde1>0.6) {           //het gaat telkens 1 mm verder wanneer de potmeter boven de 0.6 staat
+        Rsy += 0.001;
+    }
+    else if (Potmeterwaarde1<0.4) {      //het gaat telkens 1 mm terug wanneer de potmeter onder de 0.4
+        Rsy -= 0.001;                       
+    }
+    else {                               //de y-waarde van de setpoint verandert niet
+        }
+}
 
 float GetReferencePosition() 
 {
@@ -48,7 +117,7 @@
 
 float GetReferencePosition2() 
 {
-    float potmeterwaarde2 = potmeter1.read();
+    float potmeterwaarde2 = potMeter1.read();
     int maxwaarde2 = 4096;                   // = 64x64
     float refP2 = -potmeterwaarde2*maxwaarde2;
     return refP2;                            // value between 0 and 4096 
@@ -56,15 +125,15 @@
     
 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.0015;                               // kind of scaled.
     float Proportional= kp*error;
     
-    float kd = 0.0004;                           // kind of scaled. 
+    float kd = 0.000008;                           // kind of scaled. 
     float VelocityError = (error - e_prev)/Ts; 
     float Derivative = kd*VelocityError;
     e_prev = error;
     
-    float ki = 0.0005;                           // kind of scaled.
+    float ki = 0.0001;                           // kind of scaled.
     e_int = e_int+Ts*error;
     float Integrator = ki*e_int;
     
@@ -75,21 +144,22 @@
 
 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.002;                             // kind of scaled.
     float Proportional2= kp2*error2;
     
-    float kd2 = 0.001;                           // kind of scaled. 
+    float kd2 = 0.000008;                           // kind of scaled. 
     float VelocityError2 = (error2 - e_prev2)/Ts; 
     float Derivative2 = kd2*VelocityError2;
     e_prev2 = error2;
     
-    float ki2 = 0.005;                           // kind of scaled.
+    float ki2 = 0.00005;                           // kind of scaled.
     e_int2 = e_int2+Ts*error2;
     float Integrator2 = ki2*e_int2;
     
     
     float motorValue2 = Proportional2 + Integrator2 + Derivative2;
     return motorValue2;
+    
 }
 
 
@@ -149,18 +219,19 @@
 
 void MeasureAndControl(void)
 {
+    SetpointRobot(); 
+    // RKI aanroepen
+    RKI();
     // hier the control of the control system
-    float refP = GetReferencePosition(); 
+    //float refP = GetReferencePosition(); 
     float Huidigepositie = Encoder(); 
     float error = (refP - Huidigepositie);// make an error
     float motorValue = FeedBackControl(error, e_prev, e_int);
     SetMotor1(motorValue);
-}
+    pc.printf("refP = %f, Huidigepositie = %f, error = %f, motorValue = %f \r\n", refP, Huidigepositie, error, motorValue);
 
-void MeasureAndControl2(void)
-{
     // hier the control of the control system
-    float refP2 = GetReferencePosition2(); 
+    //float refP2 = GetReferencePosition2(); 
     float Huidigepositie2 = Encoder2(); 
     float error2 = (refP2 - Huidigepositie2);// make an error
     float motorValue2 = FeedBackControl2(error2, e_prev2, e_int2);
@@ -175,7 +246,7 @@
     M1E.period(PwmPeriod);
     Treecko.attach(MeasureAndControl, Ts);   //Elke 1 seconde zorgt de ticker voor het runnen en uitlezen van de verschillende 
                                             //functies en analoge signalen. Veranderingen worden elke 1 seconde doorgevoerd.
-    DubbelTreecko.attach(MeasureAndControl2, Ts);
+    //DubbelTreecko.attach(MeasureAndControl2, Ts);
      pc.baud(115200);
      
     while(1)