Revision 13:912749d30059, committed 2017-11-02

Comitter:

Annelotte

Date:

Thu Nov 02 20:51:00 2017 +0000

Parent:

12:9a1a60c95316

Commit message:

Werkend? De functie van Annelotte nieuw
;

Changed in this revision

diff -r 9a1a60c95316 -r 912749d30059 main.cpp
--- a/main.cpp	Thu Nov 02 19:42:03 2017 +0000
+++ b/main.cpp	Thu Nov 02 20:51:00 2017 +0000
@@ -35,39 +35,48 @@
 
 //RKI
 double pi = 3.14159265359;
-double SetPx = 0.38;     //Setpoint position x-coordinate from changePosition (EMG dependent)
-double SetPy = 0.30;     //Setpoint position y-coordinate from changePosition (EMG dependent)
-double q1 = 0;           //Reference position q1 from calibration (only the first time)
-double q2 = (pi/2);      //Reference position q2 from calibration (only the first time): LET OP! DE MOTOR
-const double L1 = 0.30;  //Length arm 1
-const double L2 = 0.38;  //Length arm 2
-double K = 10;            //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.1;         //Desired time step
-double Motor1Set;        //Motor1 angle
-double Motor2Set;        //Motor2 angle
-double p;
-double pp;
-double bb;
-double cc;
-double a;
-double aa;
-double tau1;
-double tau2;
-
+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 Motor1Set = 0;      //Reference position motor 1
+double Motor2Set = 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()
 {
-    p=sin(q1)*L1;
-    pp=sin(q2)*L2;
-    a=cos(q1)*L1;
-    aa=cos(q2)*L2;
-    bb=SetPy;
-    cc=SetPx;
-    tau1 = (p + pp)*bb - (a + aa)*cc;
-    tau2 = (bb - a)*pp + (p - cc)*aa;
-    if (tau1 < 0.00001){
+    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;
+    
+    
+    /*if (tau1 < 0.00001){
       tau1 = 0;
     }
     else {
@@ -76,14 +85,11 @@
       tau2 = 0;
     }
     else{
-        tau2 = tau2;}
-    q1 = q1 + tau1*(K*T)/B1;     //Calculate desired joint 1 position
-    q2 = q2 + tau2*(K*T)/B2;     //Calculate desired joint 2 position
+        tau2 = tau2;}*/
     
     int maxwaarde = 4096;                   // = 64x64
-    
-    Motor1Set = (q1/(2*pi))*maxwaarde;
-    Motor2Set = (q2/(2*pi))*maxwaarde;
+    Motor2Set = (((0.5*pi) + q1 - q2)/(2*pi))*maxwaarde;
+    Motor1Set = (((0.5*pi) - q1)/(2*pi))*maxwaarde;
     
     //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
@@ -98,21 +104,21 @@
     bool knop2 = Knopje2;
 
     if  (knop1 == false){        //(Potmeterwaarde2>0.6) {
-        SetPx += 0.001;    // hoe veel verder gaat hij? 1 cm? 10 cm?
+        Rsx += 0.001;    // hoe veel verder gaat hij? 1 cm? 10 cm?
     }
     else if  (knop1 == true){  //(Potmeterwaarde2<0.4) {
                                 //SetPx -= 0.001;
     }
-    else
-    {}
+    /*else
+    {}*/
     if (knop2 == false){         //(Potmeterwaarde1>0.6) {
-        SetPy += 0.001;
+        Rsy += 0.001;
     }
     else if (knop2 ==true){    //(Potmeterwaarde1<0.4) {
                                 //SetPy -= 0.001;
     }
-    else
-    {}
+    /*else
+    {}*/
     //pc.printf("Setpointx = %f, Setpointy = %f \r\n", SetPx, SetPy);
 }
 
@@ -131,7 +137,7 @@
     float refP2 = Potmeterwaarde2*maxwaarde2;
     return refP2;                            // value between 0 and 4096 
 }*/
-    
+    /*
 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.0005;                             // kind of scaled.
@@ -224,12 +230,13 @@
     float Huidigepositie2 = motor2.getPosition ();
     return Huidigepositie2;             // huidige positie = current position
 }
-
+*/
 void MeasureAndControl(void)
 {
-     // RKI aanroepen
+    SetpointRobot(); 
+    // RKI aanroepen
     RKI();
-    
+    /*
     // hier the control of the control system
     //float refP = GetReferencePosition(); 
     float Huidigepositie = Encoder(); 
@@ -242,9 +249,9 @@
     float Huidigepositie2 = Encoder2(); 
     error2 = (Motor2Set - Huidigepositie2);// make an error
     float motorValue2 = FeedBackControl2(error2, e_prev2, e_int2);
-    SetMotor2(motorValue2);
+    SetMotor2(motorValue2);*/
 }
-
+/*
 void Autodemo_or_demo()
 {
     if (autodemo_done == 0)
@@ -271,12 +278,12 @@
     }
     
 }
-
+*/
 
 int main()
 {
     M1E.period(PwmPeriod);
-    Treecko.attach(&Autodemo_or_demo, Ts);   //Elke 1 seconde zorgt de ticker voor het runnen en uitlezen van de verschillende 
+    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.
     pc.baud(115200);
      
@@ -284,9 +291,9 @@
     while(1) 
     {
         //wait(0.2);
-        float B = motor1.getPosition();
-        pc.printf("Setpointx = %f, Setpointy = %f, tau1 = %f, tau2 = %f, Motor1Set = %f, Motor2Set = %f,", SetPx, SetPy, tau1, tau2, Motor1Set, Motor2Set);
-        pc.printf(" q1 = %f, q2 = %f, error1 = %f, error2 = %f \r\n", q1, q2,error1, error2);
+        //float B = motor1.getPosition();
+        pc.printf("Setpointx = %f, Setpointy = %f, tau1 = %f, tau2 = %f, Motor1Set = %f, Motor2Set = %f,", Rsx, Rsy, Tor1, Tor2, Motor1Set, Motor2Set);
+        pc.printf(" q1 = %f, q2 = %f \r\n", q1, q2);
         //float positie = B%4096;
         //pc.printf("pos: %d, speed %f, potmeter = %f V, \r\n",motor1.getPosition(), motor1.getSpeed(),(potMeter2.read()*3.3)); //potmeter uitlezen. tussen 0-1. voltage, dus *3.3V
         //pc.printf("q1 = %f, q2 = %f, Motor1Set = %f, Motor2Set = %f \r\n", q1, q2, Motor1Set, Motor2Set);