Revision 4:afa85283eb18, committed 2017-11-01

Comitter:

DiondeGreef

Date:

Wed Nov 01 11:50:05 2017 +0000

Parent:

3:2ffbee47fb38

Commit message:

afdd

Changed in this revision

diff -r 2ffbee47fb38 -r afa85283eb18 main.cpp
--- a/main.cpp	Tue Oct 31 21:50:46 2017 +0000
+++ b/main.cpp	Wed Nov 01 11:50:05 2017 +0000
@@ -24,8 +24,10 @@
 
 DigitalOut motorDirection(D4);
 PwmOut motorSpeed(D5);
+DigitalOut motorDirection1(D4); //nog goede channel toevoegen
+PwmOut motorSpeed1(D5);         //nog goede channel toevoegen
 
-MODSERIAL pc(USBTX, USBRX);
+MODSERIAL   pc(USBTX, USBRX);
 
 Ticker      sample_timer;
 Ticker      cal_timer;
@@ -34,16 +36,19 @@
 DigitalOut  led2(LED_GREEN);                  
 const int size = 100;
 vector<double> S(size,0);
+vector<double> S1(size,0);
 double meanSum = 0;
 
 double maxsignal = 0;
+double emgX = 0;
+double emgY = 0;
 
 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 q1 = encoder1.getPosition()/131.25; //Calibreren nog toevoegen
+double q2 = encoder2.getPosition()/131.25; //Calibreren mist nog
+double Periode = 1/500;   //1000 is het aantal Hz
 
 //Filter toevoegen, bestaande uit notch, high- en lowpass filter
 BiQuadChain Notch;
@@ -85,6 +90,16 @@
 
 //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);
+
+//Making matrices globaly     
+Matrix      JAPPAPP(2,2);
+Matrix      qdot(2,1);
+Matrix      Vdes(2,1);
+Matrix      qsetpoint(2,1);     
+     
+     
+
+
      
 double findRMS(vector<double> array)
 {
@@ -121,13 +136,17 @@
         }
     */
     S.erase(S.begin());
+    S1.erase(S1.begin());
     double b = bqc.step(emg0.read()-0.4542);
+    double c = bqc.step(emg1.read()-0.4542);
     S.push_back(b);
+    S1.push_back(c);
     //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]);
+    emgX = findRMS(S);
+    emgY = findRMS(S1);
+    scope.set(1, emgX);
+    //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 ))
@@ -136,7 +155,7 @@
     /* To indicate that the function is working, the LED is toggled */
     led = !led;
     
-    motorSpeed.write(0.38080*(a/maxsignal));
+    motorSpeed.write(0.38080*(emgX/maxsignal));
     motorDirection.write(1);
 }
 
@@ -156,7 +175,6 @@
                 double signalfinal = findRMS(S);
             if (signalfinal >= maxsignal){
                 maxsignal = signalfinal; //keep changing the maximal signal
-                pc.baud(9600);
                 pc.printf("%d \n",maxsignal);
             }
         }
@@ -164,8 +182,27 @@
     }
 }
 
+
+void qSetpointSet(){
+        // 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;    
+}
+
 int main()
 {   
+pc.baud(115200);
     //Constructing the notch filter chain
     Notch.add( &bqNotch1 ).add( &bqNotch2 ).add( &bqNotch3 );
     Notch50.add( &bq1 ).add( &bq2 ).add( &bq3 );
@@ -174,41 +211,19 @@
     /**Attach the 'sample' function to the timer 'sample_timer'.
     * this ensures that 'sample' is executed every... 0.001 seconds = 1000 Hz
     */
-    sample_timer.attach(&sample, 0.002);
+    sample_timer.attach(&sample, Periode);
     cal_timer.attach(&calibration, 0.002);//ticker to check if motor is being calibrated
     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);
-
-    // 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) {
-    
-    
+    qdot.print();                    //Alleen visualisatie
+    wait(0.5);  
     }
 
 }
\ No newline at end of file