juijiu
Dependencies: HIDScope QEI biquadFilter mbed
Fork of MotorArchitecture1-11 by
main.cpp
- Committer:
- WouterJS
- Date:
- 2018-10-19
- Revision:
- 0:3710031b2621
- Child:
- 1:a9c933f1dc71
File content as of revision 0:3710031b2621:
#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; } } }