Version 1: coming together. With PD controller but without inverse kinematics
Dependencies: Encoder MODSERIAL mbed
Fork of motoraansturing_met_EMG by
main.cpp@2:55bff07c1058, 2013-11-02 (annotated)
- Committer:
- Tess
- Date:
- Sat Nov 02 12:12:42 2013 +0000
- Revision:
- 2:55bff07c1058
- Parent:
- 1:1c22ce9f370b
Version final; everything is in here.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
jorick92 | 0:73dcc8fb9900 | 1 | #include "mbed.h" |
jorick92 | 0:73dcc8fb9900 | 2 | #include "encoder.h" |
jorick92 | 0:73dcc8fb9900 | 3 | #include "MODSERIAL.h" |
jorick92 | 0:73dcc8fb9900 | 4 | |
jorick92 | 0:73dcc8fb9900 | 5 | //high pass filter constantes 15Hz cutoff 4e orde |
Tess | 1:1c22ce9f370b | 6 | #define NUM0 0.2754 // constante |
Tess | 1:1c22ce9f370b | 7 | #define NUM1 -1.1017 // z^-1 |
Tess | 1:1c22ce9f370b | 8 | #define NUM2 1.6525 // z^-2etc. |
Tess | 1:1c22ce9f370b | 9 | #define NUM3 -1.1017 |
Tess | 1:1c22ce9f370b | 10 | #define NUM4 0.2754 |
jorick92 | 0:73dcc8fb9900 | 11 | |
Tess | 1:1c22ce9f370b | 12 | #define DEN0 1 // constante |
Tess | 1:1c22ce9f370b | 13 | #define DEN1 -1.5704 |
Tess | 1:1c22ce9f370b | 14 | #define DEN2 1.2756 |
Tess | 1:1c22ce9f370b | 15 | #define DEN3 -0.4844 |
Tess | 1:1c22ce9f370b | 16 | #define DEN4 0.0762 |
jorick92 | 0:73dcc8fb9900 | 17 | |
jorick92 | 0:73dcc8fb9900 | 18 | //lowpass filter constantes 40 Hz 4e orde |
Tess | 1:1c22ce9f370b | 19 | #define NUM0_2 0.4328 // constante |
Tess | 1:1c22ce9f370b | 20 | #define NUM1_2 1.7314 // z^-1 |
Tess | 1:1c22ce9f370b | 21 | #define NUM2_2 2.5971 // z^-2etc. |
Tess | 1:1c22ce9f370b | 22 | #define NUM3_2 1.7314 |
Tess | 1:1c22ce9f370b | 23 | #define NUM4_2 0.4328 |
jorick92 | 0:73dcc8fb9900 | 24 | |
jorick92 | 0:73dcc8fb9900 | 25 | |
Tess | 1:1c22ce9f370b | 26 | #define DEN0_2 1 // constante |
Tess | 1:1c22ce9f370b | 27 | #define DEN1_2 2.3695 |
Tess | 1:1c22ce9f370b | 28 | #define DEN2_2 2.3140 |
Tess | 1:1c22ce9f370b | 29 | #define DEN3_2 1.0547 |
Tess | 1:1c22ce9f370b | 30 | #define DEN4_2 0.1874 |
jorick92 | 0:73dcc8fb9900 | 31 | |
jorick92 | 0:73dcc8fb9900 | 32 | //lowpass filter constantes 3Hz 4e orde |
Tess | 1:1c22ce9f370b | 33 | #define NUM0_3 0.0000624 // constante |
Tess | 1:1c22ce9f370b | 34 | #define NUM1_3 0.0002495 // z^-1 |
Tess | 1:1c22ce9f370b | 35 | #define NUM2_3 0.0003743 // z^-2etc. |
Tess | 1:1c22ce9f370b | 36 | #define NUM3_3 0.0002495 |
Tess | 1:1c22ce9f370b | 37 | #define NUM4_3 0.0000624 |
jorick92 | 0:73dcc8fb9900 | 38 | |
Tess | 1:1c22ce9f370b | 39 | #define DEN0_3 1 // constante |
Tess | 1:1c22ce9f370b | 40 | #define DEN1_3 -3.5078 |
Tess | 1:1c22ce9f370b | 41 | #define DEN2_3 4.6409 |
Tess | 1:1c22ce9f370b | 42 | #define DEN3_3 -2.7427 |
Tess | 1:1c22ce9f370b | 43 | #define DEN4_3 0.6105 |
jorick92 | 0:73dcc8fb9900 | 44 | |
jorick92 | 0:73dcc8fb9900 | 45 | /******************************************************************************* |
jorick92 | 0:73dcc8fb9900 | 46 | * * |
jorick92 | 0:73dcc8fb9900 | 47 | * Code can be found at http://mbed.org/users/vsluiter/code/BMT-K9-Regelaar/ * |
jorick92 | 0:73dcc8fb9900 | 48 | * * |
jorick92 | 0:73dcc8fb9900 | 49 | ********************************************************************************/ |
jorick92 | 0:73dcc8fb9900 | 50 | |
jorick92 | 0:73dcc8fb9900 | 51 | /** keep_in_range -> float in, and keep_in_range if less than min, or larger than max **/ |
jorick92 | 0:73dcc8fb9900 | 52 | void keep_in_range(float * in, float min, float max); |
jorick92 | 0:73dcc8fb9900 | 53 | |
Tess | 1:1c22ce9f370b | 54 | /** variable to show when a new loop can be started */ |
Tess | 1:1c22ce9f370b | 55 | /** volatile means that it can be changed in an interrupt routine, and that that change is visible in the main loop. */ |
jorick92 | 0:73dcc8fb9900 | 56 | |
jorick92 | 0:73dcc8fb9900 | 57 | volatile bool looptimerflag; |
jorick92 | 0:73dcc8fb9900 | 58 | |
jorick92 | 0:73dcc8fb9900 | 59 | /** function called by Ticker "looptimer" */ |
Tess | 1:1c22ce9f370b | 60 | /** variable 'looptimerflag' is set to 'true' each time the looptimer expires.*/ |
jorick92 | 0:73dcc8fb9900 | 61 | void setlooptimerflag(void) |
jorick92 | 0:73dcc8fb9900 | 62 | { |
jorick92 | 0:73dcc8fb9900 | 63 | looptimerflag = true; |
jorick92 | 0:73dcc8fb9900 | 64 | } |
jorick92 | 0:73dcc8fb9900 | 65 | |
Tess | 1:1c22ce9f370b | 66 | //emg variabelen -> analoge input |
Tess | 1:1c22ce9f370b | 67 | float emg_value_biceps, emg_value_triceps, emg_value_flexoren, emg_value_extensoren, dy; |
Tess | 1:1c22ce9f370b | 68 | AnalogIn emg_biceps(PTB0); |
Tess | 1:1c22ce9f370b | 69 | AnalogIn emg_triceps(PTB1); |
Tess | 1:1c22ce9f370b | 70 | AnalogIn emg_flexoren(PTB2); |
Tess | 1:1c22ce9f370b | 71 | AnalogIn emg_extensoren(PTB3); |
jorick92 | 0:73dcc8fb9900 | 72 | |
jorick92 | 0:73dcc8fb9900 | 73 | /* |
jorick92 | 0:73dcc8fb9900 | 74 | DIT IS DE FILTER FUNCTIE! aanroepen door "filter(signaal nummer)" |
jorick92 | 0:73dcc8fb9900 | 75 | filter(1): biceps meten |
jorick92 | 0:73dcc8fb9900 | 76 | filter(2): triceps meten |
jorick92 | 0:73dcc8fb9900 | 77 | filter(3): flexoren meten |
Tess | 1:1c22ce9f370b | 78 | filter(3): extensoren meten |
jorick92 | 0:73dcc8fb9900 | 79 | */ |
Tess | 1:1c22ce9f370b | 80 | float filter(int sig_number) |
Tess | 1:1c22ce9f370b | 81 | { |
jorick92 | 0:73dcc8fb9900 | 82 | float sig_out; |
Tess | 1:1c22ce9f370b | 83 | |
Tess | 1:1c22ce9f370b | 84 | // eerst variabelen definiëren |
Tess | 1:1c22ce9f370b | 85 | |
jorick92 | 0:73dcc8fb9900 | 86 | //biceps |
Tess | 1:1c22ce9f370b | 87 | //filter 1 |
jorick92 | 0:73dcc8fb9900 | 88 | float in0_biceps =0; |
jorick92 | 0:73dcc8fb9900 | 89 | static float in1_biceps =0, in2_biceps = 0, in3_biceps = 0, in4_biceps = 0; |
jorick92 | 0:73dcc8fb9900 | 90 | static float out0_biceps = 0, out1_biceps = 0 , out2_biceps = 0, out3_biceps = 0, out4_biceps = 0; |
Tess | 1:1c22ce9f370b | 91 | //filter 2 |
jorick92 | 0:73dcc8fb9900 | 92 | float in0_2_biceps =0; |
jorick92 | 0:73dcc8fb9900 | 93 | static float in1_2_biceps =0, in2_2_biceps = 0, in3_2_biceps = 0, in4_2_biceps = 0; |
jorick92 | 0:73dcc8fb9900 | 94 | static float out0_2_biceps = 0, out1_2_biceps = 0 , out2_2_biceps = 0, out3_2_biceps = 0, out4_2_biceps = 0; |
Tess | 1:1c22ce9f370b | 95 | //filter 3 |
jorick92 | 0:73dcc8fb9900 | 96 | float in0_3_biceps =0; |
jorick92 | 0:73dcc8fb9900 | 97 | static float in1_3_biceps =0, in2_3_biceps = 0, in3_3_biceps = 0, in4_3_biceps = 0; |
jorick92 | 0:73dcc8fb9900 | 98 | static float out0_3_biceps = 0, out1_3_biceps = 0 , out2_3_biceps = 0, out3_3_biceps = 0, out4_3_biceps = 0; |
Tess | 1:1c22ce9f370b | 99 | |
jorick92 | 0:73dcc8fb9900 | 100 | //triceps |
Tess | 1:1c22ce9f370b | 101 | //filter 1 |
jorick92 | 0:73dcc8fb9900 | 102 | float in0_triceps =0; |
jorick92 | 0:73dcc8fb9900 | 103 | static float in1_triceps =0, in2_triceps = 0, in3_triceps = 0, in4_triceps = 0; |
jorick92 | 0:73dcc8fb9900 | 104 | static float out0_triceps = 0, out1_triceps = 0 , out2_triceps = 0, out3_triceps = 0, out4_triceps = 0; |
Tess | 1:1c22ce9f370b | 105 | //filter 2 |
jorick92 | 0:73dcc8fb9900 | 106 | float in0_2_triceps =0; |
jorick92 | 0:73dcc8fb9900 | 107 | static float in1_2_triceps =0, in2_2_triceps = 0, in3_2_triceps = 0, in4_2_triceps = 0; |
jorick92 | 0:73dcc8fb9900 | 108 | static float out0_2_triceps = 0, out1_2_triceps = 0 , out2_2_triceps = 0, out3_2_triceps = 0, out4_2_triceps = 0; |
Tess | 1:1c22ce9f370b | 109 | //filter 3 |
jorick92 | 0:73dcc8fb9900 | 110 | float in0_3_triceps =0; |
jorick92 | 0:73dcc8fb9900 | 111 | static float in1_3_triceps =0, in2_3_triceps = 0, in3_3_triceps = 0, in4_3_triceps = 0; |
jorick92 | 0:73dcc8fb9900 | 112 | static float out0_3_triceps = 0, out1_3_triceps = 0 , out2_3_triceps = 0, out3_3_triceps = 0, out4_3_triceps = 0; |
Tess | 1:1c22ce9f370b | 113 | |
jorick92 | 0:73dcc8fb9900 | 114 | //flexoren |
Tess | 1:1c22ce9f370b | 115 | //filter 1 |
jorick92 | 0:73dcc8fb9900 | 116 | float in0_flexoren =0; |
jorick92 | 0:73dcc8fb9900 | 117 | static float in1_flexoren =0, in2_flexoren = 0, in3_flexoren = 0, in4_flexoren = 0; |
jorick92 | 0:73dcc8fb9900 | 118 | static float out0_flexoren = 0, out1_flexoren = 0 , out2_flexoren = 0, out3_flexoren = 0, out4_flexoren = 0; |
Tess | 1:1c22ce9f370b | 119 | //filter 2 |
jorick92 | 0:73dcc8fb9900 | 120 | float in0_2_flexoren =0; |
jorick92 | 0:73dcc8fb9900 | 121 | static float in1_2_flexoren =0, in2_2_flexoren = 0, in3_2_flexoren = 0, in4_2_flexoren = 0; |
jorick92 | 0:73dcc8fb9900 | 122 | static float out0_2_flexoren = 0, out1_2_flexoren = 0 , out2_2_flexoren = 0, out3_2_flexoren = 0, out4_2_flexoren = 0; |
Tess | 1:1c22ce9f370b | 123 | //filter 3 |
jorick92 | 0:73dcc8fb9900 | 124 | float in0_3_flexoren =0; |
jorick92 | 0:73dcc8fb9900 | 125 | static float in1_3_flexoren =0, in2_3_flexoren = 0, in3_3_flexoren = 0, in4_3_flexoren = 0; |
jorick92 | 0:73dcc8fb9900 | 126 | static float out0_3_flexoren = 0, out1_3_flexoren = 0 , out2_3_flexoren = 0, out3_3_flexoren = 0, out4_3_flexoren = 0; |
Tess | 1:1c22ce9f370b | 127 | |
jorick92 | 0:73dcc8fb9900 | 128 | //extensoren |
Tess | 1:1c22ce9f370b | 129 | //filter 1 |
jorick92 | 0:73dcc8fb9900 | 130 | float in0_extensoren =0; |
jorick92 | 0:73dcc8fb9900 | 131 | static float in1_extensoren =0, in2_extensoren = 0, in3_extensoren = 0, in4_extensoren = 0; |
jorick92 | 0:73dcc8fb9900 | 132 | static float out0_extensoren = 0, out1_extensoren = 0 , out2_extensoren = 0, out3_extensoren = 0, out4_extensoren = 0; |
Tess | 1:1c22ce9f370b | 133 | //filter 2 |
jorick92 | 0:73dcc8fb9900 | 134 | float in0_2_extensoren =0; |
jorick92 | 0:73dcc8fb9900 | 135 | static float in1_2_extensoren =0, in2_2_extensoren = 0, in3_2_extensoren = 0, in4_2_extensoren = 0; |
jorick92 | 0:73dcc8fb9900 | 136 | static float out0_2_extensoren = 0, out1_2_extensoren = 0 , out2_2_extensoren = 0, out3_2_extensoren = 0, out4_2_extensoren = 0; |
Tess | 1:1c22ce9f370b | 137 | //filter 3 |
jorick92 | 0:73dcc8fb9900 | 138 | float in0_3_extensoren =0; |
jorick92 | 0:73dcc8fb9900 | 139 | static float in1_3_extensoren =0, in2_3_extensoren = 0, in3_3_extensoren = 0, in4_3_extensoren = 0; |
jorick92 | 0:73dcc8fb9900 | 140 | static float out0_3_extensoren = 0, out1_3_extensoren = 0 , out2_3_extensoren = 0, out3_3_extensoren = 0, out4_3_extensoren = 0; |
Tess | 1:1c22ce9f370b | 141 | |
Tess | 1:1c22ce9f370b | 142 | |
Tess | 1:1c22ce9f370b | 143 | switch(sig_number) { |
Tess | 1:1c22ce9f370b | 144 | case 1: |
jorick92 | 0:73dcc8fb9900 | 145 | // signaal filteren op 15 Hz HIGHPASS |
Tess | 1:1c22ce9f370b | 146 | in4_biceps = in3_biceps; |
Tess | 1:1c22ce9f370b | 147 | in3_biceps = in2_biceps; |
Tess | 1:1c22ce9f370b | 148 | in2_biceps = in1_biceps; |
Tess | 1:1c22ce9f370b | 149 | in1_biceps = in0_biceps; |
jorick92 | 0:73dcc8fb9900 | 150 | in0_biceps = emg_biceps.read(); |
Tess | 1:1c22ce9f370b | 151 | out4_biceps = out3_biceps; |
Tess | 1:1c22ce9f370b | 152 | out3_biceps = out2_biceps; |
Tess | 1:1c22ce9f370b | 153 | out2_biceps = out1_biceps; |
Tess | 1:1c22ce9f370b | 154 | out1_biceps = out0_biceps; |
Tess | 1:1c22ce9f370b | 155 | out0_biceps = (NUM0*in0_biceps + NUM1*in1_biceps + NUM2*in2_biceps + NUM3*in3_biceps + NUM4*in4_biceps - DEN1*out1_biceps - DEN2*out2_biceps - DEN3*out3_biceps - DEN4*out4_biceps ) / DEN0; |
Tess | 1:1c22ce9f370b | 156 | |
jorick92 | 0:73dcc8fb9900 | 157 | //signaal filteren op 40 HZ LOWPASS |
Tess | 1:1c22ce9f370b | 158 | in4_2_biceps = in3_2_biceps; |
Tess | 1:1c22ce9f370b | 159 | in3_2_biceps = in2_2_biceps; |
Tess | 1:1c22ce9f370b | 160 | in2_2_biceps = in1_2_biceps; |
Tess | 1:1c22ce9f370b | 161 | in1_2_biceps = in0_2_biceps; |
jorick92 | 0:73dcc8fb9900 | 162 | in0_2_biceps = out0_biceps; |
Tess | 1:1c22ce9f370b | 163 | out4_2_biceps = out3_2_biceps; |
Tess | 1:1c22ce9f370b | 164 | out3_2_biceps = out2_2_biceps; |
Tess | 1:1c22ce9f370b | 165 | out2_2_biceps = out1_2_biceps; |
Tess | 1:1c22ce9f370b | 166 | out1_2_biceps = out0_2_biceps; |
jorick92 | 0:73dcc8fb9900 | 167 | out0_2_biceps = (NUM0_2*in0_2_biceps + NUM1_2*in1_2_biceps + NUM2_2*in2_2_biceps + NUM3_2*in3_2_biceps + NUM4_2*in4_2_biceps - DEN1_2*out1_2_biceps - DEN2_2*out2_2_biceps - DEN3_2*out3_2_biceps - DEN4_2*out4_2_biceps ) / DEN0_2; |
Tess | 1:1c22ce9f370b | 168 | |
jorick92 | 0:73dcc8fb9900 | 169 | //signaal filteren op 5Hz LOWPASS |
Tess | 1:1c22ce9f370b | 170 | in4_3_biceps = in3_3_biceps; |
Tess | 1:1c22ce9f370b | 171 | in3_3_biceps = in2_3_biceps; |
Tess | 1:1c22ce9f370b | 172 | in2_3_biceps = in1_3_biceps; |
Tess | 1:1c22ce9f370b | 173 | in1_3_biceps = in0_3_biceps; |
jorick92 | 0:73dcc8fb9900 | 174 | in0_3_biceps = abs(out0_2_biceps); |
Tess | 1:1c22ce9f370b | 175 | out4_3_biceps = out3_3_biceps; |
Tess | 1:1c22ce9f370b | 176 | out3_3_biceps = out2_3_biceps; |
Tess | 1:1c22ce9f370b | 177 | out2_3_biceps = out1_3_biceps; |
Tess | 1:1c22ce9f370b | 178 | out1_3_biceps = out0_3_biceps; |
Tess | 1:1c22ce9f370b | 179 | out0_3_biceps = (NUM0_3*in0_3_biceps + NUM1_3*in1_3_biceps + NUM2_3*in2_3_biceps + NUM3_3*in3_3_biceps + NUM4_3*in4_3_biceps - DEN1_3*out1_3_biceps - DEN2_3*out2_3_biceps - DEN3_3*out3_3_biceps - DEN4_3*out4_3_biceps ) / DEN0_3; |
jorick92 | 0:73dcc8fb9900 | 180 | sig_out = out0_3_biceps; |
jorick92 | 0:73dcc8fb9900 | 181 | break; |
jorick92 | 0:73dcc8fb9900 | 182 | case 2: |
jorick92 | 0:73dcc8fb9900 | 183 | // signaal filteren op 15 Hz HIGHPASS |
Tess | 1:1c22ce9f370b | 184 | in4_triceps = in3_triceps; |
Tess | 1:1c22ce9f370b | 185 | in3_triceps = in2_triceps; |
Tess | 1:1c22ce9f370b | 186 | in2_triceps = in1_triceps; |
Tess | 1:1c22ce9f370b | 187 | in1_triceps = in0_triceps; |
jorick92 | 0:73dcc8fb9900 | 188 | in0_triceps = emg_triceps.read(); |
Tess | 1:1c22ce9f370b | 189 | out4_triceps = out3_triceps; |
Tess | 1:1c22ce9f370b | 190 | out3_triceps = out2_triceps; |
Tess | 1:1c22ce9f370b | 191 | out2_triceps = out1_triceps; |
Tess | 1:1c22ce9f370b | 192 | out1_triceps = out0_triceps; |
Tess | 1:1c22ce9f370b | 193 | out0_triceps = (NUM0*in0_triceps + NUM1*in1_triceps + NUM2*in2_triceps + NUM3*in3_triceps + NUM4*in4_triceps - DEN1*out1_triceps - DEN2*out2_triceps - DEN3*out3_triceps - DEN4*out4_triceps ) / DEN0; |
Tess | 1:1c22ce9f370b | 194 | |
jorick92 | 0:73dcc8fb9900 | 195 | //signaal filteren op 40 HZ LOWPASS |
Tess | 1:1c22ce9f370b | 196 | in4_2_triceps = in3_2_triceps; |
Tess | 1:1c22ce9f370b | 197 | in3_2_triceps = in2_2_triceps; |
Tess | 1:1c22ce9f370b | 198 | in2_2_triceps = in1_2_triceps; |
Tess | 1:1c22ce9f370b | 199 | in1_2_triceps = in0_2_triceps; |
jorick92 | 0:73dcc8fb9900 | 200 | in0_2_triceps = out0_triceps; |
Tess | 1:1c22ce9f370b | 201 | out4_2_triceps = out3_2_triceps; |
Tess | 1:1c22ce9f370b | 202 | out3_2_triceps = out2_2_triceps; |
Tess | 1:1c22ce9f370b | 203 | out2_2_triceps = out1_2_triceps; |
Tess | 1:1c22ce9f370b | 204 | out1_2_triceps = out0_2_triceps; |
jorick92 | 0:73dcc8fb9900 | 205 | out0_2_triceps = (NUM0_2*in0_2_triceps + NUM1_2*in1_2_triceps + NUM2_2*in2_2_triceps + NUM3_2*in3_2_triceps + NUM4_2*in4_2_triceps - DEN1_2*out1_2_triceps - DEN2_2*out2_2_triceps - DEN3_2*out3_2_triceps - DEN4_2*out4_2_triceps ) / DEN0_2; |
Tess | 1:1c22ce9f370b | 206 | |
jorick92 | 0:73dcc8fb9900 | 207 | //signaal filteren op 5Hz LOWPASS |
Tess | 1:1c22ce9f370b | 208 | in4_3_triceps = in3_3_triceps; |
Tess | 1:1c22ce9f370b | 209 | in3_3_triceps = in2_3_triceps; |
Tess | 1:1c22ce9f370b | 210 | in2_3_triceps = in1_3_triceps; |
Tess | 1:1c22ce9f370b | 211 | in1_3_triceps = in0_3_triceps; |
jorick92 | 0:73dcc8fb9900 | 212 | in0_3_triceps = abs(out0_2_triceps); |
Tess | 1:1c22ce9f370b | 213 | out4_3_triceps = out3_3_triceps; |
Tess | 1:1c22ce9f370b | 214 | out3_3_triceps = out2_3_triceps; |
Tess | 1:1c22ce9f370b | 215 | out2_3_triceps = out1_3_triceps; |
Tess | 1:1c22ce9f370b | 216 | out1_3_triceps = out0_3_triceps; |
Tess | 1:1c22ce9f370b | 217 | out0_3_triceps = (NUM0_3*in0_3_triceps + NUM1_3*in1_3_triceps + NUM2_3*in2_3_triceps + NUM3_3*in3_3_triceps + NUM4_3*in4_3_triceps - DEN1_3*out1_3_triceps - DEN2_3*out2_3_triceps - DEN3_3*out3_3_triceps - DEN4_3*out4_3_triceps ) / DEN0_3; |
jorick92 | 0:73dcc8fb9900 | 218 | sig_out = out0_3_triceps; |
jorick92 | 0:73dcc8fb9900 | 219 | break; |
jorick92 | 0:73dcc8fb9900 | 220 | case 3: |
jorick92 | 0:73dcc8fb9900 | 221 | // signaal filteren op 15 Hz HIGHPASS |
Tess | 1:1c22ce9f370b | 222 | in4_flexoren = in3_flexoren; |
Tess | 1:1c22ce9f370b | 223 | in3_flexoren = in2_flexoren; |
Tess | 1:1c22ce9f370b | 224 | in2_flexoren = in1_flexoren; |
Tess | 1:1c22ce9f370b | 225 | in1_flexoren = in0_flexoren; |
jorick92 | 0:73dcc8fb9900 | 226 | in0_flexoren = emg_flexoren.read(); |
Tess | 1:1c22ce9f370b | 227 | out4_flexoren = out3_flexoren; |
Tess | 1:1c22ce9f370b | 228 | out3_flexoren = out2_flexoren; |
Tess | 1:1c22ce9f370b | 229 | out2_flexoren = out1_flexoren; |
Tess | 1:1c22ce9f370b | 230 | out1_flexoren = out0_flexoren; |
Tess | 1:1c22ce9f370b | 231 | out0_flexoren = (NUM0*in0_flexoren + NUM1*in1_flexoren + NUM2*in2_flexoren + NUM3*in3_flexoren + NUM4*in4_flexoren - DEN1*out1_flexoren - DEN2*out2_flexoren - DEN3*out3_flexoren - DEN4*out4_flexoren ) / DEN0; |
Tess | 1:1c22ce9f370b | 232 | |
jorick92 | 0:73dcc8fb9900 | 233 | //signaal filteren op 40 HZ LOWPASS |
Tess | 1:1c22ce9f370b | 234 | in4_2_flexoren = in3_2_flexoren; |
Tess | 1:1c22ce9f370b | 235 | in3_2_flexoren = in2_2_flexoren; |
Tess | 1:1c22ce9f370b | 236 | in2_2_flexoren = in1_2_flexoren; |
Tess | 1:1c22ce9f370b | 237 | in1_2_flexoren = in0_2_flexoren; |
jorick92 | 0:73dcc8fb9900 | 238 | in0_2_flexoren = out0_flexoren; |
Tess | 1:1c22ce9f370b | 239 | out4_2_flexoren = out3_2_flexoren; |
Tess | 1:1c22ce9f370b | 240 | out3_2_flexoren = out2_2_flexoren; |
Tess | 1:1c22ce9f370b | 241 | out2_2_flexoren = out1_2_flexoren; |
Tess | 1:1c22ce9f370b | 242 | out1_2_flexoren = out0_2_flexoren; |
jorick92 | 0:73dcc8fb9900 | 243 | out0_2_flexoren = (NUM0_2*in0_2_flexoren + NUM1_2*in1_2_flexoren + NUM2_2*in2_2_flexoren + NUM3_2*in3_2_flexoren + NUM4_2*in4_2_flexoren - DEN1_2*out1_2_flexoren - DEN2_2*out2_2_flexoren - DEN3_2*out3_2_flexoren - DEN4_2*out4_2_flexoren ) / DEN0_2; |
Tess | 1:1c22ce9f370b | 244 | |
jorick92 | 0:73dcc8fb9900 | 245 | //signaal filteren op 5Hz LOWPASS |
Tess | 1:1c22ce9f370b | 246 | in4_3_flexoren = in3_3_flexoren; |
Tess | 1:1c22ce9f370b | 247 | in3_3_flexoren = in2_3_flexoren; |
Tess | 1:1c22ce9f370b | 248 | in2_3_flexoren = in1_3_flexoren; |
Tess | 1:1c22ce9f370b | 249 | in1_3_flexoren = in0_3_flexoren; |
jorick92 | 0:73dcc8fb9900 | 250 | in0_3_flexoren = abs(out0_2_flexoren); |
Tess | 1:1c22ce9f370b | 251 | out4_3_flexoren = out3_3_flexoren; |
Tess | 1:1c22ce9f370b | 252 | out3_3_flexoren = out2_3_flexoren; |
Tess | 1:1c22ce9f370b | 253 | out2_3_flexoren = out1_3_flexoren; |
Tess | 1:1c22ce9f370b | 254 | out1_3_flexoren = out0_3_flexoren; |
Tess | 1:1c22ce9f370b | 255 | out0_3_flexoren = (NUM0_3*in0_3_flexoren + NUM1_3*in1_3_flexoren + NUM2_3*in2_3_flexoren + NUM3_3*in3_3_flexoren + NUM4_3*in4_3_flexoren - DEN1_3*out1_3_flexoren - DEN2_3*out2_3_flexoren - DEN3_3*out3_3_flexoren - DEN4_3*out4_3_flexoren ) / DEN0_3; |
jorick92 | 0:73dcc8fb9900 | 256 | sig_out = out0_3_flexoren; |
jorick92 | 0:73dcc8fb9900 | 257 | break; |
jorick92 | 0:73dcc8fb9900 | 258 | case 4: |
jorick92 | 0:73dcc8fb9900 | 259 | // signaal filteren op 15 Hz HIGHPASS |
Tess | 1:1c22ce9f370b | 260 | in4_extensoren = in3_extensoren; |
Tess | 1:1c22ce9f370b | 261 | in3_extensoren = in2_extensoren; |
Tess | 1:1c22ce9f370b | 262 | in2_extensoren = in1_extensoren; |
Tess | 1:1c22ce9f370b | 263 | in1_extensoren = in0_extensoren; |
jorick92 | 0:73dcc8fb9900 | 264 | in0_extensoren = emg_extensoren.read(); |
Tess | 1:1c22ce9f370b | 265 | out4_extensoren = out3_extensoren; |
Tess | 1:1c22ce9f370b | 266 | out3_extensoren = out2_extensoren; |
Tess | 1:1c22ce9f370b | 267 | out2_extensoren = out1_extensoren; |
Tess | 1:1c22ce9f370b | 268 | out1_extensoren = out0_extensoren; |
Tess | 1:1c22ce9f370b | 269 | out0_extensoren = (NUM0*in0_extensoren + NUM1*in1_extensoren + NUM2*in2_extensoren + NUM3*in3_extensoren + NUM4*in4_extensoren - DEN1*out1_extensoren - DEN2*out2_extensoren - DEN3*out3_extensoren - DEN4*out4_extensoren ) / DEN0; |
Tess | 1:1c22ce9f370b | 270 | |
jorick92 | 0:73dcc8fb9900 | 271 | //signaal filteren op 40 HZ LOWPASS |
Tess | 1:1c22ce9f370b | 272 | in4_2_extensoren = in3_2_extensoren; |
Tess | 1:1c22ce9f370b | 273 | in3_2_extensoren = in2_2_extensoren; |
Tess | 1:1c22ce9f370b | 274 | in2_2_extensoren = in1_2_extensoren; |
Tess | 1:1c22ce9f370b | 275 | in1_2_extensoren = in0_2_extensoren; |
jorick92 | 0:73dcc8fb9900 | 276 | in0_2_extensoren = out0_extensoren; |
Tess | 1:1c22ce9f370b | 277 | out4_2_extensoren = out3_2_extensoren; |
Tess | 1:1c22ce9f370b | 278 | out3_2_extensoren = out2_2_extensoren; |
Tess | 1:1c22ce9f370b | 279 | out2_2_extensoren = out1_2_extensoren; |
Tess | 1:1c22ce9f370b | 280 | out1_2_extensoren = out0_2_extensoren; |
jorick92 | 0:73dcc8fb9900 | 281 | out0_2_extensoren = (NUM0_2*in0_2_extensoren + NUM1_2*in1_2_extensoren + NUM2_2*in2_2_extensoren + NUM3_2*in3_2_extensoren + NUM4_2*in4_2_extensoren - DEN1_2*out1_2_extensoren - DEN2_2*out2_2_extensoren - DEN3_2*out3_2_extensoren - DEN4_2*out4_2_extensoren ) / DEN0_2; |
Tess | 1:1c22ce9f370b | 282 | |
jorick92 | 0:73dcc8fb9900 | 283 | //signaal filteren op 5Hz LOWPASS |
Tess | 1:1c22ce9f370b | 284 | in4_3_extensoren = in3_3_extensoren; |
Tess | 1:1c22ce9f370b | 285 | in3_3_extensoren = in2_3_extensoren; |
Tess | 1:1c22ce9f370b | 286 | in2_3_extensoren = in1_3_extensoren; |
Tess | 1:1c22ce9f370b | 287 | in1_3_extensoren = in0_3_extensoren; |
jorick92 | 0:73dcc8fb9900 | 288 | in0_3_extensoren = abs(out0_2_extensoren); |
Tess | 1:1c22ce9f370b | 289 | out4_3_extensoren = out3_3_extensoren; |
Tess | 1:1c22ce9f370b | 290 | out3_3_extensoren = out2_3_extensoren; |
Tess | 1:1c22ce9f370b | 291 | out2_3_extensoren = out1_3_extensoren; |
Tess | 1:1c22ce9f370b | 292 | out1_3_extensoren = out0_3_extensoren; |
Tess | 1:1c22ce9f370b | 293 | out0_3_extensoren = (NUM0_3*in0_3_extensoren + NUM1_3*in1_3_extensoren + NUM2_3*in2_3_extensoren + NUM3_3*in3_3_extensoren + NUM4_3*in4_3_extensoren - DEN1_3*out1_3_extensoren - DEN2_3*out2_3_extensoren - DEN3_3*out3_3_extensoren - DEN4_3*out4_3_extensoren ) / DEN0_3; |
jorick92 | 0:73dcc8fb9900 | 294 | sig_out = out0_3_extensoren; |
jorick92 | 0:73dcc8fb9900 | 295 | break; |
jorick92 | 0:73dcc8fb9900 | 296 | } |
jorick92 | 0:73dcc8fb9900 | 297 | return sig_out; |
jorick92 | 0:73dcc8fb9900 | 298 | } |
jorick92 | 0:73dcc8fb9900 | 299 | |
jorick92 | 0:73dcc8fb9900 | 300 | int main() |
jorick92 | 0:73dcc8fb9900 | 301 | { |
jorick92 | 0:73dcc8fb9900 | 302 | //LOCAL VARIABLES |
Tess | 1:1c22ce9f370b | 303 | Encoder motorA(PTD4,PTC8); // encoder, using my encoder library |
jorick92 | 0:73dcc8fb9900 | 304 | Encoder motorB(PTD0,PTD2); |
Tess | 1:1c22ce9f370b | 305 | MODSERIAL pc(USBTX,USBRX); // MODSERIAL to get non-blocking Serial |
Tess | 1:1c22ce9f370b | 306 | PwmOut pwm_motorA(PTA12); // PWM control to motor |
Tess | 1:1c22ce9f370b | 307 | PwmOut pwm_motorB(PTA5); // PWM control to motor |
Tess | 1:1c22ce9f370b | 308 | DigitalOut motordirA(PTD3); // Direction pin |
Tess | 1:1c22ce9f370b | 309 | DigitalOut motordirB(PTD1); // Direction pin |
Tess | 2:55bff07c1058 | 310 | |
jorick92 | 0:73dcc8fb9900 | 311 | /* variable to store setpoint in */ |
jorick92 | 0:73dcc8fb9900 | 312 | float setpointA; |
jorick92 | 0:73dcc8fb9900 | 313 | float setpointB; |
jorick92 | 0:73dcc8fb9900 | 314 | float setpoint_beginA; |
jorick92 | 0:73dcc8fb9900 | 315 | float setpoint_beginB; |
jorick92 | 0:73dcc8fb9900 | 316 | float setpoint_rechtsonderA; |
jorick92 | 0:73dcc8fb9900 | 317 | float setpoint_rechtsonderB; |
jorick92 | 0:73dcc8fb9900 | 318 | |
jorick92 | 0:73dcc8fb9900 | 319 | /* variable to store pwm value in*/ |
jorick92 | 0:73dcc8fb9900 | 320 | float pwm_to_motorA; |
jorick92 | 0:73dcc8fb9900 | 321 | float pwm_to_begin_motorA = 0; |
jorick92 | 0:73dcc8fb9900 | 322 | float pwm_to_begin_motorB = 0; |
jorick92 | 0:73dcc8fb9900 | 323 | float pwm_to_motorB; |
jorick92 | 0:73dcc8fb9900 | 324 | float pwm_to_rechtsonder_motorA; |
jorick92 | 0:73dcc8fb9900 | 325 | float pwm_to_rechtsonder_motorB; |
Tess | 2:55bff07c1058 | 326 | |
Tess | 1:1c22ce9f370b | 327 | /* variable for PD controller*/ |
jorick92 | 0:73dcc8fb9900 | 328 | const float dt = 0.002; |
Tess | 1:1c22ce9f370b | 329 | float Kp = 0.001; //0.0208 |
Tess | 1:1c22ce9f370b | 330 | float Kd = 0.00004342; //0.0006897 |
jorick92 | 0:73dcc8fb9900 | 331 | float error_t0_A = 0; |
jorick92 | 0:73dcc8fb9900 | 332 | float error_t0_B = 0; |
jorick92 | 0:73dcc8fb9900 | 333 | float error_ti_A; |
jorick92 | 0:73dcc8fb9900 | 334 | float error_ti_B; |
jorick92 | 0:73dcc8fb9900 | 335 | float P_regelaar_A; |
jorick92 | 0:73dcc8fb9900 | 336 | float P_regelaar_B; |
jorick92 | 0:73dcc8fb9900 | 337 | float D_regelaar_A; |
jorick92 | 0:73dcc8fb9900 | 338 | float D_regelaar_B; |
jorick92 | 0:73dcc8fb9900 | 339 | float output_regelaar_A; |
jorick92 | 0:73dcc8fb9900 | 340 | float output_regelaar_B; |
jorick92 | 0:73dcc8fb9900 | 341 | |
Tess | 1:1c22ce9f370b | 342 | /* variable to store positions in*/ |
jorick92 | 0:73dcc8fb9900 | 343 | int32_t positionmotorA_t0; |
jorick92 | 0:73dcc8fb9900 | 344 | int32_t positionmotorB_t0; |
jorick92 | 0:73dcc8fb9900 | 345 | int32_t positionmotorA_t_1; |
jorick92 | 0:73dcc8fb9900 | 346 | int32_t positionmotorB_t_1; |
jorick92 | 0:73dcc8fb9900 | 347 | int32_t positiondifference_motorA; |
jorick92 | 0:73dcc8fb9900 | 348 | int32_t positiondifference_motorB; |
jorick92 | 0:73dcc8fb9900 | 349 | |
Tess | 2:55bff07c1058 | 350 | /* inverse kinematica */ |
Tess | 2:55bff07c1058 | 351 | float dy; //dy waarde tussen -1 en 1 -1 -vmax; 1 vmax |
Tess | 2:55bff07c1058 | 352 | float dx; //dx waarde tussen -1 en 1 -1 -vmax; 1 vmax |
Tess | 2:55bff07c1058 | 353 | const float vmax = 2; // m/s |
Tess | 2:55bff07c1058 | 354 | const float delta_t = 0.005; // 1/samplefrequentie, dus tijd tussen twee meetpunten |
Tess | 2:55bff07c1058 | 355 | float X_positie; |
Tess | 2:55bff07c1058 | 356 | float Y_positie; |
Tess | 2:55bff07c1058 | 357 | float X_positie_begin; |
Tess | 2:55bff07c1058 | 358 | float Y_positie_begin; |
Tess | 2:55bff07c1058 | 359 | float puls_motorA; |
Tess | 2:55bff07c1058 | 360 | float puls_motorB; |
Tess | 2:55bff07c1058 | 361 | float kwadraat_X_positie; |
Tess | 2:55bff07c1058 | 362 | float kwadraat_Y_positie; |
Tess | 2:55bff07c1058 | 363 | float phi_A_pulsen_positie_begin; |
Tess | 2:55bff07c1058 | 364 | float phi_B_pulsen_positie_begin; |
Tess | 2:55bff07c1058 | 365 | float phi_A_positie_begin; |
Tess | 2:55bff07c1058 | 366 | float phi_B_positie_begin; |
Tess | 2:55bff07c1058 | 367 | float phi_1; |
Tess | 2:55bff07c1058 | 368 | float lengte_arm = 276; // in mm anders rare imaginaire getallen |
Tess | 2:55bff07c1058 | 369 | float phi_A; |
Tess | 2:55bff07c1058 | 370 | float phi_B; |
Tess | 2:55bff07c1058 | 371 | float Puls_motorA; |
Tess | 2:55bff07c1058 | 372 | float Puls_motorB; |
Tess | 2:55bff07c1058 | 373 | float phi_A_pulsen; |
Tess | 2:55bff07c1058 | 374 | float phi_B_pulsen; |
Tess | 2:55bff07c1058 | 375 | |
jorick92 | 0:73dcc8fb9900 | 376 | //START OF CODE |
jorick92 | 0:73dcc8fb9900 | 377 | |
Tess | 1:1c22ce9f370b | 378 | pc.baud(115200); // Set the baudrate (use this number in RealTerm too!) |
Tess | 1:1c22ce9f370b | 379 | |
Tess | 1:1c22ce9f370b | 380 | // In dit stukje code zorgen we ervoor dat de arm gaat draaien naar rechts en stopt als het tegen het frame komt. Eerst motor B botsen dan motor A botsen. |
Tess | 1:1c22ce9f370b | 381 | // motor B zit onder en motor A zit boven en dus op zijn kop (en dus setpoint moet - zijn). |
jorick92 | 0:73dcc8fb9900 | 382 | |
Tess | 1:1c22ce9f370b | 383 | motordirB.write(0); |
Tess | 1:1c22ce9f370b | 384 | pwm_motorB.write(.08); |
Tess | 1:1c22ce9f370b | 385 | positionmotorB_t0 = motorB.getPosition(); |
Tess | 1:1c22ce9f370b | 386 | do { |
Tess | 1:1c22ce9f370b | 387 | wait(0.2); |
Tess | 1:1c22ce9f370b | 388 | positionmotorB_t_1 = positionmotorB_t0 ; |
Tess | 1:1c22ce9f370b | 389 | positionmotorB_t0 = motorB.getPosition(); |
Tess | 1:1c22ce9f370b | 390 | positiondifference_motorB = abs(positionmotorB_t0 - positionmotorB_t_1); |
Tess | 1:1c22ce9f370b | 391 | } while(positiondifference_motorB > 10); |
Tess | 1:1c22ce9f370b | 392 | motorB.setPosition(0); |
Tess | 1:1c22ce9f370b | 393 | pwm_motorB.write(0); |
jorick92 | 0:73dcc8fb9900 | 394 | |
Tess | 1:1c22ce9f370b | 395 | wait(1); // willen nu even dat tussen ene actie en andere actie 1 seconde wacht. |
jorick92 | 0:73dcc8fb9900 | 396 | |
Tess | 1:1c22ce9f370b | 397 | motordirA.write(1); |
Tess | 1:1c22ce9f370b | 398 | pwm_motorA.write(.08); |
Tess | 1:1c22ce9f370b | 399 | positionmotorA_t0 = motorA.getPosition(); |
Tess | 1:1c22ce9f370b | 400 | do { |
Tess | 1:1c22ce9f370b | 401 | wait(0.2); |
Tess | 1:1c22ce9f370b | 402 | positionmotorA_t_1 = positionmotorA_t0 ; |
Tess | 1:1c22ce9f370b | 403 | positionmotorA_t0 = motorA.getPosition(); |
Tess | 1:1c22ce9f370b | 404 | positiondifference_motorA = abs(positionmotorA_t0 - positionmotorA_t_1); |
Tess | 1:1c22ce9f370b | 405 | } while(positiondifference_motorA > 10); |
Tess | 1:1c22ce9f370b | 406 | motorA.setPosition(0); |
Tess | 1:1c22ce9f370b | 407 | pwm_motorA.write(0); |
jorick92 | 0:73dcc8fb9900 | 408 | |
Tess | 1:1c22ce9f370b | 409 | wait(1); // willen nu even dat tussen ene actie en andere actie 1 seconde wacht. |
jorick92 | 0:73dcc8fb9900 | 410 | |
Tess | 1:1c22ce9f370b | 411 | // Hierna willen we de motor van zijn alleruiterste positie naar de x-as hebben. Hiervoor moet motor A eerst op de x-as worden gezet. Hiervoor moet motor A 4.11 graden (63) naar links. |
jorick92 | 0:73dcc8fb9900 | 412 | |
Tess | 1:1c22ce9f370b | 413 | motordirA.write(0); |
Tess | 1:1c22ce9f370b | 414 | pwm_motorA.write(.08); |
Tess | 1:1c22ce9f370b | 415 | do { |
Tess | 1:1c22ce9f370b | 416 | setpoint_beginA = -63; // x-as |
Tess | 1:1c22ce9f370b | 417 | pwm_to_begin_motorA = abs((setpoint_beginA + motorA.getPosition()) *.001); // + omdat men met een negatieve hoekverdraaiing werkt. |
Tess | 1:1c22ce9f370b | 418 | wait(0.2); |
Tess | 1:1c22ce9f370b | 419 | keep_in_range(&pwm_to_begin_motorA, -1, 1 ); |
Tess | 1:1c22ce9f370b | 420 | motordirA.write(0); |
Tess | 1:1c22ce9f370b | 421 | pwm_motorA.write(pwm_to_begin_motorA); |
Tess | 1:1c22ce9f370b | 422 | } while(pwm_to_begin_motorA <= 0); |
Tess | 1:1c22ce9f370b | 423 | motorA.setPosition(0); |
Tess | 1:1c22ce9f370b | 424 | pwm_motorA.write(0); |
jorick92 | 0:73dcc8fb9900 | 425 | |
Tess | 1:1c22ce9f370b | 426 | wait(1); // willen nu even dat tussen ene actie en andere actie 1 seconde wacht. |
jorick92 | 0:73dcc8fb9900 | 427 | |
Tess | 2:55bff07c1058 | 428 | // hierna moet motor A naar de rechtsonder A4. Motor A 532 (hoek 59.8 graden). |
jorick92 | 0:73dcc8fb9900 | 429 | |
Tess | 1:1c22ce9f370b | 430 | motordirA.write(0); |
Tess | 1:1c22ce9f370b | 431 | pwm_motorA.write(0.08); |
Tess | 1:1c22ce9f370b | 432 | do { |
Tess | 1:1c22ce9f370b | 433 | setpoint_rechtsonderA = -532; // rechtsonder positie A4 |
Tess | 1:1c22ce9f370b | 434 | pwm_to_rechtsonder_motorA = abs((setpoint_rechtsonderA + motorA.getPosition()) *.001); |
Tess | 1:1c22ce9f370b | 435 | wait(0.2); |
Tess | 1:1c22ce9f370b | 436 | keep_in_range(&pwm_to_rechtsonder_motorA, -1, 1 ); |
Tess | 1:1c22ce9f370b | 437 | motordirA.write(0); |
Tess | 1:1c22ce9f370b | 438 | pwm_motorA.write(pwm_to_rechtsonder_motorA); |
Tess | 1:1c22ce9f370b | 439 | } while(pwm_to_rechtsonder_motorA <= 0); |
Tess | 1:1c22ce9f370b | 440 | pwm_motorA.write(0); |
jorick92 | 0:73dcc8fb9900 | 441 | |
Tess | 1:1c22ce9f370b | 442 | wait(1); |
Tess | 1:1c22ce9f370b | 443 | |
Tess | 1:1c22ce9f370b | 444 | // Hierna moet motor B 21.6 (192) graden naar links om naar x-as te gaan. |
jorick92 | 0:73dcc8fb9900 | 445 | |
Tess | 1:1c22ce9f370b | 446 | motordirB.write(1); |
Tess | 1:1c22ce9f370b | 447 | pwm_motorB.write(.08); |
Tess | 1:1c22ce9f370b | 448 | do { |
Tess | 1:1c22ce9f370b | 449 | setpoint_beginB = 192; // x-as |
Tess | 1:1c22ce9f370b | 450 | pwm_to_begin_motorB = abs((setpoint_beginB - motorB.getPosition()) *.001); |
Tess | 1:1c22ce9f370b | 451 | wait(0.2); |
Tess | 1:1c22ce9f370b | 452 | keep_in_range(&pwm_to_begin_motorB, -1, 1 ); |
Tess | 1:1c22ce9f370b | 453 | motordirB.write(1); |
Tess | 1:1c22ce9f370b | 454 | pwm_motorB.write(pwm_to_begin_motorB); |
Tess | 1:1c22ce9f370b | 455 | } while(pwm_to_begin_motorB <= 0); |
Tess | 1:1c22ce9f370b | 456 | motorB.setPosition(0); |
Tess | 1:1c22ce9f370b | 457 | pwm_motorB.write(0); |
jorick92 | 0:73dcc8fb9900 | 458 | |
Tess | 1:1c22ce9f370b | 459 | wait(1); // willen nu even dat tussen ene actie en andere actie 1 seconde wacht. |
jorick92 | 0:73dcc8fb9900 | 460 | |
Tess | 2:55bff07c1058 | 461 | // Hierna moet motor B van x-as naar de rechtsonder A4 positie. Motor B 268 (30.2 graden). |
jorick92 | 0:73dcc8fb9900 | 462 | |
Tess | 1:1c22ce9f370b | 463 | motordirB.write(1); |
Tess | 1:1c22ce9f370b | 464 | pwm_motorB.write(0.08); |
Tess | 1:1c22ce9f370b | 465 | do { |
Tess | 2:55bff07c1058 | 466 | setpoint_rechtsonderB = 268; // rechtsonder positie A4 |
Tess | 1:1c22ce9f370b | 467 | pwm_to_rechtsonder_motorB = abs((setpoint_rechtsonderB - motorB.getPosition()) *.001); |
Tess | 1:1c22ce9f370b | 468 | wait(0.2); |
Tess | 1:1c22ce9f370b | 469 | keep_in_range(&pwm_to_rechtsonder_motorB, -1, 1 ); |
Tess | 1:1c22ce9f370b | 470 | motordirB.write(1); |
Tess | 1:1c22ce9f370b | 471 | pwm_motorB.write(pwm_to_rechtsonder_motorB); |
Tess | 1:1c22ce9f370b | 472 | } while(pwm_to_rechtsonder_motorB <= 0); |
Tess | 1:1c22ce9f370b | 473 | pwm_motorB.write(0); |
jorick92 | 0:73dcc8fb9900 | 474 | |
Tess | 1:1c22ce9f370b | 475 | wait(1); |
Tess | 1:1c22ce9f370b | 476 | |
Tess | 1:1c22ce9f370b | 477 | // Nu zijn de motoren gekalibreed en staan ze op de startpositie (rechtsonderhoek van A4). |
Tess | 1:1c22ce9f370b | 478 | // Hierna het script dat EMG wordt omgezet in een positie verandering |
jorick92 | 0:73dcc8fb9900 | 479 | |
Tess | 1:1c22ce9f370b | 480 | /*Create a ticker, and let it call the function 'setlooptimerflag' every 0.01s */ |
Tess | 1:1c22ce9f370b | 481 | Ticker looptimer; |
Tess | 1:1c22ce9f370b | 482 | looptimer.attach(setlooptimerflag,0.01); |
jorick92 | 0:73dcc8fb9900 | 483 | |
Tess | 1:1c22ce9f370b | 484 | //INFINITE LOOP |
Tess | 1:1c22ce9f370b | 485 | while(1) { |
Tess | 1:1c22ce9f370b | 486 | |
Tess | 1:1c22ce9f370b | 487 | while(looptimerflag != true); |
Tess | 1:1c22ce9f370b | 488 | looptimerflag = false; |
jorick92 | 0:73dcc8fb9900 | 489 | |
Tess | 1:1c22ce9f370b | 490 | // HIER EMG!! |
Tess | 1:1c22ce9f370b | 491 | float emg_value_biceps; |
Tess | 1:1c22ce9f370b | 492 | float emg_value_triceps; |
Tess | 1:1c22ce9f370b | 493 | float emg_value_flexoren; |
Tess | 1:1c22ce9f370b | 494 | float emg_value_extensoren; |
Tess | 1:1c22ce9f370b | 495 | float dy; // verschil tussen biceps en triceps, daarmee snelheid en richting aangeven. |
Tess | 2:55bff07c1058 | 496 | float dx; |
Tess | 2:55bff07c1058 | 497 | |
Tess | 1:1c22ce9f370b | 498 | emg_value_biceps = ((100*(filter(1))-0.18)/0.49); // dit is om waarde tussen 0 en 1 te krijgen. filter(1) zegt biceps, 0.18 offset aftrekken, 0.49 maximale waarde, 100 omdat procent |
Tess | 1:1c22ce9f370b | 499 | emg_value_triceps = ((100*(filter(2))-0.18)/0.35); // 0.35 maximale waarde van triceps |
Tess | 2:55bff07c1058 | 500 | //emg_value_flexoren = ((100*filter(3))-0.00000)/0.000); |
Tess | 2:55bff07c1058 | 501 | //emg_value_extensoren = ((100*filter(4))-0.00000)/0.0000); |
jorick92 | 0:73dcc8fb9900 | 502 | |
Tess | 1:1c22ce9f370b | 503 | if(emg_value_biceps < 0.10) { // lager dan 10% doe dan niks - threshold |
Tess | 1:1c22ce9f370b | 504 | emg_value_biceps=0; |
Tess | 1:1c22ce9f370b | 505 | } else { |
Tess | 1:1c22ce9f370b | 506 | emg_value_biceps = emg_value_biceps; |
Tess | 1:1c22ce9f370b | 507 | } |
Tess | 1:1c22ce9f370b | 508 | if(emg_value_triceps < 0.20) { // lager dan 20% doe dan niks - threshold - triceps fluctueren meer |
Tess | 1:1c22ce9f370b | 509 | emg_value_triceps=0; |
Tess | 1:1c22ce9f370b | 510 | } else { |
Tess | 1:1c22ce9f370b | 511 | emg_value_triceps=emg_value_triceps; |
Tess | 1:1c22ce9f370b | 512 | } |
Tess | 2:55bff07c1058 | 513 | /*if(emg_value_flexoren < 0.10){ |
Tess | 2:55bff07c1058 | 514 | emg_value_flexoren = 0; |
Tess | 2:55bff07c1058 | 515 | } else { |
Tess | 2:55bff07c1058 | 516 | emg_value_flexoren = emg_value_flexoren; |
Tess | 2:55bff07c1058 | 517 | } |
Tess | 2:55bff07c1058 | 518 | if(emg_value_extensoren<0.20){ |
Tess | 2:55bff07c1058 | 519 | emg_value_extensoren = 0; |
Tess | 2:55bff07c1058 | 520 | } else { |
Tess | 2:55bff07c1058 | 521 | emg_value_extensoren = emg_value_extensoren; |
Tess | 2:55bff07c1058 | 522 | }*/ |
jorick92 | 0:73dcc8fb9900 | 523 | |
Tess | 1:1c22ce9f370b | 524 | dy = emg_value_biceps - emg_value_triceps; |
Tess | 2:55bff07c1058 | 525 | // dx = emg_value_flexoren - emg_value_extensoren; |
Tess | 1:1c22ce9f370b | 526 | if(pc.rxBufferGetSize(0)-pc.rxBufferGetCount() > 30) |
Tess | 1:1c22ce9f370b | 527 | pc.printf("%.6f\n",dy); |
Tess | 1:1c22ce9f370b | 528 | |
Tess | 2:55bff07c1058 | 529 | // inverse kinematica |
Tess | 2:55bff07c1058 | 530 | phi_A_pulsen_positie_begin = motorA.getPosition(); |
Tess | 2:55bff07c1058 | 531 | phi_B_pulsen_positie_begin = motorB.getPosition(); |
Tess | 2:55bff07c1058 | 532 | |
Tess | 2:55bff07c1058 | 533 | phi_A_positie_begin = (360/3200) * phi_A_pulsen_positie_begin; |
Tess | 2:55bff07c1058 | 534 | phi_B_positie_begin = (360/3200) * phi_B_pulsen_positie_begin; |
Tess | 2:55bff07c1058 | 535 | |
Tess | 2:55bff07c1058 | 536 | phi_1 = phi_A_positie_begin - phi_B_positie_begin; |
Tess | 2:55bff07c1058 | 537 | |
Tess | 2:55bff07c1058 | 538 | X_positie_begin = 2 * lengte_arm * sin(0.5 * phi_1) * cos(90 - 0.5 * phi_A_positie_begin - 0.5 * phi_B_positie_begin); |
Tess | 2:55bff07c1058 | 539 | Y_positie_begin = 2 * lengte_arm * sin(0.5 * phi_1) * sin(90 - 0.5 * phi_A_positie_begin - 0.5 * phi_B_positie_begin); |
Tess | 2:55bff07c1058 | 540 | |
Tess | 2:55bff07c1058 | 541 | X_positie = dx * vmax * delta_t + X_positie_begin; |
Tess | 2:55bff07c1058 | 542 | Y_positie = dy * vmax * delta_t + Y_positie_begin; |
Tess | 2:55bff07c1058 | 543 | |
Tess | 2:55bff07c1058 | 544 | kwadraat_X_positie = pow(X_positie,2); |
Tess | 2:55bff07c1058 | 545 | kwadraat_Y_positie = pow(Y_positie,2); |
Tess | 2:55bff07c1058 | 546 | |
Tess | 2:55bff07c1058 | 547 | phi_A = 180 - acos(sqrt(kwadraat_X_positie+kwadraat_Y_positie)/(2*lengte_arm)) - atan(Y_positie/X_positie); |
Tess | 2:55bff07c1058 | 548 | phi_B = 180 - phi_A - acos(-(kwadraat_X_positie + kwadraat_Y_positie) / (2 * pow(lengte_arm,2))+1); |
Tess | 2:55bff07c1058 | 549 | |
Tess | 2:55bff07c1058 | 550 | phi_A_pulsen = (3200/360) * phi_A; |
Tess | 2:55bff07c1058 | 551 | phi_B_pulsen = (3200/360) * phi_B; |
Tess | 2:55bff07c1058 | 552 | |
Tess | 2:55bff07c1058 | 553 | // motor A moet de hoek altijd binnen 53.4 tot en met 124.3 graden liggen |
Tess | 1:1c22ce9f370b | 554 | // motor B moet de hoek altijd binnen 30.2 tot en met -16.5 graden liggen |
Tess | 2:55bff07c1058 | 555 | keep_in_range(&phi_A_pulsen, -1104, -474); // voor motor moet bereik zijn -1104 tot -474 |
Tess | 2:55bff07c1058 | 556 | keep_in_range(&phi_B_pulsen, -146, 268); // voor motor moet bereik zijn -146 tot 268 |
Tess | 2:55bff07c1058 | 557 | |
Tess | 2:55bff07c1058 | 558 | Puls_motorA = phi_A_pulsen - phi_A_pulsen_positie_begin; |
Tess | 2:55bff07c1058 | 559 | Puls_motorB = phi_B_pulsen - phi_B_pulsen_positie_begin; |
jorick92 | 0:73dcc8fb9900 | 560 | |
Tess | 1:1c22ce9f370b | 561 | //PD regelaar voor motor A |
Tess | 1:1c22ce9f370b | 562 | wait(dt); |
Tess | 2:55bff07c1058 | 563 | error_ti_A = puls_motorA - motorA.getPosition(); |
Tess | 1:1c22ce9f370b | 564 | P_regelaar_A = Kp * error_ti_A; |
Tess | 1:1c22ce9f370b | 565 | D_regelaar_A = Kd * ((error_ti_A - error_t0_A) / dt); |
Tess | 1:1c22ce9f370b | 566 | error_t0_A = error_ti_A; |
Tess | 1:1c22ce9f370b | 567 | output_regelaar_A = P_regelaar_A + D_regelaar_A; |
jorick92 | 0:73dcc8fb9900 | 568 | |
Tess | 1:1c22ce9f370b | 569 | //PD regelaar voor motor B |
Tess | 1:1c22ce9f370b | 570 | wait(dt); |
Tess | 2:55bff07c1058 | 571 | error_ti_B = puls_motorB - motorB.getPosition(); |
Tess | 1:1c22ce9f370b | 572 | P_regelaar_B = Kp * error_ti_B; |
Tess | 1:1c22ce9f370b | 573 | D_regelaar_B = Kd * ((error_ti_B - error_t0_B) / dt); |
Tess | 1:1c22ce9f370b | 574 | error_t0_B = error_ti_B; |
Tess | 1:1c22ce9f370b | 575 | output_regelaar_B = P_regelaar_B + D_regelaar_B; |
jorick92 | 0:73dcc8fb9900 | 576 | |
Tess | 1:1c22ce9f370b | 577 | /* This is a PD-action! store in pwm_to_motor */ |
Tess | 1:1c22ce9f370b | 578 | pwm_to_motorA = output_regelaar_A; |
Tess | 1:1c22ce9f370b | 579 | pwm_to_motorB = output_regelaar_B; |
jorick92 | 0:73dcc8fb9900 | 580 | |
Tess | 1:1c22ce9f370b | 581 | keep_in_range(&pwm_to_motorA, -1,1); |
Tess | 1:1c22ce9f370b | 582 | keep_in_range(&pwm_to_motorB, -1,1); |
jorick92 | 0:73dcc8fb9900 | 583 | |
Tess | 1:1c22ce9f370b | 584 | if(pwm_to_motorA > 0) |
Tess | 1:1c22ce9f370b | 585 | motordirA.write(1); |
Tess | 1:1c22ce9f370b | 586 | else |
Tess | 1:1c22ce9f370b | 587 | motordirA.write(0); |
Tess | 1:1c22ce9f370b | 588 | if(pwm_to_motorB > 0) |
Tess | 1:1c22ce9f370b | 589 | motordirB.write(1); |
Tess | 1:1c22ce9f370b | 590 | else |
Tess | 1:1c22ce9f370b | 591 | motordirB.write(0); |
Tess | 1:1c22ce9f370b | 592 | |
Tess | 1:1c22ce9f370b | 593 | pwm_motorA.write(abs(pwm_to_motorA)); |
Tess | 1:1c22ce9f370b | 594 | pwm_motorB.write(abs(pwm_to_motorB)); |
jorick92 | 0:73dcc8fb9900 | 595 | } |
jorick92 | 0:73dcc8fb9900 | 596 | } |
jorick92 | 0:73dcc8fb9900 | 597 | |
jorick92 | 0:73dcc8fb9900 | 598 | void keep_in_range(float * in, float min, float max) |
jorick92 | 0:73dcc8fb9900 | 599 | { |
jorick92 | 0:73dcc8fb9900 | 600 | *in > min ? *in < max? : *in = max: *in = min; |
jorick92 | 0:73dcc8fb9900 | 601 | } |
jorick92 | 0:73dcc8fb9900 | 602 | |
jorick92 | 0:73dcc8fb9900 | 603 | |
jorick92 | 0:73dcc8fb9900 | 604 | |
jorick92 | 0:73dcc8fb9900 | 605 | |
jorick92 | 0:73dcc8fb9900 | 606 | |
jorick92 | 0:73dcc8fb9900 | 607 |