Thijs Lubberman / Mbed 2 deprecated MotorArchitecture1-11

juijiu

Dependencies:   HIDScope QEI biquadFilter mbed

Fork of MotorArchitecture1-11 by Wouter Schuttert

main.cpp@0:3710031b2621, 2018-10-19 (annotated)

Committer:
WouterJS
Date:
Fri Oct 19 10:37:25 2018 +0000
Revision:
0:3710031b2621
Child:
1:a9c933f1dc71
Code for 2 link robotic arm, emg controlled; ; ;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
WouterJS 0:3710031b2621 1 #include "mbed.h"
WouterJS 0:3710031b2621 2 #include "BiQuad.h"
WouterJS 0:3710031b2621 3 #include "HIDScope.h"
WouterJS 0:3710031b2621 4 #include <sys/time.h>
WouterJS 0:3710031b2621 5
WouterJS 0:3710031b2621 6 class Timer
WouterJS 0:3710031b2621 7 {
WouterJS 0:3710031b2621 8 private:
WouterJS 0:3710031b2621 9 struct timeval start_t;
WouterJS 0:3710031b2621 10 public:
WouterJS 0:3710031b2621 11 double start() { gettimeofday(&start_t, NULL); }
WouterJS 0:3710031b2621 12 double get_ms() {
WouterJS 0:3710031b2621 13 struct timeval now;
WouterJS 0:3710031b2621 14 gettimeofday(&now, NULL);
WouterJS 0:3710031b2621 15 return (now.tv_usec-start_t.tv_usec)/(double)1000.0 +
WouterJS 0:3710031b2621 16 (now.tv_sec-start_t.tv_sec)*(double)1000.0;
WouterJS 0:3710031b2621 17 }
WouterJS 0:3710031b2621 18 double get_ms_reset() {
WouterJS 0:3710031b2621 19 double res = get_ms();
WouterJS 0:3710031b2621 20 reset();
WouterJS 0:3710031b2621 21 return res;
WouterJS 0:3710031b2621 22 }
WouterJS 0:3710031b2621 23 Timer() { start(); }
WouterJS 0:3710031b2621 24 };
WouterJS 0:3710031b2621 25
WouterJS 0:3710031b2621 26 Timer t();
WouterJS 0:3710031b2621 27 double used_ms;
WouterJS 0:3710031b2621 28 Serial pc(USBTX,USBRX);// serial connection to pc
WouterJS 0:3710031b2621 29
WouterJS 0:3710031b2621 30 DigitalOut led_red(LED_RED);
WouterJS 0:3710031b2621 31 DigitalOut led_green(LED_RED);
WouterJS 0:3710031b2621 32 DigitalIn buttonR(D2);//rigth button on biorobotics shield
WouterJS 0:3710031b2621 33 DigitalIn buttonL(D3);//rigth button on biorobotics shield
WouterJS 0:3710031b2621 34
WouterJS 0:3710031b2621 35 DigitalOut motor1_direction(D4);// draairichting motor 1 (1 is CW encoder als je daar op kijkt en CW shaft als je daar op kijkt)
WouterJS 0:3710031b2621 36 PwmOut motor1_speed_control(D5);//aanstuursnelheid motor 1
WouterJS 0:3710031b2621 37
WouterJS 0:3710031b2621 38 DigitalOut motor2_direction(D7);// draairichting motor 2 (1 is CW encoder als je daar op kijkt en CW shaft als je daar op kijkt)
WouterJS 0:3710031b2621 39 PwmOut motor2_speed_control(D6);//aanstuursnelheid motor 2
WouterJS 0:3710031b2621 40
WouterJS 0:3710031b2621 41 enum States {failure, wait, calib_emg, operational, demo};
WouterJS 0:3710031b2621 42 enum Operations {rest, forward, backward, up, down};
WouterJS 0:3710031b2621 43
WouterJS 0:3710031b2621 44 States current_state;
WouterJS 0:3710031b2621 45 Operations movement = rest;
WouterJS 0:3710031b2621 46
WouterJS 0:3710031b2621 47 float max1 = 0.3; //initial threshold value for emg signals, changes during calibration
WouterJS 0:3710031b2621 48 float max2 = 0.3;
WouterJS 0:3710031b2621 49
WouterJS 0:3710031b2621 50 Ticker sample_timer;
WouterJS 0:3710031b2621 51 Ticker loop_timer;
WouterJS 0:3710031b2621 52
WouterJS 0:3710031b2621 53 HIDScope scope( 3 );
WouterJS 0:3710031b2621 54
WouterJS 0:3710031b2621 55 AnalogIn raw_emg1_input(A0);//input for first emg signal 1, for the modes
WouterJS 0:3710031b2621 56 AnalogIn raw_emg2_input(A1);//input for first emg signal 2, for the strength
WouterJS 0:3710031b2621 57
WouterJS 0:3710031b2621 58 volatile float raw_filteredsignal1;//the first filtered emg signal 1
WouterJS 0:3710031b2621 59 volatile float raw_filteredsignal2;//the first filtered emg signal 2
WouterJS 0:3710031b2621 60
WouterJS 0:3710031b2621 61 volatile float filteredsignal1;//the first filtered emg signal 1
WouterJS 0:3710031b2621 62 volatile float filteredsignal2;//the first filtered emg signal 2
WouterJS 0:3710031b2621 63
WouterJS 0:3710031b2621 64 bool state_changed = false;
WouterJS 0:3710031b2621 65
WouterJS 0:3710031b2621 66 measureall(){ // changes all variables according in sync with the rest of the code
WouterJS 0:3710031b2621 67
WouterJS 0:3710031b2621 68 emg1_input = emg1_input.read();
WouterJS 0:3710031b2621 69 emg2_input = emg2_input.read();
WouterJS 0:3710031b2621 70 filterall();
WouterJS 0:3710031b2621 71 filteredsignal1 = raw_filteredsignal1;
WouterJS 0:3710031b2621 72 filteredsignal2 = raw_filteredsignal2;
WouterJS 0:3710031b2621 73 //Reading of motor
WouterJS 0:3710031b2621 74
WouterJS 0:3710031b2621 75
WouterJS 0:3710031b2621 76
WouterJS 0:3710031b2621 77 }
WouterJS 0:3710031b2621 78
WouterJS 0:3710031b2621 79
WouterJS 0:3710031b2621 80 void filterall()
WouterJS 0:3710031b2621 81 {
WouterJS 0:3710031b2621 82 //Highpass Biquad 5 Hz
WouterJS 0:3710031b2621 83 static BiQuad HighPass(0.95653708, -1.91307417, 0.95653708, -1.91118480, 0.91496354);
WouterJS 0:3710031b2621 84 float high1 = HighPass.step(raw_emg1_input);
WouterJS 0:3710031b2621 85 float high2 = HighPass.step(raw_emg2_input);
WouterJS 0:3710031b2621 86 // Rectify the signal(absolute value)
WouterJS 0:3710031b2621 87 float abs1 = fabs(high1);
WouterJS 0:3710031b2621 88 float abs2 = fabs(high2);
WouterJS 0:3710031b2621 89 //Lowpass Biquad 10 Hz
WouterJS 0:3710031b2621 90 static BiQuad LowPass(0.00362164, 0.00724327, 0.00362164, -1.82267251, 0.83715906);
WouterJS 0:3710031b2621 91 float low1 = LowPass.step(abs1);
WouterJS 0:3710031b2621 92 float low2 = LowPass.step(abs2);
WouterJS 0:3710031b2621 93
WouterJS 0:3710031b2621 94 raw_filteredsignal1 = low1;
WouterJS 0:3710031b2621 95 raw_filteredsignal2 = low2;
WouterJS 0:3710031b2621 96
WouterJS 0:3710031b2621 97 }
WouterJS 0:3710031b2621 98
WouterJS 0:3710031b2621 99 void scopedata()
WouterJS 0:3710031b2621 100 {
WouterJS 0:3710031b2621 101 scope.set(0,emg1_input); //
WouterJS 0:3710031b2621 102 scope.set(1,filteredsignal1); //
WouterJS 0:3710031b2621 103 scope.set(2,filteredsignal2); //
WouterJS 0:3710031b2621 104 scope.send(); // send info to HIDScope server
WouterJS 0:3710031b2621 105 }
WouterJS 0:3710031b2621 106
WouterJS 0:3710031b2621 107 void loop_function() {
WouterJS 0:3710031b2621 108 measureall();
WouterJS 0:3710031b2621 109 switch(current_state) {
WouterJS 0:3710031b2621 110 case failure:
WouterJS 0:3710031b2621 111 do_state_failure(); // a separate function for what happens in each state
WouterJS 0:3710031b2621 112 break;
WouterJS 0:3710031b2621 113 case calib_emg:
WouterJS 0:3710031b2621 114 do_state_calib_emg();
WouterJS 0:3710031b2621 115 break;
WouterJS 0:3710031b2621 116 case operational:
WouterJS 0:3710031b2621 117 do_state_operational();
WouterJS 0:3710031b2621 118 break;
WouterJS 0:3710031b2621 119 case wait;
WouterJS 0:3710031b2621 120 do_state_wait();
WouterJS 0:3710031b2621 121 break;
WouterJS 0:3710031b2621 122 }
WouterJS 0:3710031b2621 123 motor_controller();
WouterJS 0:3710031b2621 124 scopedata(); // Outputs data to the computer
WouterJS 0:3710031b2621 125 }
WouterJS 0:3710031b2621 126
WouterJS 0:3710031b2621 127 do_state_failure(){
WouterJS 0:3710031b2621 128 //al motor movement to zero!
WouterJS 0:3710031b2621 129 wait(1000);
WouterJS 0:3710031b2621 130 };
WouterJS 0:3710031b2621 131
WouterJS 0:3710031b2621 132 do_state_calib_emg(){
WouterJS 0:3710031b2621 133 if (state_changed==true) {
WouterJS 0:3710031b2621 134 state_changed = false;
WouterJS 0:3710031b2621 135 }
WouterJS 0:3710031b2621 136 if(filteredsignal1 > max1){//calibrate to a new maximum
WouterJS 0:3710031b2621 137 max1 = filteredsignal1;
WouterJS 0:3710031b2621 138 }
WouterJS 0:3710031b2621 139 if(filteredsignal2 > max2){//calibrate to a new maximum
WouterJS 0:3710031b2621 140 max2 = filteredsignal2;
WouterJS 0:3710031b2621 141 }
WouterJS 0:3710031b2621 142
WouterJS 0:3710031b2621 143 if (filteredsignal1 > (0.75*max1) && filteredsignal2 > (0.75*max2)) {
WouterJS 0:3710031b2621 144 current_state = operational;
WouterJS 0:3710031b2621 145 used_ms = t.get_ms_reset();
WouterJS 0:3710031b2621 146 state_changed = true;
WouterJS 0:3710031b2621 147 }
WouterJS 0:3710031b2621 148 }
WouterJS 0:3710031b2621 149
WouterJS 0:3710031b2621 150 do_state_operational(){
WouterJS 0:3710031b2621 151 if (state_changed==true) {
WouterJS 0:3710031b2621 152 state_changed = false;
WouterJS 0:3710031b2621 153 }
WouterJS 0:3710031b2621 154
WouterJS 0:3710031b2621 155 switch(States) {// a separate function for what happens in each state
WouterJS 0:3710031b2621 156 case rest:
WouterJS 0:3710031b2621 157 if (filteredsignal2 > (0.6*max))) {//
WouterJS 0:3710031b2621 158 current_state = wait;
WouterJS 0:3710031b2621 159 state_changed = true;
WouterJS 0:3710031b2621 160 }
WouterJS 0:3710031b2621 161 if( t.get_ms() > 1000 && filteredsignal1 > (0.6max))
WouterJS 0:3710031b2621 162 {
WouterJS 0:3710031b2621 163 States = forward;
WouterJS 0:3710031b2621 164 used_ms = t.get_ms_reset();
WouterJS 0:3710031b2621 165 }
WouterJS 0:3710031b2621 166 break;
WouterJS 0:3710031b2621 167 case forward:
WouterJS 0:3710031b2621 168 do_forward();
WouterJS 0:3710031b2621 169 if( t.get_ms() > 1000 && filteredsignal1 > (0.6max))
WouterJS 0:3710031b2621 170 {
WouterJS 0:3710031b2621 171 States = backward;
WouterJS 0:3710031b2621 172 used_ms = t.get_ms_reset();
WouterJS 0:3710031b2621 173 }
WouterJS 0:3710031b2621 174 break;
WouterJS 0:3710031b2621 175 case backward:
WouterJS 0:3710031b2621 176 do_backward();
WouterJS 0:3710031b2621 177 if( t.get_ms() > 1000 && filteredsignal1 > (0.6max))
WouterJS 0:3710031b2621 178 {
WouterJS 0:3710031b2621 179 States = up;
WouterJS 0:3710031b2621 180 used_ms = t.get_ms_reset();
WouterJS 0:3710031b2621 181 }
WouterJS 0:3710031b2621 182 break;
WouterJS 0:3710031b2621 183 case up:
WouterJS 0:3710031b2621 184 do_up();
WouterJS 0:3710031b2621 185 if( t.get_ms() > 1000 && filteredsignal1 > (0.6max))
WouterJS 0:3710031b2621 186 {
WouterJS 0:3710031b2621 187 States = wait;
WouterJS 0:3710031b2621 188 used_ms = t.get_ms_reset();
WouterJS 0:3710031b2621 189 }
WouterJS 0:3710031b2621 190 break;
WouterJS 0:3710031b2621 191 case down:
WouterJS 0:3710031b2621 192 do_down();
WouterJS 0:3710031b2621 193 if( t.get_ms() > 1000 && filteredsignal1 > (0.6max))
WouterJS 0:3710031b2621 194 {
WouterJS 0:3710031b2621 195 States = wait;
WouterJS 0:3710031b2621 196 used_ms = t.get_ms_reset();
WouterJS 0:3710031b2621 197 }
WouterJS 0:3710031b2621 198 break;
WouterJS 0:3710031b2621 199 }
WouterJS 0:3710031b2621 200
WouterJS 0:3710031b2621 201
WouterJS 0:3710031b2621 202 }
WouterJS 0:3710031b2621 203
WouterJS 0:3710031b2621 204 do_state_wait(){
WouterJS 0:3710031b2621 205 if (state_changed==true) {
WouterJS 0:3710031b2621 206 state_changed = false;
WouterJS 0:3710031b2621 207 }
WouterJS 0:3710031b2621 208
WouterJS 0:3710031b2621 209 if (filteredsignal1 > (0.75*max1) && filteredsignal2 > (0.75*max2) {
WouterJS 0:3710031b2621 210 current_state = operational;
WouterJS 0:3710031b2621 211 state_changed = true;
WouterJS 0:3710031b2621 212 }
WouterJS 0:3710031b2621 213 }
WouterJS 0:3710031b2621 214
WouterJS 0:3710031b2621 215
WouterJS 0:3710031b2621 216
WouterJS 0:3710031b2621 217 int main()
WouterJS 0:3710031b2621 218 {
WouterJS 0:3710031b2621 219
WouterJS 0:3710031b2621 220 loop_timer.attach(&loop_function, 0.002);
WouterJS 0:3710031b2621 221
WouterJS 0:3710031b2621 222 pc.baud(115200);
WouterJS 0:3710031b2621 223
WouterJS 0:3710031b2621 224 while (true) {
WouterJS 0:3710031b2621 225 if(buttonR == true){
WouterJS 0:3710031b2621 226 current_state = failure;
WouterJS 0:3710031b2621 227 }
WouterJS 0:3710031b2621 228 }
WouterJS 0:3710031b2621 229 }