Diff: main.cpp

Revision:

1:e3db171abbb2

Parent:

0:ec8fa8a84edd

Child:

2:9f343567723c

diff -r ec8fa8a84edd -r e3db171abbb2 main.cpp
--- a/main.cpp	Wed Nov 01 16:45:26 2017 +0000
+++ b/main.cpp	Wed Nov 01 21:13:47 2017 +0000
@@ -16,7 +16,7 @@
 MODSERIAL pc(USBTX,USBRX);
 
 float PwmPeriod = 1.0/5000.0;           //set up of PWM periode (5000 Hz, want 5000 periodes in 1 seconde)
-const float Ts = 1/500;                   // tickettijd/ sample time
+const float Ts = 0.1;                   // tickettijd/ sample time
 float e_prev = 0; 
 float e_int = 0;
 float PwmPeriod2 = 1.0/5000.0;           //set up of PWM periode (5000 Hz, want 5000 periodes in 1 seconde)
@@ -27,16 +27,21 @@
 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)
+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 = 1/500;       //Desired time step
+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;
 
 
 //tweede motor
@@ -47,14 +52,19 @@
 
 void RKI()
 {
-    p=sin(SetPx);
-    q1 = q1 + ((sin(q1)*L1 + sin(q2)*L2)*SetPy - (cos(q1)*L1 + cos(q2)*L2)*SetPx)*(K*T)/B1;     //Calculate desired joint 1 position
-    q2 = q2 + ((SetPy - cos(q1)*L1)*sin(q2)*L2 + (sin(q1)*L1 - SetPx)*cos(q2)*L2)*(K*T)/B2;     //Calculate desired joint 2 position
+    p=sin(q1)*L1;
+    pp=sin(q2)*L2;
+    a=cos(q1)*L1;
+    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
     
     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
     
-    //pc.printf("waarde p = %f \r\n",p);
+    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);
 }
@@ -64,22 +74,22 @@
     double Potmeterwaarde2 = potMeter2.read();
     double Potmeterwaarde1 = potMeter1.read();
 
-    if (Potmeterwaarde2>0.5) {
+    if (Potmeterwaarde2>0.4) {
         SetPx++;    // hoe veel verder gaat hij? 1 cm? 10 cm?
     }
-    if (Potmeterwaarde2<0.5) {
+    if (Potmeterwaarde2<0.6) {
         SetPx--;
     }
-    if (Potmeterwaarde1>0.5) {
+    if (Potmeterwaarde1>0.4) {
         SetPy++;
     }
-    if (Potmeterwaarde1<0.5) {
+    if (Potmeterwaarde1<0.6) {
         SetPy--;
     }
     //pc.printf("Setpointx = %f, Setpointy = %f \r\n", SetPx, SetPy);
 }
 
-float GetReferencePosition() 
+/*float GetReferencePosition() 
 {
     float Potmeterwaarde = potMeter2.read();
     int maxwaarde = 4096;                   // = 64x64
@@ -89,11 +99,11 @@
 
 float GetReferencePosition2() 
 {
-    float potmeterwaarde2 = potMeter1.read();
+    float Potmeterwaarde2 = potMeter1.read();
     int maxwaarde2 = 4096;                   // = 64x64
-    float refP2 = potmeterwaarde2*maxwaarde2;
+    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)
 {
@@ -138,11 +148,11 @@
 {
     if (motorValue >= 0)
     {
-        M1D = 0;
+        M1D = 0;                    //direction ...
     }
     else 
     {
-        M1D = 1;
+        M1D = 1;                    //direction ...
     }
 
     if  (fabs(motorValue) > 1)    
@@ -215,16 +225,15 @@
     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.
-      pc.baud(115200);
+    pc.baud(115200);
      
      
     while(1) 
     {
-        wait(0.2);
+        //wait(0.2);
         float B = motor1.getPosition();
-        float Potmeterwaarde = potMeter2.read();
         //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, Setpointx = %f, Setpointy = %f \r\n", q1, q2, Motor1Set, Motor2Set, SetPx, SetPy);   
+        //pc.printf("q1 = %f, q2 = %f, Motor1Set = %f, Motor2Set = %f \r\n", q1, q2, Motor1Set, Motor2Set);   
     }
 }