Diff: main.cpp

Revision:

0:3710031b2621

Child:

1:a9c933f1dc71

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Oct 19 10:37:25 2018 +0000
@@ -0,0 +1,229 @@
+#include "mbed.h"
+#include "BiQuad.h"
+#include "HIDScope.h"
+#include <sys/time.h>
+
+class Timer
+{
+private:
+    struct timeval start_t;
+public:
+    double start() { gettimeofday(&start_t, NULL); }
+    double get_ms() {
+       struct timeval now;
+       gettimeofday(&now, NULL);
+       return (now.tv_usec-start_t.tv_usec)/(double)1000.0 +
+              (now.tv_sec-start_t.tv_sec)*(double)1000.0;
+    }
+    double get_ms_reset() {
+      double res = get_ms();
+      reset();
+      return res;
+    }
+    Timer() { start(); }
+};
+
+Timer t();
+double used_ms;
+Serial pc(USBTX,USBRX);// serial connection to pc
+
+DigitalOut led_red(LED_RED);
+DigitalOut led_green(LED_RED);
+DigitalIn buttonR(D2);//rigth button on biorobotics shield
+DigitalIn buttonL(D3);//rigth button on biorobotics shield
+
+DigitalOut motor1_direction(D4);// draairichting motor 1 (1 is CW encoder als je daar op kijkt en CW shaft als je daar op kijkt)
+PwmOut motor1_speed_control(D5);//aanstuursnelheid motor 1
+
+DigitalOut motor2_direction(D7);// draairichting motor 2 (1 is CW encoder als je daar op kijkt en CW shaft als je daar op kijkt)
+PwmOut motor2_speed_control(D6);//aanstuursnelheid motor 2
+
+enum States {failure, wait, calib_emg, operational, demo};
+enum Operations {rest, forward, backward, up, down};
+
+States current_state;
+Operations movement = rest;
+
+float max1 = 0.3; //initial threshold value for emg signals, changes during calibration
+float max2 = 0.3;
+
+Ticker      sample_timer;
+Ticker      loop_timer;
+
+HIDScope    scope( 3 );
+
+AnalogIn raw_emg1_input(A0);//input for first emg signal 1, for the modes
+AnalogIn raw_emg2_input(A1);//input for first emg signal 2, for the strength
+
+volatile float raw_filteredsignal1;//the first filtered emg signal 1
+volatile float raw_filteredsignal2;//the first filtered emg signal 2
+
+volatile float filteredsignal1;//the first filtered emg signal 1
+volatile float filteredsignal2;//the first filtered emg signal 2
+
+bool state_changed = false;
+
+measureall(){ // changes all variables according in sync with the rest of the code
+    
+    emg1_input = emg1_input.read();
+    emg2_input = emg2_input.read();
+    filterall();
+    filteredsignal1 = raw_filteredsignal1;
+    filteredsignal2 = raw_filteredsignal2;
+    //Reading of motor
+    
+    
+    
+}
+
+
+void filterall()
+{
+    //Highpass Biquad 5 Hz 
+static BiQuad HighPass(0.95653708,  -1.91307417, 0.95653708, -1.91118480, 0.91496354);
+float high1 = HighPass.step(raw_emg1_input);
+float high2 = HighPass.step(raw_emg2_input);
+    // Rectify the signal(absolute value)
+float abs1 = fabs(high1);
+float abs2 = fabs(high2);
+    //Lowpass Biquad 10 Hz
+static BiQuad LowPass(0.00362164,  0.00724327, 0.00362164, -1.82267251, 0.83715906);
+float low1 = LowPass.step(abs1);
+float low2 = LowPass.step(abs2);
+
+raw_filteredsignal1 = low1;
+raw_filteredsignal2 = low2;
+
+}
+
+void scopedata()
+{
+    scope.set(0,emg1_input); // 
+    scope.set(1,filteredsignal1); //    
+    scope.set(2,filteredsignal2); //
+    scope.send(); // send info to HIDScope server
+}
+
+void loop_function() {
+    measureall();    
+    switch(current_state) {
+    case failure:
+        do_state_failure(); // a separate function for what happens in each state
+        break;
+    case calib_emg:
+        do_state_calib_emg();
+        break;
+    case operational:
+        do_state_operational();
+    break;
+    case wait;
+        do_state_wait();
+        break;  
+}
+motor_controller();
+scopedata(); // Outputs data to the computer
+}
+
+do_state_failure(){    
+    //al motor movement to zero!
+    wait(1000);    
+    };
+
+do_state_calib_emg(){    
+    if (state_changed==true) {
+        state_changed = false;        
+    }
+    if(filteredsignal1 > max1){//calibrate to a new maximum
+       max1 = filteredsignal1;
+       }
+    if(filteredsignal2 > max2){//calibrate to a new maximum
+       max2 = filteredsignal2;
+       }
+       
+    if (filteredsignal1 > (0.75*max1) && filteredsignal2 > (0.75*max2)) {
+        current_state = operational;
+        used_ms = t.get_ms_reset();
+        state_changed = true;
+    }    
+}
+
+do_state_operational(){
+     if (state_changed==true) {
+        state_changed = false;        
+    }
+    
+    switch(States) {// a separate function for what happens in each state
+    case rest:
+        if (filteredsignal2 > (0.6*max))) {// 
+        current_state = wait;
+        state_changed = true;
+    }              
+        if( t.get_ms() > 1000 && filteredsignal1 > (0.6max))
+        {
+            States = forward;
+            used_ms = t.get_ms_reset();
+            }
+        break;
+    case forward:
+        do_forward();
+         if( t.get_ms() > 1000 && filteredsignal1 > (0.6max))
+        {
+            States = backward;
+            used_ms = t.get_ms_reset();
+            }
+        break;
+    case backward:
+        do_backward();
+         if( t.get_ms() > 1000 && filteredsignal1 > (0.6max))
+        {
+            States = up;
+            used_ms = t.get_ms_reset();
+            }
+        break;    
+    case up:
+        do_up();
+         if( t.get_ms() > 1000 && filteredsignal1 > (0.6max))
+        {
+            States = wait;
+            used_ms = t.get_ms_reset();
+            }
+        break;
+    case down:
+        do_down();
+        if( t.get_ms() > 1000 && filteredsignal1 > (0.6max))
+        {
+            States = wait;
+            used_ms = t.get_ms_reset();
+            }
+        break;    
+    }
+       
+    
+}
+
+do_state_wait(){
+    if (state_changed==true) {
+        state_changed = false;        
+    }   
+       
+    if (filteredsignal1 > (0.75*max1) && filteredsignal2 > (0.75*max2) {
+        current_state = operational;
+        state_changed = true;
+    } 
+}
+
+
+
+int main()
+{      
+       
+    loop_timer.attach(&loop_function, 0.002);
+    
+    pc.baud(115200);
+    
+    while (true) {         
+    if(buttonR == true){
+        current_state = failure; 
+        }
+    }
+}
\ No newline at end of file