Marleen van Hoorn / Mbed 2 deprecated Project_script

inverse kinematics toegevoegd en tickers samengevoegd tot 1 ticker

Dependencies:   HIDScope MODSERIAL biquadFilter mbed

Fork of Project_script by Marijke Zondag

main.cpp@12:eaed305a76c3, 2018-10-19 (annotated)

Committer:
MarijkeZondag
Date:
Fri Oct 19 14:39:35 2018 +0000
Revision:
12:eaed305a76c3
Parent:
11:b95b0e9e1b89
Child:
13:a3d4b4daf5b4
Alleen nog moving average erin pleuren;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
vsluiter 0:c8f15874531b 1 #include "mbed.h"
vsluiter 0:c8f15874531b 2 #include "MODSERIAL.h"
MarijkeZondag 10:39ec51206c8b 3 #include "BiQuad.h"
MarijkeZondag 10:39ec51206c8b 4 #include "HIDScope.h"
MarijkeZondag 10:39ec51206c8b 5 #include <math.h>
vsluiter 0:c8f15874531b 6
MarijkeZondag 10:39ec51206c8b 7 AnalogIn emg0_in (A0);
MarijkeZondag 10:39ec51206c8b 8 AnalogIn emg1_in (A1);
MarijkeZondag 10:39ec51206c8b 9 AnalogIn emg2_in (A2);
MarijkeZondag 10:39ec51206c8b 10
MarijkeZondag 9:c722418997b5 11 DigitalIn button2 (D10); //Let op, is deze niet bezet?
MarijkeZondag 9:c722418997b5 12 InterruptIn encoderA (D9);
MarijkeZondag 9:c722418997b5 13 InterruptIn encoderB (D8);
MarijkeZondag 6:f4bbb73f3989 14
MarijkeZondag 9:c722418997b5 15 DigitalOut directionpin1 (D4);
MarijkeZondag 9:c722418997b5 16 DigitalOut directionpin2 (D7);
MarijkeZondag 9:c722418997b5 17 PwmOut pwmpin1 (D5);
MarijkeZondag 9:c722418997b5 18 PwmOut pwmpin2 (D6);
MarijkeZondag 9:c722418997b5 19
MarijkeZondag 6:f4bbb73f3989 20
vsluiter 0:c8f15874531b 21 MODSERIAL pc(USBTX, USBRX);
vsluiter 0:c8f15874531b 22
MarijkeZondag 9:c722418997b5 23
MarijkeZondag 9:c722418997b5 24 //Global variables
MarijkeZondag 12:eaed305a76c3 25 int encoder = 0;
MarijkeZondag 12:eaed305a76c3 26 const float T = 0.001f; //Ticker period
MarijkeZondag 10:39ec51206c8b 27
MarijkeZondag 10:39ec51206c8b 28 //Biquad
MarijkeZondag 10:39ec51206c8b 29 BiQuadChain emg0band;
MarijkeZondag 10:39ec51206c8b 30 BiQuad emg0band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 );
MarijkeZondag 10:39ec51206c8b 31 BiQuad emg0band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 );
MarijkeZondag 10:39ec51206c8b 32 BiQuad emg0band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 );
MarijkeZondag 10:39ec51206c8b 33
MarijkeZondag 10:39ec51206c8b 34 BiQuadChain emg1band;
MarijkeZondag 10:39ec51206c8b 35 BiQuad emg1band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 );
MarijkeZondag 10:39ec51206c8b 36 BiQuad emg1band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 );
MarijkeZondag 10:39ec51206c8b 37 BiQuad emg1band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 );
MarijkeZondag 10:39ec51206c8b 38
MarijkeZondag 10:39ec51206c8b 39 BiQuadChain emg2band;
MarijkeZondag 10:39ec51206c8b 40 BiQuad emg2band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 );
MarijkeZondag 10:39ec51206c8b 41 BiQuad emg2band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 );
MarijkeZondag 10:39ec51206c8b 42 BiQuad emg2band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 );
MarijkeZondag 10:39ec51206c8b 43
MarijkeZondag 10:39ec51206c8b 44 BiQuad notch1( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); //Notch filter
MarijkeZondag 10:39ec51206c8b 45
MarijkeZondag 10:39ec51206c8b 46
MarijkeZondag 10:39ec51206c8b 47 //Tickers
MarijkeZondag 12:eaed305a76c3 48 Ticker filter_tick;
MarijkeZondag 12:eaed305a76c3 49 Ticker MovAg_tick;
MarijkeZondag 9:c722418997b5 50
MarijkeZondag 9:c722418997b5 51 //Functions
MarijkeZondag 10:39ec51206c8b 52 void EMGFilter0()
MarijkeZondag 10:39ec51206c8b 53 {
MarijkeZondag 11:b95b0e9e1b89 54 double emg0 = emg0_in.read();
MarijkeZondag 11:b95b0e9e1b89 55 double bandpass0 = emg0band.step(emg0);
MarijkeZondag 11:b95b0e9e1b89 56 double absolute0 = fabs(bandpass0);
MarijkeZondag 11:b95b0e9e1b89 57 double notch0 = notch1.step(absolute0);
MarijkeZondag 10:39ec51206c8b 58 }
MarijkeZondag 10:39ec51206c8b 59
MarijkeZondag 10:39ec51206c8b 60 void EMGFilter1()
MarijkeZondag 10:39ec51206c8b 61 {
MarijkeZondag 11:b95b0e9e1b89 62 double emg1 = emg1_in.read();
MarijkeZondag 11:b95b0e9e1b89 63 double bandpass1 = emg1band.step(emg1);
MarijkeZondag 11:b95b0e9e1b89 64 double absolute1 = fabs(bandpass1);
MarijkeZondag 11:b95b0e9e1b89 65 double notch1 = notch1.step(absolute1);
MarijkeZondag 10:39ec51206c8b 66 }
MarijkeZondag 10:39ec51206c8b 67
MarijkeZondag 10:39ec51206c8b 68 void EMGFilter2()
MarijkeZondag 10:39ec51206c8b 69 {
MarijkeZondag 11:b95b0e9e1b89 70 double emg2 = emg2_in.read();
MarijkeZondag 11:b95b0e9e1b89 71 double bandpass2 = emg2band.step(emg2);
MarijkeZondag 11:b95b0e9e1b89 72 double absolute2 = fabs(bandpass2);
MarijkeZondag 11:b95b0e9e1b89 73 double notch2 = notch1.step(absolute2);
MarijkeZondag 10:39ec51206c8b 74 }
MarijkeZondag 10:39ec51206c8b 75
MarijkeZondag 12:eaed305a76c3 76 void emg_filtered() //call all filter functions
MarijkeZondag 12:eaed305a76c3 77 {
MarijkeZondag 12:eaed305a76c3 78 EMGFilter0();
MarijkeZondag 12:eaed305a76c3 79 EMGFilter1();
MarijkeZondag 12:eaed305a76c3 80 EMGFilter2();
MarijkeZondag 12:eaed305a76c3 81 }
MarijkeZondag 12:eaed305a76c3 82
MarijkeZondag 12:eaed305a76c3 83 void MovAg() //calculate moving average
MarijkeZondag 12:eaed305a76c3 84 {
MarijkeZondag 12:eaed305a76c3 85 for i =
MarijkeZondag 12:eaed305a76c3 86 }
MarijkeZondag 12:eaed305a76c3 87
MarijkeZondag 8:895d941a5910 88 void encoderA_rise()
MarijkeZondag 8:895d941a5910 89 {
MarijkeZondag 8:895d941a5910 90 if(encoderB==false)
MarijkeZondag 8:895d941a5910 91 {
MarijkeZondag 8:895d941a5910 92 encoder++;
MarijkeZondag 8:895d941a5910 93 }
MarijkeZondag 8:895d941a5910 94 else
MarijkeZondag 8:895d941a5910 95 {
MarijkeZondag 8:895d941a5910 96 encoder--;
MarijkeZondag 8:895d941a5910 97 }
MarijkeZondag 8:895d941a5910 98 }
MarijkeZondag 8:895d941a5910 99
MarijkeZondag 8:895d941a5910 100 void encoderA_fall()
MarijkeZondag 8:895d941a5910 101 {
MarijkeZondag 8:895d941a5910 102 if(encoderB==true)
MarijkeZondag 8:895d941a5910 103 {
MarijkeZondag 8:895d941a5910 104 encoder++;
MarijkeZondag 8:895d941a5910 105 }
MarijkeZondag 8:895d941a5910 106 else
MarijkeZondag 8:895d941a5910 107 {
MarijkeZondag 8:895d941a5910 108 encoder--;
MarijkeZondag 8:895d941a5910 109 }
MarijkeZondag 8:895d941a5910 110 }
MarijkeZondag 8:895d941a5910 111
MarijkeZondag 8:895d941a5910 112 void encoderB_rise()
MarijkeZondag 8:895d941a5910 113 {
MarijkeZondag 8:895d941a5910 114 if(encoderA==true)
MarijkeZondag 8:895d941a5910 115 {
MarijkeZondag 8:895d941a5910 116 encoder++;
MarijkeZondag 8:895d941a5910 117 }
MarijkeZondag 8:895d941a5910 118 else
MarijkeZondag 8:895d941a5910 119 {
MarijkeZondag 8:895d941a5910 120 encoder--;
MarijkeZondag 8:895d941a5910 121 }
MarijkeZondag 8:895d941a5910 122 }
MarijkeZondag 8:895d941a5910 123
MarijkeZondag 8:895d941a5910 124 void encoderB_fall()
MarijkeZondag 8:895d941a5910 125 {
MarijkeZondag 8:895d941a5910 126 if(encoderA==false)
MarijkeZondag 8:895d941a5910 127 {
MarijkeZondag 8:895d941a5910 128 encoder++;
MarijkeZondag 8:895d941a5910 129 }
MarijkeZondag 8:895d941a5910 130 else
MarijkeZondag 8:895d941a5910 131 {
MarijkeZondag 8:895d941a5910 132 encoder--;
MarijkeZondag 8:895d941a5910 133 }
MarijkeZondag 8:895d941a5910 134 }
MarijkeZondag 8:895d941a5910 135
MarijkeZondag 8:895d941a5910 136
MarijkeZondag 9:c722418997b5 137 // Main function start.
MarijkeZondag 8:895d941a5910 138
vsluiter 0:c8f15874531b 139 int main()
MarijkeZondag 3:a3ad58758696 140 {
vsluiter 0:c8f15874531b 141 pc.baud(115200);
MarijkeZondag 6:f4bbb73f3989 142 pc.printf("hello\n\r");
MarijkeZondag 10:39ec51206c8b 143
MarijkeZondag 10:39ec51206c8b 144 //EMG signaal filteren
MarijkeZondag 10:39ec51206c8b 145
MarijkeZondag 12:eaed305a76c3 146 filter_tick.attach(&emg_filtered,T); //EMG signals filtered every T sec.
MarijkeZondag 12:eaed305a76c3 147 MovAg_tick.attach(&MovAg,T); //Moving average calculation every T sec.
MarijkeZondag 12:eaed305a76c3 148
MarijkeZondag 10:39ec51206c8b 149 while(true){/*do not return from main()*/}
MarijkeZondag 10:39ec51206c8b 150
MarijkeZondag 10:39ec51206c8b 151
MarijkeZondag 6:f4bbb73f3989 152 pwmpin1.period_us(60); //60 microseconds PWM period, 16.7 kHz
MarijkeZondag 6:f4bbb73f3989 153
MarijkeZondag 8:895d941a5910 154 encoderA.rise(&encoderA_rise);
MarijkeZondag 8:895d941a5910 155 encoderA.fall(&encoderA_fall);
MarijkeZondag 8:895d941a5910 156 encoderB.rise(&encoderB_rise);
MarijkeZondag 8:895d941a5910 157 encoderB.fall(&encoderB_fall);
vsluiter 0:c8f15874531b 158
MarijkeZondag 2:2f4444f1504d 159 while (true)
MarijkeZondag 2:2f4444f1504d 160 {
MarijkeZondag 10:39ec51206c8b 161 //Motor aansturen en encoder uitlezen
MarijkeZondag 6:f4bbb73f3989 162 float u1 = potmetervalue1;
MarijkeZondag 6:f4bbb73f3989 163 float u2 = potmetervalue2;
MarijkeZondag 6:f4bbb73f3989 164
MarijkeZondag 6:f4bbb73f3989 165 float m1 = ((u1*2.0f)-1.0f);
MarijkeZondag 6:f4bbb73f3989 166 float m2 = ((u2*2.0f)-1.0f);
MarijkeZondag 7:f32005d13749 167
MarijkeZondag 7:f32005d13749 168 pwmpin1 = fabs(m1*0.6f)+0.4f; //pwm duty cycle can only be positive, floating, 0.4f is "inefficiënt", dit tellen we erbij op, en keer 0.6 om te corrigeren voor de helling.
MarijkeZondag 7:f32005d13749 169 directionpin1.write(m1>0); //Indien waar, motor draait rechtsom. Indien niet waar, motor draait linksom.
MarijkeZondag 7:f32005d13749 170 wait(0.01f); //zodat de code niet oneindig doorgaat.
MarijkeZondag 7:f32005d13749 171 pwmpin2 = fabs(m2*0.6f)+0.4f;
MarijkeZondag 8:895d941a5910 172 directionpin2.write(m2>0);
MarijkeZondag 10:39ec51206c8b 173
MarijkeZondag 10:39ec51206c8b 174 float encoderDegrees = float(encoder)*(360.0/8400.0);
MarijkeZondag 9:c722418997b5 175
MarijkeZondag 10:39ec51206c8b 176 pc.printf("Encoder count: %f \n\r",encoderDegrees);
MarijkeZondag 10:39ec51206c8b 177
vsluiter 0:c8f15874531b 178 }
vsluiter 0:c8f15874531b 179 }
MarijkeZondag 4:fbea05c8fe2b 180