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@11:b95b0e9e1b89, 2018-10-19 (annotated)

Committer:
MarijkeZondag
Date:
Fri Oct 19 14:15:43 2018 +0000
Revision:
11:b95b0e9e1b89
Parent:
10:39ec51206c8b
Child:
12:eaed305a76c3
EMG filter update;

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 8:895d941a5910 25 int encoder = 0;
MarijkeZondag 10:39ec51206c8b 26
MarijkeZondag 10:39ec51206c8b 27 //Biquad
MarijkeZondag 10:39ec51206c8b 28 BiQuadChain emg0band;
MarijkeZondag 10:39ec51206c8b 29 BiQuad emg0band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 );
MarijkeZondag 10:39ec51206c8b 30 BiQuad emg0band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 );
MarijkeZondag 10:39ec51206c8b 31 BiQuad emg0band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 );
MarijkeZondag 10:39ec51206c8b 32
MarijkeZondag 10:39ec51206c8b 33 BiQuadChain emg1band;
MarijkeZondag 10:39ec51206c8b 34 BiQuad emg1band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 );
MarijkeZondag 10:39ec51206c8b 35 BiQuad emg1band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 );
MarijkeZondag 10:39ec51206c8b 36 BiQuad emg1band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 );
MarijkeZondag 10:39ec51206c8b 37
MarijkeZondag 10:39ec51206c8b 38 BiQuadChain emg2band;
MarijkeZondag 10:39ec51206c8b 39 BiQuad emg2band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 );
MarijkeZondag 10:39ec51206c8b 40 BiQuad emg2band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 );
MarijkeZondag 10:39ec51206c8b 41 BiQuad emg2band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 );
MarijkeZondag 10:39ec51206c8b 42
MarijkeZondag 10:39ec51206c8b 43 BiQuad notch1( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); //Notch filter
MarijkeZondag 10:39ec51206c8b 44
MarijkeZondag 10:39ec51206c8b 45
MarijkeZondag 10:39ec51206c8b 46 //Tickers
MarijkeZondag 10:39ec51206c8b 47 Ticker emg_filter_tick;
MarijkeZondag 10:39ec51206c8b 48 Ticker Mov_av_tick;
MarijkeZondag 9:c722418997b5 49
MarijkeZondag 9:c722418997b5 50 //Functions
MarijkeZondag 10:39ec51206c8b 51 void EMGFilter0()
MarijkeZondag 10:39ec51206c8b 52 {
MarijkeZondag 11:b95b0e9e1b89 53 double emg0 = emg0_in.read();
MarijkeZondag 11:b95b0e9e1b89 54 double bandpass0 = emg0band.step(emg0);
MarijkeZondag 11:b95b0e9e1b89 55 double absolute0 = fabs(bandpass0);
MarijkeZondag 11:b95b0e9e1b89 56 double notch0 = notch1.step(absolute0);
MarijkeZondag 10:39ec51206c8b 57 }
MarijkeZondag 10:39ec51206c8b 58
MarijkeZondag 10:39ec51206c8b 59 void EMGFilter1()
MarijkeZondag 10:39ec51206c8b 60 {
MarijkeZondag 11:b95b0e9e1b89 61 double emg1 = emg1_in.read();
MarijkeZondag 11:b95b0e9e1b89 62 double bandpass1 = emg1band.step(emg1);
MarijkeZondag 11:b95b0e9e1b89 63 double absolute1 = fabs(bandpass1);
MarijkeZondag 11:b95b0e9e1b89 64 double notch1 = notch1.step(absolute1);
MarijkeZondag 10:39ec51206c8b 65 }
MarijkeZondag 10:39ec51206c8b 66
MarijkeZondag 10:39ec51206c8b 67 void EMGFilter2()
MarijkeZondag 10:39ec51206c8b 68 {
MarijkeZondag 11:b95b0e9e1b89 69 double emg2 = emg2_in.read();
MarijkeZondag 11:b95b0e9e1b89 70 double bandpass2 = emg2band.step(emg2);
MarijkeZondag 11:b95b0e9e1b89 71 double absolute2 = fabs(bandpass2);
MarijkeZondag 11:b95b0e9e1b89 72 double notch2 = notch1.step(absolute2);
MarijkeZondag 10:39ec51206c8b 73 }
MarijkeZondag 10:39ec51206c8b 74
MarijkeZondag 8:895d941a5910 75 void encoderA_rise()
MarijkeZondag 8:895d941a5910 76 {
MarijkeZondag 8:895d941a5910 77 if(encoderB==false)
MarijkeZondag 8:895d941a5910 78 {
MarijkeZondag 8:895d941a5910 79 encoder++;
MarijkeZondag 8:895d941a5910 80 }
MarijkeZondag 8:895d941a5910 81 else
MarijkeZondag 8:895d941a5910 82 {
MarijkeZondag 8:895d941a5910 83 encoder--;
MarijkeZondag 8:895d941a5910 84 }
MarijkeZondag 8:895d941a5910 85 }
MarijkeZondag 8:895d941a5910 86
MarijkeZondag 8:895d941a5910 87 void encoderA_fall()
MarijkeZondag 8:895d941a5910 88 {
MarijkeZondag 8:895d941a5910 89 if(encoderB==true)
MarijkeZondag 8:895d941a5910 90 {
MarijkeZondag 8:895d941a5910 91 encoder++;
MarijkeZondag 8:895d941a5910 92 }
MarijkeZondag 8:895d941a5910 93 else
MarijkeZondag 8:895d941a5910 94 {
MarijkeZondag 8:895d941a5910 95 encoder--;
MarijkeZondag 8:895d941a5910 96 }
MarijkeZondag 8:895d941a5910 97 }
MarijkeZondag 8:895d941a5910 98
MarijkeZondag 8:895d941a5910 99 void encoderB_rise()
MarijkeZondag 8:895d941a5910 100 {
MarijkeZondag 8:895d941a5910 101 if(encoderA==true)
MarijkeZondag 8:895d941a5910 102 {
MarijkeZondag 8:895d941a5910 103 encoder++;
MarijkeZondag 8:895d941a5910 104 }
MarijkeZondag 8:895d941a5910 105 else
MarijkeZondag 8:895d941a5910 106 {
MarijkeZondag 8:895d941a5910 107 encoder--;
MarijkeZondag 8:895d941a5910 108 }
MarijkeZondag 8:895d941a5910 109 }
MarijkeZondag 8:895d941a5910 110
MarijkeZondag 8:895d941a5910 111 void encoderB_fall()
MarijkeZondag 8:895d941a5910 112 {
MarijkeZondag 8:895d941a5910 113 if(encoderA==false)
MarijkeZondag 8:895d941a5910 114 {
MarijkeZondag 8:895d941a5910 115 encoder++;
MarijkeZondag 8:895d941a5910 116 }
MarijkeZondag 8:895d941a5910 117 else
MarijkeZondag 8:895d941a5910 118 {
MarijkeZondag 8:895d941a5910 119 encoder--;
MarijkeZondag 8:895d941a5910 120 }
MarijkeZondag 8:895d941a5910 121 }
MarijkeZondag 8:895d941a5910 122
MarijkeZondag 8:895d941a5910 123
MarijkeZondag 9:c722418997b5 124 // Main function start.
MarijkeZondag 8:895d941a5910 125
vsluiter 0:c8f15874531b 126 int main()
MarijkeZondag 3:a3ad58758696 127 {
vsluiter 0:c8f15874531b 128 pc.baud(115200);
MarijkeZondag 6:f4bbb73f3989 129 pc.printf("hello\n\r");
MarijkeZondag 10:39ec51206c8b 130
MarijkeZondag 10:39ec51206c8b 131 //EMG signaal filteren
MarijkeZondag 10:39ec51206c8b 132
MarijkeZondag 10:39ec51206c8b 133 // Deze? of die hierondbqc.add( &bq1 ).add( &bq2 ).add( &bq3 );
MarijkeZondag 10:39ec51206c8b 134 bqc = bq1 * bq2 * bq3;
MarijkeZondag 10:39ec51206c8b 135 emgSampleTicker.attach(&emgSample,0.01); //100 Hz
MarijkeZondag 10:39ec51206c8b 136 while(true){/*do not return from main()*/}
MarijkeZondag 10:39ec51206c8b 137
MarijkeZondag 10:39ec51206c8b 138
MarijkeZondag 6:f4bbb73f3989 139 pwmpin1.period_us(60); //60 microseconds PWM period, 16.7 kHz
MarijkeZondag 6:f4bbb73f3989 140
MarijkeZondag 8:895d941a5910 141 encoderA.rise(&encoderA_rise);
MarijkeZondag 8:895d941a5910 142 encoderA.fall(&encoderA_fall);
MarijkeZondag 8:895d941a5910 143 encoderB.rise(&encoderB_rise);
MarijkeZondag 8:895d941a5910 144 encoderB.fall(&encoderB_fall);
vsluiter 0:c8f15874531b 145
MarijkeZondag 2:2f4444f1504d 146 while (true)
MarijkeZondag 2:2f4444f1504d 147 {
MarijkeZondag 10:39ec51206c8b 148 //Motor aansturen en encoder uitlezen
MarijkeZondag 6:f4bbb73f3989 149 float u1 = potmetervalue1;
MarijkeZondag 6:f4bbb73f3989 150 float u2 = potmetervalue2;
MarijkeZondag 6:f4bbb73f3989 151
MarijkeZondag 6:f4bbb73f3989 152 float m1 = ((u1*2.0f)-1.0f);
MarijkeZondag 6:f4bbb73f3989 153 float m2 = ((u2*2.0f)-1.0f);
MarijkeZondag 7:f32005d13749 154
MarijkeZondag 7:f32005d13749 155 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 156 directionpin1.write(m1>0); //Indien waar, motor draait rechtsom. Indien niet waar, motor draait linksom.
MarijkeZondag 7:f32005d13749 157 wait(0.01f); //zodat de code niet oneindig doorgaat.
MarijkeZondag 7:f32005d13749 158 pwmpin2 = fabs(m2*0.6f)+0.4f;
MarijkeZondag 8:895d941a5910 159 directionpin2.write(m2>0);
MarijkeZondag 10:39ec51206c8b 160
MarijkeZondag 10:39ec51206c8b 161 float encoderDegrees = float(encoder)*(360.0/8400.0);
MarijkeZondag 9:c722418997b5 162
MarijkeZondag 10:39ec51206c8b 163 pc.printf("Encoder count: %f \n\r",encoderDegrees);
MarijkeZondag 10:39ec51206c8b 164
vsluiter 0:c8f15874531b 165 }
vsluiter 0:c8f15874531b 166 }
MarijkeZondag 4:fbea05c8fe2b 167