Diff: main.cpp

Revision:

0:545f4726720c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Oct 15 08:23:49 2018 +0000
@@ -0,0 +1,74 @@
+#include "mbed.h"
+#include "MODSERIAL.h"
+#include "QEI.h"
+#include "Butterworth.h"
+
+PwmOut pwmpin(D5);
+DigitalOut directionpin(D4);
+AnalogIn potmeter(A2);
+
+AnalogIn    emg0( A0 );
+AnalogIn    emg1( A1 );
+
+MODSERIAL pc(USBTX, USBRX);
+QEI wheel (D13, D12, NC, 32);
+
+float angle_resolution = (360.0/32.0)*(1.0/131.0);  //degrees/counts 
+
+float mean(float *samples, int n) {
+    float sum = 0.0;
+    for (int i=0; i<n; i++) {
+        sum += samples[i];
+    }
+    return sum / (float)n;
+}
+
+int main()
+{   
+    pc.baud(115200);
+    
+    pwmpin.period_us(60);
+    int pulses = 0;
+    float angle;
+    int n = 1;
+    int i = 0;
+    float meanmuscle;
+    float u[500];
+    float dxmax = 0.1;
+    
+    while (true) {
+        u[i] = emg0.read();
+        
+        pulses = wheel.getPulses();
+        angle = pulses*angle_resolution;      
+        if(n%500 == 0){
+            pc.printf("Angle is: %f degrees \r\n", meanmuscle);
+            
+            // center signal around 0
+            u = u - mean(u);
+            // here we have to filter the signal and take absolute value
+            //
+            // now take mean of filtered signal and convert to a dx
+            y = mean(u);
+            if(y < 0.02){
+                dx = 0;
+            }
+            else if(y > 0.2){
+                dx = 1
+            }
+            else{
+                dx=(y - 0.02)/0.18*dxmax;
+            }
+            // 0.02 and 0.2 obtained from analysing filtered output
+            // use dx for inverse kinematics combined with current x to obtain
+            // desired positiion. this will be thetaref     
+            meanmuscle = (mean(u,500)-0.1)*4;
+            directionpin = meanmuscle > 0.0f;
+            pwmpin = fabs(meanmuscle);
+            i=0;
+        }
+        n++;    
+        i++;
+        wait(0.001);     
+    }
+}
\ No newline at end of file