Diff: main.cpp

Revision:

13:ec4708dab45d

Parent:

12:7f280a661e71

Child:

14:dc89250ebc52

--- a/main.cpp	Thu Oct 03 15:08:32 2019 +0000
+++ b/main.cpp	Fri Oct 11 12:58:49 2019 +0000
@@ -1,33 +1,87 @@
 #include "mbed.h"
-//#include "HIDScope.h"
+#include "HIDScope.h"
 //#include "QEI.h"
 #include "MODSERIAL.h"
 //#include "BiQuad.h"
 #include "FastPWM.h"
 #include <iostream>
 MODSERIAL pc(USBTX, USBRX);
-AnalogIn ain(A0);
-DigitalOut dir(D4);
+AnalogIn ain2(A2);
+AnalogIn ain1(A3);
+DigitalOut dir2(D4);
+DigitalOut dir1(D7);
 //DigitalOut pwm(D5);
 
 
 Ticker ticktick;
-PwmOut motor1_pwm(D5);
+//D4,D7 direction of motors 2,1 on board, D5,D6- PWM of motors 2,1 on board
+PwmOut motor2_pwm(D5);
+PwmOut motor1_pwm(D6);
+AnalogIn    emg0( A0 );
+AnalogIn    emg1( A1 );
+
+Ticker      sample_timer;
+HIDScope    scope( 2 );
+DigitalOut  led(LED1);
+volatile float P;
 
+/** Sample function
+ * this function samples the emg and sends it to HIDScope
+ **/
+void sample()
+{
+    static int count=0;
+    static float RMS_value=0;
+    static float HighPass_value=0;
+    count+=1;
+    static float RMS[150];
+    static float HighPass[30];
+    float I1;
+    float If;
+    I1=emg0.read(); //read signal
+    HighPass_value+=(I1-HighPass[count%30])/30.0;
+    HighPass[count%30]=I1;
+    If=pow(I1-HighPass_value,2.0f); // Highpass-filtered value squared
+    RMS_value+=(If-RMS[count%150])/150.0;
+    RMS[count%150]=If;
+    /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */
+    P=sqrt(RMS_value);
+    scope.set(0, P ); // send root mean squared
+    scope.set(1, emg0.read() );
+    /* 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 */
+    led = !led;
+}
 void setPWM(void)
 {
-   float rd=ain.read();
-   motor1_pwm.write(rd);
+   float Q;
+   if (7*P>1)
+   {
+       Q=1.0;
+    }
+    else
+    {
+       Q=7*P;
+    }
+   motor1_pwm.write(Q);
+   motor2_pwm.write(ain1.read());
 }
 
 int main()
 {
+    sample_timer.attach(&sample, 0.002);
     ticktick.attach(setPWM,0.1);
     int frequency_pwm=10000;
     motor1_pwm.period(1.0/frequency_pwm);
+    motor2_pwm.period(1.0/frequency_pwm);
     pc.printf("Starting...");
     
     while (true) {
         wait(10);
+        dir1=!dir1;
+        dir2=!dir2;
     }
 }