Diff: main.cpp

Revision:

18:1e4f697a92cb

Parent:

17:dbdbd1edc260

Child:

19:591572f4e4b5

diff -r dbdbd1edc260 -r 1e4f697a92cb main.cpp
--- a/main.cpp	Thu Nov 02 22:32:42 2017 +0000
+++ b/main.cpp	Fri Nov 03 02:06:37 2017 +0000
@@ -13,7 +13,7 @@
 double looptime = 0.002f;
 double value;
 double home1;
-DigitalIn button (D4);
+DigitalIn button (D1);
 
 //Encoder/motor
 double Huidigepositie1;
@@ -35,8 +35,6 @@
 AnalogIn potMeter2(A1);
 AnalogIn potMeter1(A2);
 
-
-
 MODSERIAL pc(USBTX,USBRX);
 
 double PwmPeriod = 1.0/5000.0;           //set up of PWM periode (5000 Hz, want 5000 periodes in 1 seconde)
@@ -55,7 +53,7 @@
 //buttons  en leds voor calibration
 DigitalIn button1(PTA4);
 
-DigitalOut led(D2)
+DigitalOut led(D2);
 
 //MVC for calibration
 double MVCLB = 0; double MVCRB = 0; double MVCLT = 0; double MVCRT = 0;
@@ -156,6 +154,33 @@
 // variabelen changePosition
 int goalx, goaly;
 
+// variables 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 Filteren() 
 {
    // LooptimeEMG.reset();
@@ -357,6 +382,50 @@
   //return maxi;
 }
 
+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 = (((-pi) + q1 - q2)/(2*pi))*maxwaarde;    //Get reference positions was eerst 0.5*pi
+}
+
+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
+        }
+}
+
 double GetReferencePosition() 
 {
     double Potmeterwaarde = potMeter2.read(); //naam moet universeel worden
@@ -372,44 +441,46 @@
     double refP2 = potmeterwaarde2*maxwaarde2;
     return refP2;                            // value between 0 and 4096 
 }
-    
-double FeedBackControl(double error, double &e_prev, double &e_int)   // schaalt de snelheid naar de snelheid zodat onze chip het begrijpt (is nog niet in werking)
+   
+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)
 {
-    double kp = 0.001;                             // kind of scaled.
-    double Proportional= kp*error;
+    float kp = 0.0015;                               // kind of scaled.
+    float Proportional= kp*error;
     
-    double kd = 0.0004;                           // kind of scaled. 
-    double VelocityError = (error - e_prev)/Ts; 
-    double Derivative = kd*VelocityError;
+    float kd = 0.000008;                           // kind of scaled. 
+    float VelocityError = (error - e_prev)/Ts; 
+    float Derivative = kd*VelocityError;
     e_prev = error;
     
-    double ki = 0.00005;                           // kind of scaled.
+    float ki = 0.0001;                           // kind of scaled.
     e_int = e_int+Ts*error;
-    double Integrator = ki*e_int;
+    float Integrator = ki*e_int;
     
-    double motorValue = Proportional + Integrator + Derivative;
+    
+    float motorValue = Proportional + Integrator + Derivative;
     return motorValue;
 }
 
-double FeedBackControl2(double error2, double &e_prev2, double &e_int2)   // schaalt de snelheid naar de snelheid zodat onze chip het begrijpt (is nog niet in werking)
+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)
 {
-    double kp2 = 0.001;                             // kind of scaled.
-    double Proportional2= kp2*error2;
+    float kp2 = 0.002;                             // kind of scaled.
+    float Proportional2= kp2*error2;
     
-    double kd2 = 0.0004;                           // kind of scaled. 
-    double VelocityError2 = (error2 - e_prev2)/Ts; 
-    double Derivative2 = kd2*VelocityError2;
+    float kd2 = 0.000008;                           // kind of scaled. 
+    float VelocityError2 = (error2 - e_prev2)/Ts; 
+    float Derivative2 = kd2*VelocityError2;
     e_prev2 = error2;
     
-    double ki2 = 0.00005;                           // kind of scaled.
+    float ki2 = 0.00005;                           // kind of scaled.
     e_int2 = e_int2+Ts*error2;
-    double Integrator2 = ki2*e_int2;
+    float Integrator2 = ki2*e_int2;
+    
     
-    double motorValue2 = Proportional2 + Integrator2 + Derivative2;
+    float motorValue2 = Proportional2 + Integrator2 + Derivative2;
     return motorValue2;
+    
 }
 
-
 void SetMotor1(double motorValue)
 {
     if (motorValue >= 0) {
@@ -430,10 +501,10 @@
 void SetMotor2(double motorValue2)
 {
     if (motorValue2 >= 0) {
-        M2D = 0;
+        M2D = 1;
     }
     else {
-        M2D = 1;
+        M2D = 0;
     }
 
     if  (fabs(motorValue2) > 1) {
@@ -446,14 +517,17 @@
 
 void MeasureAndControl(void)
 {
+    SetpointRobot(); 
+    // RKI aanroepen
+    RKI();
     // hier the control of the 1st control system
-    double refP = GetReferencePosition();                    //KOMT UIT RKI
+    //double refP = GetReferencePosition();                    //KOMT UIT RKI
     double Huidigepositie = motor1.getPosition(); 
     double error = (refP - Huidigepositie);// make an error
     double motorValue = FeedBackControl(error, e_prev, e_int);
     SetMotor1(motorValue);
     // hier the control of the 2nd control system
-    double refP2 = GetReferencePosition2(); 
+    //double refP2 = GetReferencePosition2(); 
     double Huidigepositie2 = motor2.getPosition(); 
     double error2 = (refP2 - Huidigepositie2);// make an error
     double motorValue2 = FeedBackControl2(error2, e_prev2, e_int2);