Boudewijn van den Berg / Mbed 2 deprecated EMGverwerking

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   Encoder MODSERIAL mbed

main.cpp@0:ba5ff341b020, 2013-10-29 (annotated)

Committer:
bouvdberg
Date:
Tue Oct 29 12:33:42 2013 +0000
Revision:
0:ba5ff341b020
Child:
1:adc1d0589d54
Robot arm aansturing

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bouvdberg 0:ba5ff341b020 1 #include "mbed.h"
bouvdberg 0:ba5ff341b020 2 #include "MODSERIAL.h"
bouvdberg 0:ba5ff341b020 3 #include "encoder.h"
bouvdberg 0:ba5ff341b020 4
bouvdberg 0:ba5ff341b020 5 // definieren constanten
bouvdberg 0:ba5ff341b020 6 #define PI 3.1415926
bouvdberg 0:ba5ff341b020 7 //plant
bouvdberg 0:ba5ff341b020 8 #define ARM1 0.36
bouvdberg 0:ba5ff341b020 9 #define ARM2 0.18
bouvdberg 0:ba5ff341b020 10 //PD
bouvdberg 0:ba5ff341b020 11 #define CP 0.0002
bouvdberg 0:ba5ff341b020 12 #define CD 0.002
bouvdberg 0:ba5ff341b020 13 #define CLP1 0.9975
bouvdberg 0:ba5ff341b020 14 #define CLP2 0.001
bouvdberg 0:ba5ff341b020 15 //Snelheid
bouvdberg 0:ba5ff341b020 16 #define SNELHEID 0.05
bouvdberg 0:ba5ff341b020 17 //LOOPTIME
bouvdberg 0:ba5ff341b020 18 #define LOOPTIME 0.01
bouvdberg 0:ba5ff341b020 19 //Filtering EMG
bouvdberg 0:ba5ff341b020 20 #define HP1 0.8187
bouvdberg 0:ba5ff341b020 21 #define HP2 20.0
bouvdberg 0:ba5ff341b020 22 #define HP3 20.0
bouvdberg 0:ba5ff341b020 23 #define LP1 0.9512
bouvdberg 0:ba5ff341b020 24 #define LP2 0.04877
bouvdberg 0:ba5ff341b020 25
bouvdberg 0:ba5ff341b020 26 void keep_in_range(float * in, float min, float max);
bouvdberg 0:ba5ff341b020 27
bouvdberg 0:ba5ff341b020 28 volatile bool looptimerflag;
bouvdberg 0:ba5ff341b020 29
bouvdberg 0:ba5ff341b020 30 void setlooptimerflag(void)
bouvdberg 0:ba5ff341b020 31 {
bouvdberg 0:ba5ff341b020 32 looptimerflag = true;
bouvdberg 0:ba5ff341b020 33 }
bouvdberg 0:ba5ff341b020 34
bouvdberg 0:ba5ff341b020 35 int main() {
bouvdberg 0:ba5ff341b020 36 AnalogIn EMG0(PTB0);
bouvdberg 0:ba5ff341b020 37 AnalogIn EMG1(PTB1);
bouvdberg 0:ba5ff341b020 38 AnalogIn EMG2(PTB2);
bouvdberg 0:ba5ff341b020 39 AnalogIn EMG3(PTB3);
bouvdberg 0:ba5ff341b020 40 AnalogIn potmeter(PTC2);
bouvdberg 0:ba5ff341b020 41 DigitalIn ButtonUP(PTE20); //Knopjes voor kalibratie
bouvdberg 0:ba5ff341b020 42 DigitalIn ButtonDOWN(PTE21);
bouvdberg 0:ba5ff341b020 43 DigitalIn ButtonSELECT(PTE22);
bouvdberg 0:ba5ff341b020 44 DigitalIn ButtonSTOP(PTE23);
bouvdberg 0:ba5ff341b020 45 DigitalOut Solenoid(PTD4); //Solenoid
bouvdberg 0:ba5ff341b020 46 Encoder motor1(PTD0,PTD2);//Moet aangesloten zijn op twee pinnen, waarvan een met interrupt mogelijkheid
bouvdberg 0:ba5ff341b020 47 Encoder motor2(;
bouvdberg 0:ba5ff341b020 48 PwmOut pwm_motor1(PTA12);
bouvdberg 0:ba5ff341b020 49 DigitalOut motordir1(PTD3);
bouvdberg 0:ba5ff341b020 50 PwmOut pwm_motor2(PTA5);
bouvdberg 0:ba5ff341b020 51 DigitalOut motordir2(PTD1);
bouvdberg 0:ba5ff341b020 52
bouvdberg 0:ba5ff341b020 53 //Variabelen verwerking EMG
bouvdberg 0:ba5ff341b020 54 float emg_value1, emg_value2, emg_value3, emg_value4;
bouvdberg 0:ba5ff341b020 55 float emg_value1min1=0.0, emg_value2min1=0.0, emg_value3min1=0.0, emg_value4min1=0.0;
bouvdberg 0:ba5ff341b020 56 float EMGhp1, EMGhp2, EMGhp3, EMGhp4, EMGlp1, EMGlp2, EMGlp3, EMGlp4;
bouvdberg 0:ba5ff341b020 57 float EMGhp1min1=0.0, EMGhp2min1=0.0, EMGhp3min1=0.0, EMGhp4min1=0.0, EMGlp1min1=0.0, EMGlp2min1=0.0, EMGlp3min1=0.0, EMGlp4min1=0.0;
bouvdberg 0:ba5ff341b020 58 float EMGmax1, EMGmin1, EMGmax2, EMGmin2, EMGmax3, EMGmin3, EMGmax4, EMGmin4;
bouvdberg 0:ba5ff341b020 59
bouvdberg 0:ba5ff341b020 60 //Variabelen bepaling input systeem
bouvdberg 0:ba5ff341b020 61 float input;
bouvdberg 0:ba5ff341b020 62 float w1, w2, wM2, phi1, phi2, theta;
bouvdberg 0:ba5ff341b020 63 float a, b, c, d, ai, bi, ci, di;
bouvdberg 0:ba5ff341b020 64 float v1, v2, v3, v4, vx, vy;
bouvdberg 0:ba5ff341b020 65
bouvdberg 0:ba5ff341b020 66 //Variabelen motoraansturing
bouvdberg 0:ba5ff341b020 67 float setpointM1, setpointM2;
bouvdberg 0:ba5ff341b020 68 float setpointmin1M1=0.0, setpointmin1M2=0.0;
bouvdberg 0:ba5ff341b020 69 float pwm_to_motor1, pwm_to_motor2;
bouvdberg 0:ba5ff341b020 70 float M1position, M2position;
bouvdberg 0:ba5ff341b020 71 float foutM1, foutM2;
bouvdberg 0:ba5ff341b020 72 float foutmin1M1=0.0, foutmin1M2=0.0;
bouvdberg 0:ba5ff341b020 73 float foutverschilM1, foutverschilM2;
bouvdberg 0:ba5ff341b020 74 float foutverschilmin1M1=0.0, foutverschilmin1M2=0.0;
bouvdberg 0:ba5ff341b020 75 float CDloop=CD/LOOPTIME;
bouvdberg 0:ba5ff341b020 76
bouvdberg 0:ba5ff341b020 77 //Kalibratie
bouvdberg 0:ba5ff341b020 78 //1. Bepalen EMGmax1 EMGmin1
bouvdberg 0:ba5ff341b020 79 //2. Bepalen EMGmax2 EMGmin2
bouvdberg 0:ba5ff341b020 80 //3. Bepalen EMGmax3 EMGmin3
bouvdberg 0:ba5ff341b020 81 //4. Bepalen EMGmax4 EMGmin4
bouvdberg 0:ba5ff341b020 82 EMGmax1=+(potmeter.read()-0.5)*;
bouvdberg 0:ba5ff341b020 83 EMGmin1=+(potmeter.read()-0.5)*;
bouvdberg 0:ba5ff341b020 84 EMGmax2=+(potmeter.read()-0.5)*;
bouvdberg 0:ba5ff341b020 85 EMGmin2=+(potmeter.read()-0.5)*;
bouvdberg 0:ba5ff341b020 86 EMGmax3=+(potmeter.read()-0.5)*;
bouvdberg 0:ba5ff341b020 87 EMGmin3=+(potmeter.read()-0.5)*;
bouvdberg 0:ba5ff341b020 88 EMGmax4=+(potmeter.read()-0.5)*;
bouvdberg 0:ba5ff341b020 89 EMGmin4=+(potmeter.read()-0.5)*;
bouvdberg 0:ba5ff341b020 90
bouvdberg 0:ba5ff341b020 91 //Aansturing
bouvdberg 0:ba5ff341b020 92 Ticker looptimer;
bouvdberg 0:ba5ff341b020 93 looptimer.attach(setlooptimerflag,LOOPTIME);
bouvdberg 0:ba5ff341b020 94 while(1) {
bouvdberg 0:ba5ff341b020 95 while(looptimerflag != true);
bouvdberg 0:ba5ff341b020 96 looptimerflag = false;
bouvdberg 0:ba5ff341b020 97
bouvdberg 0:ba5ff341b020 98 //uitlezen
bouvdberg 0:ba5ff341b020 99 emg_value1 = EMG0.read();
bouvdberg 0:ba5ff341b020 100 emg_value2 = EMG1.read();
bouvdberg 0:ba5ff341b020 101 emg_value3 = EMG2.read();
bouvdberg 0:ba5ff341b020 102 emg_value4 = EMG3.read();
bouvdberg 0:ba5ff341b020 103
bouvdberg 0:ba5ff341b020 104 //filtering
bouvdberg 0:ba5ff341b020 105 EMGhp1=HP1*EMGhp1min1+HP2*emg_value1-HP3*emg_value1min1;
bouvdberg 0:ba5ff341b020 106 EMGhp2=HP1*EMGhp2min1+HP2*emg_value2-HP3*emg_value2min1;
bouvdberg 0:ba5ff341b020 107 EMGhp3=HP1*EMGhp3min1+HP2*emg_value3-HP3*emg_value3min1;
bouvdberg 0:ba5ff341b020 108 EMGhp4=HP1*EMGhp4min1+HP2*emg_value4-HP3*emg_value4min1;
bouvdberg 0:ba5ff341b020 109 EMGhp1=abs(EMGhp1);
bouvdberg 0:ba5ff341b020 110 EMGhp2=abs(EMGhp2);
bouvdberg 0:ba5ff341b020 111 EMGhp3=abs(EMGhp3);
bouvdberg 0:ba5ff341b020 112 EMGhp4=abs(EMGhp4);
bouvdberg 0:ba5ff341b020 113 EMGlp1=LP1*EMGlp1min1+LP2*EMGhp1min1;
bouvdberg 0:ba5ff341b020 114 EMGlp2=LP1*EMGlp2min1+LP2*EMGhp2min1;
bouvdberg 0:ba5ff341b020 115 EMGlp3=LP1*EMGlp3min1+LP2*EMGhp3min1;
bouvdberg 0:ba5ff341b020 116 EMGlp4=LP1*EMGlp4min1+LP2*EMGhp4min1;
bouvdberg 0:ba5ff341b020 117
bouvdberg 0:ba5ff341b020 118 //berekenen setpoint
bouvdberg 0:ba5ff341b020 119 //hoekinput
bouvdberg 0:ba5ff341b020 120 v1=(EMGlp1-EMGmin1)/(EMGmax1-EMGmin1);
bouvdberg 0:ba5ff341b020 121 v2=(EMGlp2-EMGmin2)/(EMGmax2-EMGmin2);
bouvdberg 0:ba5ff341b020 122 v3=(EMGlp3-EMGmin3)/(EMGmax3-EMGmin3);
bouvdberg 0:ba5ff341b020 123 v4=(EMGlp4-EMGmin4)/(EMGmax4-EMGmin4);
bouvdberg 0:ba5ff341b020 124 input=tan((v1-v2)/(v3-v4));
bouvdberg 0:ba5ff341b020 125
bouvdberg 0:ba5ff341b020 126 //snelheidsvector
bouvdberg 0:ba5ff341b020 127 vx=cos(input)*SNELHEID;
bouvdberg 0:ba5ff341b020 128 vy=sin(input)*SNELHEID;
bouvdberg 0:ba5ff341b020 129
bouvdberg 0:ba5ff341b020 130 //input positie
bouvdberg 0:ba5ff341b020 131 phi1=motor1.getPosition();// Integraal van ideale positie maken!!!!!!!!!!
bouvdberg 0:ba5ff341b020 132 theta=motor2.getPosition();
bouvdberg 0:ba5ff341b020 133 phi2=theta+phi1-PI;
bouvdberg 0:ba5ff341b020 134
bouvdberg 0:ba5ff341b020 135 //Jacobiaan
bouvdberg 0:ba5ff341b020 136 // [a b]
bouvdberg 0:ba5ff341b020 137 // [c d]
bouvdberg 0:ba5ff341b020 138 a=cos(phi1)*ARM1;
bouvdberg 0:ba5ff341b020 139 b=sin(phi1)*ARM1;
bouvdberg 0:ba5ff341b020 140 c=cos(phi2)*ARM2;
bouvdberg 0:ba5ff341b020 141 d=sin(phi2)*ARM2;
bouvdberg 0:ba5ff341b020 142
bouvdberg 0:ba5ff341b020 143 //inverse
bouvdberg 0:ba5ff341b020 144 // [ai bi]
bouvdberg 0:ba5ff341b020 145 // [ci di]
bouvdberg 0:ba5ff341b020 146 ai=d/(a*d-b*c);
bouvdberg 0:ba5ff341b020 147 bi=-b/(a*d-b*c);
bouvdberg 0:ba5ff341b020 148 ci=-c/(a*d-b*c);
bouvdberg 0:ba5ff341b020 149 di=a/(a*d-b*c);
bouvdberg 0:ba5ff341b020 150
bouvdberg 0:ba5ff341b020 151 //vermenigvuldiging
bouvdberg 0:ba5ff341b020 152 // [ai bi] [vx] [w1]
bouvdberg 0:ba5ff341b020 153 // [ci di] * [vy] = [w2]
bouvdberg 0:ba5ff341b020 154 w1=ai*vx+bi*vy; //=wM1 hoeksnelheid van motor 1
bouvdberg 0:ba5ff341b020 155 w2=ci*vx+di*vy;
bouvdberg 0:ba5ff341b020 156 wM2=w2-w1;//hoeksnelheid motor 2
bouvdberg 0:ba5ff341b020 157
bouvdberg 0:ba5ff341b020 158 //Motoraansturing
bouvdberg 0:ba5ff341b020 159 setpointM1 = (w1/(2.0*PI))*3200.0*LOOPTIME+setpointmin1M1;
bouvdberg 0:ba5ff341b020 160 setpointM2 = (wM2/(2.0*PI))*3200.0*LOOPTIME+setpointmin1M2;
bouvdberg 0:ba5ff341b020 161 M1position = motor1.getPosition();
bouvdberg 0:ba5ff341b020 162 M2position = motor2.getPosition();
bouvdberg 0:ba5ff341b020 163 foutM1 = setpointM1-M1position;
bouvdberg 0:ba5ff341b020 164 foutM2 = setpointM2-M2position;
bouvdberg 0:ba5ff341b020 165 foutverschilM1 = foutM1-foutmin1M1;
bouvdberg 0:ba5ff341b020 166 foutverschilM2 = foutM2-foutmin1M2;
bouvdberg 0:ba5ff341b020 167 foutverschilM1 = CLP1*foutverschilmin1M1+CLP2*foutverschilM1;
bouvdberg 0:ba5ff341b020 168 foutverschilM2 = CLP1*foutverschilmin1M2+CLP2*foutverschilM2;
bouvdberg 0:ba5ff341b020 169 pwm_to_motor1 = foutM1*CP+foutverschilM1*CDloop;
bouvdberg 0:ba5ff341b020 170 pwm_to_motor2 = foutM2*CP+foutverschilM2*CDloop;
bouvdberg 0:ba5ff341b020 171 keep_in_range(&pwm_to_motor1, -1,1);
bouvdberg 0:ba5ff341b020 172 keep_in_range(&pwm_to_motor2, -1,1);
bouvdberg 0:ba5ff341b020 173 keep_in_range(&setpointM1, -800,800);//Juiste hoeken invoeren
bouvdberg 0:ba5ff341b020 174 keep_in_range(&setpointM2, 0,1500);
bouvdberg 0:ba5ff341b020 175 if(pwm_to_motor1 > 0)
bouvdberg 0:ba5ff341b020 176 motordir1 = 0;
bouvdberg 0:ba5ff341b020 177 else
bouvdberg 0:ba5ff341b020 178 motordir1 = 1;
bouvdberg 0:ba5ff341b020 179 if(pwm_to_motor2 > 0)
bouvdberg 0:ba5ff341b020 180 motordir2 = 0;
bouvdberg 0:ba5ff341b020 181 else
bouvdberg 0:ba5ff341b020 182 motordir2 = 1;
bouvdberg 0:ba5ff341b020 183
bouvdberg 0:ba5ff341b020 184 //WRITE VALUE TO MOTOR
bouvdberg 0:ba5ff341b020 185 pwm_motor1.write(abs(pwm_to_motor1));
bouvdberg 0:ba5ff341b020 186 pwm_motor2.write(abs(pwm_to_motor2));
bouvdberg 0:ba5ff341b020 187
bouvdberg 0:ba5ff341b020 188 //Definieren waarden in de verleden tijd
bouvdberg 0:ba5ff341b020 189 foutmin1M1=foutM1;
bouvdberg 0:ba5ff341b020 190 foutmin1M2=foutM2;
bouvdberg 0:ba5ff341b020 191 foutverschilmin1M1=foutverschilM1;
bouvdberg 0:ba5ff341b020 192 foutverschilmin1M2=foutverschilM2;
bouvdberg 0:ba5ff341b020 193 setpointmin1M1=setpointM1;
bouvdberg 0:ba5ff341b020 194 setpointmin1M2=setpointM2;
bouvdberg 0:ba5ff341b020 195 }
bouvdberg 0:ba5ff341b020 196 }
bouvdberg 0:ba5ff341b020 197
bouvdberg 0:ba5ff341b020 198 void keep_in_range(float * in, float min, float max)
bouvdberg 0:ba5ff341b020 199 {
bouvdberg 0:ba5ff341b020 200 *in > min ? *in < max? : *in = max: *in = min;
bouvdberg 0:ba5ff341b020 201 }