Tess Groeneveld / motoraansturing_met_EMG_met_alles_versie2

Version 1: coming together. With PD controller but without inverse kinematics

Dependencies:   Encoder MODSERIAL mbed

Fork of motoraansturing_met_EMG by Jorick Leferink

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?

UserRevisionLine numberNew 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