Diff: main.cpp

Revision:

8:1efebfebe733

Parent:

7:b53f0c4cf2b9

Child:

9:d5c561b3ea5a

--- a/main.cpp	Fri Oct 26 10:24:09 2018 +0000
+++ b/main.cpp	Tue Oct 30 12:17:35 2018 +0000
@@ -2,279 +2,101 @@
 #include "math.h"
 #include "BiQuad.h"
 #include "Servo.h"
-
-
-
+#include <string>
+#include "QEI.h"
 
-//Define objects
-AnalogIn    emgL(A0);                                                           // EMG Left Arm
-AnalogIn    emgR(A1);                                                           // EMG Right Arm
-AnalogIn    emgS(A2);                                                           // EMG Servo spier
-DigitalOut  ledB(LED_BLUE);                                                     // Informative LED for gaining zero and max
-DigitalOut  ledR(LED_RED);
-DigitalOut  ledG(LED_GREEN);                                                    // Informative LED for gaining zero and max
-DigitalIn   CalButton(PTA4);                                                    // Button used for gaining zero and max
-DigitalIn   zeromax(PTC6);                                                      // Button used for switching between zero and max
-Ticker      emgSampleTicker;                                                    // Ticker for sample frequency           
-                                          
-Servo myservo(D9);
+//-----------------GET ENCODER VALUES -------------------------
+QEI Encoder1(D12,D13,NC,64,QEI::X2_ENCODING);
+QEI Encoder2(D2,D3,NC,64,QEI::X2_ENCODING);
+Ticker EncoderTicker;
 
 DigitalOut motor1direction(D7);
 PwmOut motor1control(D6);
-
+ 
 DigitalOut motor2direction(D4);
 PwmOut motor2control(D5);
-
-InterruptIn button1(D10);
-InterruptIn button2(D11);
-
- int P= 200;                                                                    // Number of points for movag first emg
- int Q = 200;                                                                   // Number of points for movag second emg
- int R = 200;                                                                   // Number of points for movag third emg
- double A[200];                                                                 // Vector for storing data of first emg
- double B[200];                                                                 // Vector for storing data of second emg
- double C[200];                                                                 // Vector for storing data of third emg
- int k = 0;                                                                     // Counter for configuration:
- double Lvector[200];                                                           // Vector for Rwaarde configuration
- double Lwaarde[2];                                                             // Vector for storage of max and zero of left biceps
- double Rvector[200];                                                           // Vector for Lwaarde configuration
- double Rwaarde[2];                                                             // Vector for storage of max and zero of right biceps
- double Svector[200];                                                           // Vector for Swaarde configuration
- double Swaarde[2];                                                             // Vector for storage of max and zero of servo emg
- int x = 0;                                                                     // Variable for switching between zero and max
- double movagR;                                                                 // Moving Average mean value of right biceps
- double movagL;                                                                 // Moving Average mean value of left biceps
- double movagS;                                                                 // Moving Average mean value of servo spier
- float thresholdL = 10;                                                         // Startvalue for threshold, to block signals -
- float thresholdR = 10;                                                         // - before the zero and max values are calculated
- float thresholdS = 10;                                                         //-------
  
-const bool clockwise1 = true;                                                    //Clockwise direction for motor 1
-const bool clockwise2 = true;                                                   //Clockwise direction for motor 2
-volatile bool direction1 = clockwise1;                                            //Bool to make direction 1 clockwise
-volatile bool direction2 = clockwise2;                                          //Bool to make direction 2 clockwise
- 
- //------------Filter parameters----------------------
+void EncoderFunc() {
+    
+    const float pi = 3.141592653589793;                             // Value of pi
+    double gearratio = 3.857142857;
+    double radiuspulley = 0.015915;                                 // Radius pulley [m]
+    double hoekgraad = Encoder1.getPulses() * 0.0857142857;         // Angle arm [degree]
+    double hoekrad = hoekgraad * 0.0174532925;
+    double hoekgraad2 = Encoder2.getPulses() * 0.0857142857;
+    double hoekrad2 = hoekgraad2 * 0.0174532925;
+    double hoekarm = hoekgraad2 / gearratio;
+    double translatie = hoekgraad /360 * 2 * pi * radiuspulley;     // Translatie arm [m]
+ }
 
-//Lowpassfilter
-//const double b0LP = 0.0014831498359569692;
-//const double b1LP = 0.0029662996719139385;
-//const double b2LP = 0.0014831498359569692;
-//const double a1LP = -1.918570032544273;
-//const double a2LP = 0.9245026318881009;
-//Highpassfilter                        Fc = 10 Hz;, Q = 0.5, Fs = 500 Hz
-const double b0HP = 0.8851221317817073;
-const double b1HP = -1.7702442635634146;
-const double b2HP = 0.8851221317817073;
-const double a1HP =  -1.7632371847263784;
-const double a2HP = 0.777251342400451;
-//Notchfilter                           Fc = 50 Hz, Q = 10, Fs = 500 Hz
-const double b0NO = 0.9714498065192796;
-const double b1NO = -1.5718388053127037;
-const double b2NO = 0.9714498065192796;
-const double a1NO = -1.5718388053127037;
-const double a2NO = 0.9428996130385592;
+//----------------INVERSE KINEMATICS ---------------------------
+double K_v = 1;                                     // Velocity constant (VALUE???) Maximaal 6.6667
+double L0 = 0.09;
+
+int main()
+{   
+EncoderTicker.attach(&EncoderFunc, 1);
 
-//--------------Filter------------
-//BiQuad LPR( b0LP, b1LP, b2LP, a1LP, a2LP ); //Lowpass filter Biquad
-BiQuad HPR( b0HP, b1HP, b2HP, a1HP, a2HP ); //Highpass filter Biquad
-BiQuad NOR( b0NO, b1NO, b2NO, a1NO, a2NO ); //Notch filter Biquad 
+double p_old_x = (translatie+L0)*cos(hoekgraad);          // Everytime the x-value from encoder calculated
+double p_old_y = (translatie+L0)*sin(hoekgraad);          // Everytime the y-value from encoder calculated
 
-//BiQuad LPL( b0LP, b1LP, b2LP, a1LP, a2LP ); //Lowpass filter Biquad
-BiQuad HPL( b0HP, b1HP, b2HP, a1HP, a2HP ); //Highpass filter Biquad
-BiQuad NOL( b0NO, b1NO, b2NO, a1NO, a2NO ); //Notch filter Biquad 
-
-//BiQuad LPS( b0LP, b1LP, b2LP, a1LP, a2LP ); //Lowpass filter Biquad
-BiQuad HPS( b0HP, b1HP, b2HP, a1HP, a2HP ); //Highpass filter Biquad
-BiQuad NOS( b0NO, b1NO, b2NO, a1NO, a2NO ); //Notch filter Biquad 
+double J_inv_1_1 = -sin(hoekgraad)/translatie;
+double J_inv_1_2 = cos(hoekgraad)/translatie;
+double J_inv_2_1 = cos(hoekgraad);
+double J_inv_2_2 = sin(hoekgraad);
 
 
-void emgSample() {                                                              // EMG function
-    
-    double emgNOFilteredL = NOL.step(emgL.read());                              // Filtered NO value of EMG signal left biceps                
-    double emgHPFilteredL = HPL.step(emgNOFilteredL);                           // Filtered HP value of EMG signal left biceps
-    double emgabsL = fabs(emgHPFilteredL);                                      // Absolute value of EMG signal left biceps  
-      
-                                                             
-    double emgNOFilteredR = NOR.step(emgR.read());                              // Filtered NO value of EMG signal right biceps                
-    double emgHPFilteredR = HPR.step(emgNOFilteredR);                           // Filtered HP value of EMG signal right biceps
-    double emgabsR = fabs(emgHPFilteredR);                                      // Absolute value of EMG signal right biceps          
-    
-    
-    double emgNOFilteredS = NOS.step(emgS.read());                              // Filtered NO value of EMG signal servo spier                
-    double emgHPFilteredS = HPS.step(emgNOFilteredS);                           // Filtered HP value of EMG signal servo spier
-    double emgabsS = fabs(emgHPFilteredS);                                      // Absolute value of EMG signal servo spier
-    
-    
-    
-    
-//-------------------Linker bicep-------------------------------------    
+// Demo path: rectangular
+    double x_path[5];       // Matrix heeft 5 elementen: beginnend vanaf element 0 tot en met element 4
+    x_path[0] = L0;
+    x_path[1] = L0;
+    x_path[2] = L0+0.15;
+    x_path[3] = L0+0.15;
+    x_path[4] = x_path[0];
+
+    double y_path[5];
+    y_path[0] = 0.0;
+    y_path[1] = 0.1;
+    y_path[2] = 0.1;
+    y_path[3] = 0.0;
+    y_path[4] = y_path[0];
+
+// for loop
+for(int i=0 ; i<=4 ; i++)
+    {
+    double p_new_x = x_path[i];
+    double p_new_y = y_path[i];
     
-    for(int i = P-1; i >= 0; i--){                                              // For-loop used for moving average
-        if (i == 0) {
-            A[i] = emgabsL;
-            }
-         else {
-             A[i] = A[i-1];
-             }   
-    }
-    double sumL = 0;
-    for (int n = 0; n < P-1; n++) {                                             // Summation of array
-        sumL = sumL + A[n];
-    }
-    movagL = sumL/P;     
-    
-    
+    double p_dot_new_x = K_v * (p_new_x - p_old_x);
+    double p_dot_new_y = K_v * (p_new_y - p_old_y);
     
-//--------------Rechter bicep---------------------------------------
-    
-    for(int i = Q-1; i >= 0; i--){                                              // For-loop used for moving average
-        if (i == 0) {
-            B[i] = emgabsR;
-            }
-         else {
-             B[i] = B[i-1];
-             }   
-    }
-    double sumR = 0;
-
-    for (int n = 0; n < Q-1; n++) {                                             // Summation of array
-    sumR = sumR + B[n];
-    }
-    movagR = sumR/Q;                                                             // Moving average value right biceps
-    
-//---------------Servo spier ------------------------------------    
+   // printf("x=%f , y=%f , p_dot_new_x=%f , p_dot_new_y=%f\n",p_new_x,p_new_y,p_dot_new_x,p_dot_new_y);
     
-    for(int i = R-1; i >= 0; i--){                                              // For-loop used for moving average
-        if (i == 0) {
-            C[i] = emgabsS;
-            }
-         else {
-             C[i] = C[i-1];
-             }   
-    }
-    double sumS = 0;
-    for (int n = 0; n < R-1; n++) {                                             // Summation of array
-        sumS = sumS + C[n];
-    }
-    movagS = sumS/R;     
+    double angle_old = atan(p_old_y/p_old_x)*180/pi;
+    double L_old = sqrt(pow(p_old_x,2)+pow(p_old_y,2));
+    
+    double angle_new = atan(p_new_y/p_new_x)*180/pi;
+    double L_new = sqrt(pow(p_new_x,2)+pow(p_new_y,2));
+    
+    if (angle_new - angle_old) <= 0 
+        {
+        motor1direction.write(direction1 = !direction1); 
+        }       
     
-    /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */
-    //scope.set(0, emgL.read());
-//    scope.set(1, movagL);
-//    scope.set(2, emgR.read());
-//    scope.set(3, movagR);
-//    scope.set(4, emgS.read());
-//    scope.set(5, movagS);
-    /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels) 
-    *  Ensure that enough channels are available (HIDScope scope( 2 ))
-    *  Finally, send all channels to the PC at once */
-  //  scope.send();                                                     // Moving average value left biceps
-
-
-    if (CalButton==0 & k<200) {                                                 // Loop used for gaining max and zero value
-        Lvector[k] = movagL;
-        Rvector[k] = movagR;
-        Svector[k] = movagS;
-        
-        if (x==0){
-        ledB = !ledB;                                                           // SPIER NIET AANSPANNEN BIJ BLAUW
-        }                                                                       // SPIER WEL AANSPANNEN BIJ ROOD
-        else if (x==1){
-        ledR = !ledR;
-        }
-        k++;
-    }    
-    else if (k==200) {                                                          // End of the loop, used for calculation value
-        double sumY = 0;
-        double sumZ = 0;
-        double sumX = 0;
-            for (int n = 0; n < 199; n++) {                                     
-            sumY = sumY + Lvector[n];
-            sumZ = sumZ + Rvector[n];
-            sumX = sumX + Svector[n];
-            } 
-        Lwaarde[x] = sumY/200;                                                  // Value of zero for left biceps
-        Rwaarde[x] = sumZ/200;                                                  // Value of zero for right biceps
-        Swaarde[x] = sumX/200;                                                  // Value of zero for Servo spier
-        k++;
-        ledB = 1;
-        ledR = 1;
-        ledG = !ledG;
-    }
-    else if (k == 201 && zeromax ==0) {                                           // Loop used for switching between zero and max
-        ledG = !ledG;
-        k = 0;
-        if (x==0) {
-            x++;
-        }
-        else if (x==1) {
-            x=0;
+      if (L_new - L_old) <= 0 
+        {
+        motor2direction.write(direction2 = !direction2);        
+        } 
+    
+        while ( (fabs(p_new_x - p_old_x)) > 0.005 || (fabs(p_new_y - p_old_y)) > 0.005 )
+        {
+            double q_dot_angle = J_inv_1_1 * p_dot_new_x + J_inv_1_2 * p_dot_new_y;     //hoekgraad1
+            double q_dot_L = J_inv_2_1 * p_dot_new_x + J_inv_2_2 * p_dot_new_y;         //translatie
+            double q_dot_q2 = q_dot_L/radiuspulley;                                     //hoekgraad2 (translatie)
+            motor1control.write(q_dot_angle);
+            wait(0.1);
+            motor2control.write(q_dot_q2);
+            wait(0.1); // %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Berekening niet tegelijk, eventuele fout? %%%
         }
     }
-    if (x==1)                                                                   // Determining threshold using zero and max
-    {
-        thresholdL = Lwaarde[0]+(Lwaarde[1]-Lwaarde[0])*(0.25f);
-        thresholdR = Rwaarde[0]+(Rwaarde[1]-Rwaarde[0])*(0.25f);
-        thresholdS = Swaarde[0]+(Swaarde[1]-Swaarde[0])*(0.25f);
-    }
-}
-//----------------Buttons to change direction motors----------------------------
-void onButtonPress1() {
-    // reverses the direction
-    motor1direction.write(direction1 = !direction1);
-    //pc.printf("direction: %s\r\n\n", direction ? "clockwise" : "counter clockwise");
-}
-
-void onButtonPress2() {
-    // reverses the direction
-    motor2direction.write(direction2 = !direction2);
-    //pc.printf("direction: %s\r\n\n", direction ? "clockwise" : "counter clockwise");
-}
-
-int main()
-{  
-ledB = 1;
-ledG = 1;
-ledR = 1;
-emgSampleTicker.attach( &emgSample, 0.002 );                                    // Ticker for EMG function
-Lwaarde[0] = 0.01;                                                              // Startvalue for zerovalue, to - 
-Rwaarde[0] = 0.01;                                                              // - prevent dividing by 0
-Swaarde[0] = 0.01;
-
-    button1.fall(&onButtonPress1);
-    button2.fall(&onButtonPress2);
-    
-    while(1) {
-        
-            
-            
-        if (movagL > thresholdL)
-        {   motor1control.write(0.12);
-            wait(0.1);
-            }
-        else {
-            motor1control.write(0);
-            wait(0.1);
-            }
-            
-        if (movagR > thresholdR)
-        {   motor2control.write(0.8);
-             wait(0.1);
-            }
-        else {
-            motor2control.write(0);
-            wait(0.1);
-            }
-            
-            
-        if (movagS > thresholdS)
-        {   myservo = 0.5;
-            wait(0.01);
-        }
-        else {
-            myservo = 0.0;
-            wait(0.01);
-            }
-    }
 }
\ No newline at end of file