Diff: main.cpp

Revision:

4:dfed3b98ffb1

Parent:

3:2ffbee47fb38

Child:

5:52badb8ee317

--- a/main.cpp	Tue Oct 31 21:50:46 2017 +0000
+++ b/main.cpp	Wed Nov 01 15:08:27 2017 +0000
@@ -1,9 +1,9 @@
- /**
- * Demo program for BiQuad and BiQuadChain classes
- * author: T.J.W. Lankhorst <t.j.w.lankhorst@student.utwente.nl> and Matthijs and Roy and Dion
- */
+/**
+* Demo program for BiQuad and BiQuadChain classes
+* author: T.J.W. Lankhorst <t.j.w.lankhorst@student.utwente.nl> and Matthijs and Roy and Dion
+*/
 #include "mbed.h"
-#include "HIDScope.h"
+//#include "HIDScope.h"
 #include <stdlib.h>
 #include <iostream>
 #include <iomanip>
@@ -21,6 +21,8 @@
 Encoder     encoder1(D13,D12);
 Encoder     encoder2(D13,D12); //moet nog even aangepast worden
 
+AnalogIn    potMeterIn1(A1);
+
 
 DigitalOut motorDirection(D4);
 PwmOut motorSpeed(D5);
@@ -29,21 +31,49 @@
 
 Ticker      sample_timer;
 Ticker      cal_timer;
-HIDScope    scope( 4 );
+Ticker      setpoint_timer;
+Ticker      m1_Ticker;
+Ticker      SP_m1_Ticker;
+//HIDScope    scope( 3 );
 DigitalOut  led(LED1);
-DigitalOut  led2(LED_GREEN);                  
+DigitalOut  led2(LED_GREEN);
 const int size = 100;
 vector<double> S(size,0);
 double meanSum = 0;
 
 double maxsignal = 0;
 
+double emgX = 0;
+double emgY = 1;
+
 double L0 = 0.123;
 double L1 = 0.37;
 double L2 = 0.41;
 double q1 = encoder1.getPosition()/131; //Calibreren nog toevoegen
 double q2 = encoder2.getPosition()/131; //Calibreren mist nog
-double Periode = 1/1000;   //1000 is het aantal Hz
+double Periode = 0.002;   //1000 is het aantal Hz
+
+
+float M1_KP = 2.5;
+float M1_KI = 1.0;
+const float M1_TS = 0.01;
+const float SP_TS = 0.01;
+const float RAD_PER_PULSE = 0.000119;
+float m1_err_int = 0;
+int motorD = 0;
+float motor1 = 0;
+float reference = 0;
+float position = 0;
+int outOfEncod = 0;
+int KP_changer = 1;
+float RotSpeed = 0;
+
+
+//Making matrices globaly
+Matrix      JAPPAPP(2,2);
+Matrix      qdot(2,1);
+Matrix      Vdes(2,1);
+Matrix      qsetpoint(2,1);
 
 //Filter toevoegen, bestaande uit notch, high- en lowpass filter
 BiQuadChain Notch;
@@ -57,26 +87,15 @@
 BiQuad bqNotch2( 1.00000e+00, -1.90228e+00, 1.00004e+00, -1.88308e+00, 9.87097e-01 );
 BiQuad bqNotch3( 1.00000e+00, -1.90219e+00, 9.99925e-01, -1.89724e+00, 9.87929e-01 );
 
-//BiQuad Notch(0.9179504645111498,-1.4852750515677946,  0.9179504645111498, -1.4852750515677946, 0.8359009290222995);
 
 //Notch filter chain for 50 Hz
 BiQuad bq1( 9.50972e-01, -1.53921e+00, 9.51003e-01, -1.57886e+00, 9.50957e-01 );
 BiQuad bq2( 1.00000e+00, -1.61851e+00, 9.99979e-01, -1.57185e+00, 9.74451e-01 );
 BiQuad bq3( 1.00000e+00, -1.61855e+00, 9.99988e-01, -1.62358e+00, 9.75921e-01 );
 
-//15 Hz Highpass filter, as recommended by multiple studies regarding EMG signals
-//BiQuad Highpass15(9.35527e-01, -1.87105e+00, 9.35527e-01, -1.86689e+00, 8.75215e-01);
-
 //2Hz Highpass
 BiQuad Highpass1(9.82385e-01, -1.96477e+00, 9.82385e-01, -1.96446e+00, 9.65081e-01);
 
-
-//20 Hz Lowpass
-//BiQuad Lowpass100( 3.62168e-03, 7.24336e-03, 3.62168e-03, -1.82269e+00, 8.37182e-01 );
-
-//250 Hz Lowpass
-//BiQuad Lowpass100 (2.92893e-01, 5.85786e-01, 2.92893e-01, -3.60822e-16, 1.71573e-01 );
-
 //80 Hz Lowpass
 BiQuad bqLP1( 5.78904e-02, 5.78898e-02, 0.00000e+00, -2.90527e-01, 0.00000e+00 );
 BiQuad bqLP2( 1.00000e+00, 2.00001e+00, 1.00001e+00, -7.53537e-01, 4.06308e-01 );
@@ -85,87 +104,98 @@
 
 //450 Hz Lowpass filter, to remove any noise (sample frequency is only 500 Hz, but still...)
 //BiQuad Lowpass450(8.00592e-01, 1.60118e+00, 8.00592e-01, 1.56102e+00, 6.41352e-01);
-     
+
+float PI( float e, const float Kp, const float Ki, float Ts, float &e_int )
+{
+    e_int += Ts * e;                                                                // e_int is changed globally because it’s ’by reference’ (&)
+    if (fabs(Kp * e + Ki * e_int) < 1) {
+        return fabs(Kp * e + Ki * e_int);
+    } else {
+        return 1;
+    }
+}
+// Next task, measure the error and apply the output to the plant
+void m1_Controller()
+{
+    reference = 5 * potMeterIn1.read();
+    //reference = qsetpoint(1,1);
+    outOfEncod = encoder1.getPosition();
+    position = RAD_PER_PULSE * outOfEncod;
+    float ref_pos = reference - position;                                          // Don’t use magic numbers!
+    //motor1 = PI(ref_pos, M1_KP, M1_KI, M1_TS, m1_err_int );
+
+    if (ref_pos <= 0) {                                                             // Don’t use magic numbers!
+        motor1 = PI(ref_pos, M1_KP, M1_KI, M1_TS, m1_err_int );
+    } else {
+        motor1 = PI(ref_pos, M1_KP, M1_KI, M1_TS, m1_err_int );
+    }
+    if (ref_pos <= 0) {
+        //float motor1DirectionPin1 = 1;
+        motorD=1;
+    } else {
+        //float motor1DirectionPin1 = 0;
+        motorD=0;
+    }
+    motorDirection.write(motorD);
+    motorSpeed.write(motor1);
+}
+
+
 double findRMS(vector<double> array)
 {
-     int i;
-     double sumsquared = 0.000;
-     double RMS;
-     //sumsquared = 0;
-     for (i = 0; i < size; i++)
-     {
+    int i;
+    double sumsquared = 0.000;
+    double RMS;
+    //sumsquared = 0;
+    for (i = 0; i < size; i++) {
         sumsquared = sumsquared + array.at(i)*array.at(i);
-     }
-     RMS = sqrt((double(1)/size)*(sumsquared)); 
-     return RMS;
+    }
+    RMS = sqrt((double(1)/size)*(sumsquared));
+    return RMS;
 }
-     
-     
-     
+
+
+
 void sample()
 {
-    /* 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, emg0.read() );
-    //scope.set(1, fabs(Notch.step(emg0.read())) );
-    scope.set(0, emg0.read()-0.4542);
-    //scope.set(1, bqcLP.step(Highpass1.step(emg0.read()-0.4542)));
-    //scope.set(2, fabs(Notch50.step(bqcLP.step(Highpass1.step(emg0.read()-0.4542)))));
-    //scope.set(3, fabs(bqc.step(emg0.read()-0.4542)));
-    //scope.set(3, Notch.step(Notch50.step(bqcLP.step(Highpass1.step(fabs(emg0.read()-0.4542))))));
-    
-    
-    //scope.set(0, fabs(Highpass1.step(Lowpass100.step(Notch.step(emg0.read())))) ); 
-    /*
-    for (int i = size-1; i > 1; i--) {
-            S[i] = S[i-1];    
-        }
-    */
     S.erase(S.begin());
     double b = bqc.step(emg0.read()-0.4542);
     S.push_back(b);
-    //S[0] = bqc.step(emg0.read()-0.4542)));
-    //Notch50.step(bqcLP.step(Highpass1.step(emg0.read()-0.4542)));
-    double a = findRMS(S);
-    scope.set(1, a);
-    scope.set(2, S[0]);
-    //meanSum = 0; */
-    /* 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();
-    /* To indicate that the function is working, the LED is toggled */
+    emgX = findRMS(S);
+
     led = !led;
-    
-    motorSpeed.write(0.38080*(a/maxsignal));
-    motorDirection.write(1);
+
+    //motorSpeed.write(0.38080*(emgX/maxsignal));
+    //motorDirection.write(1);
 }
 
-void calibration()
+
+void qSetpointSet()
 {
-    //function to determine maximal EMG input in order to allow motorcontrol to be activated
-    
-    if (button1.read() == false){
-        led2 = !led2;
-        for (int n = 1; n < 5000; n++){ //calibrate for 5000 samples or 10 seconds
-            
-                S.erase(S.begin());
-                double b = bqc.step(emg0.read()-0.4542);
-                S.push_back(b);
-                //S[0] = bqc.step(emg0.read()-0.4542)));
-                //Notch50.step(bqcLP.step(Highpass1.step(emg0.read()-0.4542)));
-                double signalfinal = findRMS(S);
-            if (signalfinal >= maxsignal){
-                maxsignal = signalfinal; //keep changing the maximal signal
-                pc.baud(9600);
-                pc.printf("%d \n",maxsignal);
-            }
-        }
-        led2 = 1;       
-    }
+    // Fill Matrix with data.
+    JAPPAPP(1,1) =  -(L0 - L0*sin(q1) + L1*sin(q1) - L2*sin(q2))/(L1*L1*cos(q1)*sin(q1) + L0*L1*cos(q1) + L0*L2*cos(q2) - L0*L1*cos(q1)*sin(q1) - L0*L2*cos(q2)*sin(q1) - L1*L2*cos(q1)*sin(q2));
+    JAPPAPP(1,2) = -(L2*cos(q2))/(L1*L1*cos(q1)*sin(q1) + L0*L1*cos(q1) + L0*L2*cos(q2) - L0*L1*cos(q1)*sin(q1) - L0*L2*cos(q2)*sin(q1) - L1*L2*cos(q1)*sin(q2));
+    JAPPAPP(2,1) = (L1*sin(q1) - L2*sin(q2))/(L1*L1*cos(q1)*sin(q1) + L0*L1*cos(q1) + L0*L2*cos(q2) - L0*L1*cos(q1)*sin(q1) - L0*L2*cos(q2)*sin(q1) - L1*L2*cos(q1)*sin(q2));
+    JAPPAPP(2,2) = (L1*cos(q1) + L2*cos(q2))/(L1*L1*cos(q1)*sin(q1) + L0*L1*cos(q1) + L0*L2*cos(q2) - L0*L1*cos(q1)*sin(q1) - L0*L2*cos(q2)*sin(q1) - L1*L2*cos(q1)*sin(q2));
+
+    // Fill Matrix with data.
+    Vdes(1,1) = emgX;        //Goede code nog toevoegen, Vx
+    Vdes(2,1) = emgY;        //goede code nog toevoegen, Vy
+
+    qdot = JAPPAPP*Vdes;
+
+    qsetpoint(1,1) = q1 + qdot(1,1)*Periode;
+    qsetpoint(2,1) = q2 + qdot(2,1)*Periode;
+
+    //motorSpeed.write(qsetpoint(1,1));
+    //motorDirection.write(1);
 }
 
+
 int main()
-{   
+{
+    pc.baud(115200);
+
     //Constructing the notch filter chain
     Notch.add( &bqNotch1 ).add( &bqNotch2 ).add( &bqNotch3 );
     Notch50.add( &bq1 ).add( &bq2 ).add( &bq3 );
@@ -175,40 +205,22 @@
     * this ensures that 'sample' is executed every... 0.001 seconds = 1000 Hz
     */
     sample_timer.attach(&sample, 0.002);
-    cal_timer.attach(&calibration, 0.002);//ticker to check if motor is being calibrated
+    setpoint_timer.attach(&qSetpointSet, 0.002);
     led2 = 1;
     /*empty loop, sample() is executed periodically*/
-    
-    pc.baud(9600);
-    DigitalOut myled(LED1);
- 
-//---
-    Matrix JAPPAPP(2,2);
-    Matrix qdot(2,1);
-    Matrix Vdes(2,1);
-    Matrix qsetpoint(2,1);
+    m1_Ticker.attach( &m1_Controller, M1_TS );
+
+    m1_Controller();
 
-    // Fill Matrix with data.
-    JAPPAPP     << -(L0 - L0*sin(q1) + L1*sin(q1) - L2*sin(q2))/(L1*L1*cos(q1)*sin(q1) + L0*L1*cos(q1) + L0*L2*cos(q2) - L0*L1*cos(q1)*sin(q1) - L0*L2*cos(q2)*sin(q1) - L1*L2*cos(q1)*sin(q2))  << -(L2*cos(q2))/(L1*L1*cos(q1)*sin(q1) + L0*L1*cos(q1) + L0*L2*cos(q2) - L0*L1*cos(q1)*sin(q1) - L0*L2*cos(q2)*sin(q1) - L1*L2*cos(q1)*sin(q2))
-                << (L1*sin(q1) - L2*sin(q2))/(L1*L1*cos(q1)*sin(q1) + L0*L1*cos(q1) + L0*L2*cos(q2) - L0*L1*cos(q1)*sin(q1) - L0*L2*cos(q2)*sin(q1) - L1*L2*cos(q1)*sin(q2))                     << (L1*cos(q1) + L2*cos(q2))/(L1*L1*cos(q1)*sin(q1) + L0*L1*cos(q1) + L0*L2*cos(q2) - L0*L1*cos(q1)*sin(q1) - L0*L2*cos(q2)*sin(q1) - L1*L2*cos(q1)*sin(q2));
-             
-             
-    // Fill Matrix with data.
-    Vdes     << emg0.read        //Goede code nog toevoegen, Vx
-             << emg1.read;      //goede code nog toevoegen, Vy
-    
-    
-    qdot = JAPPAPP*Vdes;
-    pc.printf("\r\nJAPPAPPP: ");     //Alleen visualisatie
-    qdot.print();                    //Alleen visualisatie
-    
-    qsetpoint = q1 + qdot*Periode;
-    
-    
-    
     while(true) {
-    
-    
+        pc.printf("%f \t",emgX);
+        pc.printf("%f \t",qdot(1,1));
+        pc.printf("%0.8f \t",qsetpoint(1,1));
+        pc.printf("%f \t",encoder1.getPosition());
+        pc.printf("%f \t",motorD);
+        pc.printf("%f \t",emgX);
+        pc.printf("%f \r\n",reference);
+
     }
 
 }
\ No newline at end of file