Jorick Leferink / motoraansturing_met_EMG

moet nog aan de gains worden gesleuteld

Dependencies:   Encoder MODSERIAL mbed

main.cpp@0:73dcc8fb9900, 2013-11-01 (annotated)

Committer:
jorick92
Date:
Fri Nov 01 08:24:02 2013 +0000
Revision:
0:73dcc8fb9900
moet nog aan de gains worden gesleuteld

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
jorick92 0:73dcc8fb9900 6 #define NUM0 0.2754 // constante
jorick92 0:73dcc8fb9900 7 #define NUM1 -1.1017 // z^-1
jorick92 0:73dcc8fb9900 8 #define NUM2 1.6525 // z^-2etc.
jorick92 0:73dcc8fb9900 9 #define NUM3 -1.1017
jorick92 0:73dcc8fb9900 10 #define NUM4 0.2754
jorick92 0:73dcc8fb9900 11
jorick92 0:73dcc8fb9900 12 #define DEN0 1 // constante
jorick92 0:73dcc8fb9900 13 #define DEN1 -1.5704
jorick92 0:73dcc8fb9900 14 #define DEN2 1.2756
jorick92 0:73dcc8fb9900 15 #define DEN3 -0.4844
jorick92 0:73dcc8fb9900 16 #define DEN4 0.0762
jorick92 0:73dcc8fb9900 17
jorick92 0:73dcc8fb9900 18 //lowpass filter constantes 40 Hz 4e orde
jorick92 0:73dcc8fb9900 19 #define NUM0_2 0.4328 // constante
jorick92 0:73dcc8fb9900 20 #define NUM1_2 1.7314 // z^-1
jorick92 0:73dcc8fb9900 21 #define NUM2_2 2.5971 // z^-2etc.
jorick92 0:73dcc8fb9900 22 #define NUM3_2 1.7314
jorick92 0:73dcc8fb9900 23 #define NUM4_2 0.4328
jorick92 0:73dcc8fb9900 24
jorick92 0:73dcc8fb9900 25
jorick92 0:73dcc8fb9900 26 #define DEN0_2 1 // constante
jorick92 0:73dcc8fb9900 27 #define DEN1_2 2.3695
jorick92 0:73dcc8fb9900 28 #define DEN2_2 2.3140
jorick92 0:73dcc8fb9900 29 #define DEN3_2 1.0547
jorick92 0:73dcc8fb9900 30 #define DEN4_2 0.1874
jorick92 0:73dcc8fb9900 31
jorick92 0:73dcc8fb9900 32 //lowpass filter constantes 3Hz 4e orde
jorick92 0:73dcc8fb9900 33 #define NUM0_3 0.0000624 // constante
jorick92 0:73dcc8fb9900 34 #define NUM1_3 0.0002495 // z^-1
jorick92 0:73dcc8fb9900 35 #define NUM2_3 0.0003743 // z^-2etc.
jorick92 0:73dcc8fb9900 36 #define NUM3_3 0.0002495
jorick92 0:73dcc8fb9900 37 #define NUM4_3 0.0000624
jorick92 0:73dcc8fb9900 38
jorick92 0:73dcc8fb9900 39 #define DEN0_3 1 // constante
jorick92 0:73dcc8fb9900 40 #define DEN1_3 -3.5078
jorick92 0:73dcc8fb9900 41 #define DEN2_3 4.6409
jorick92 0:73dcc8fb9900 42 #define DEN3_3 -2.7427
jorick92 0:73dcc8fb9900 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 // dit is voor schakelaar die aan en uit wil gaan
jorick92 0:73dcc8fb9900 52 DigitalIn toggle(PTD7);
jorick92 0:73dcc8fb9900 53
jorick92 0:73dcc8fb9900 54 void toggle_on()
jorick92 0:73dcc8fb9900 55 {
jorick92 0:73dcc8fb9900 56 }
jorick92 0:73dcc8fb9900 57
jorick92 0:73dcc8fb9900 58 void toggle_off()
jorick92 0:73dcc8fb9900 59 {
jorick92 0:73dcc8fb9900 60 // do nothing
jorick92 0:73dcc8fb9900 61 }
jorick92 0:73dcc8fb9900 62
jorick92 0:73dcc8fb9900 63 /** keep_in_range -> float in, and keep_in_range if less than min, or larger than max **/
jorick92 0:73dcc8fb9900 64 void keep_in_range(float * in, float min, float max);
jorick92 0:73dcc8fb9900 65
jorick92 0:73dcc8fb9900 66 /** variable to show when a new loop can be started*/
jorick92 0:73dcc8fb9900 67 /** volatile means that it can be changed in an */
jorick92 0:73dcc8fb9900 68 /** interrupt routine, and that that change is vis-*/
jorick92 0:73dcc8fb9900 69 /** ible in the main loop. */
jorick92 0:73dcc8fb9900 70
jorick92 0:73dcc8fb9900 71 volatile bool looptimerflag;
jorick92 0:73dcc8fb9900 72
jorick92 0:73dcc8fb9900 73 /** function called by Ticker "looptimer" */
jorick92 0:73dcc8fb9900 74 /** variable 'looptimerflag' is set to 'true' */
jorick92 0:73dcc8fb9900 75 /** each time the looptimer expires. */
jorick92 0:73dcc8fb9900 76 void setlooptimerflag(void)
jorick92 0:73dcc8fb9900 77 {
jorick92 0:73dcc8fb9900 78 looptimerflag = true;
jorick92 0:73dcc8fb9900 79 }
jorick92 0:73dcc8fb9900 80
jorick92 0:73dcc8fb9900 81 //emg variabelen
jorick92 0:73dcc8fb9900 82 float emg_value_biceps, emg_value_triceps, emg_value_flexoren, emg_value_extensoren, dy;
jorick92 0:73dcc8fb9900 83 AnalogIn emg_biceps(PTB0); //Analog input
jorick92 0:73dcc8fb9900 84 AnalogIn emg_triceps(PTB1);
jorick92 0:73dcc8fb9900 85 AnalogIn emg_flexoren(PTB2);
jorick92 0:73dcc8fb9900 86 AnalogIn emg_extensoren(PTB3);
jorick92 0:73dcc8fb9900 87
jorick92 0:73dcc8fb9900 88 /*
jorick92 0:73dcc8fb9900 89 DIT IS DE FILTER FUNCTIE! aanroepen door "filter(signaal nummer)"
jorick92 0:73dcc8fb9900 90 filter(1): biceps meten
jorick92 0:73dcc8fb9900 91 filter(2): triceps meten
jorick92 0:73dcc8fb9900 92 filter(3): flexoren meten
jorick92 0:73dcc8fb9900 93 filter(3): extensoren meten
jorick92 0:73dcc8fb9900 94 */
jorick92 0:73dcc8fb9900 95 float filter(int sig_number){
jorick92 0:73dcc8fb9900 96 float sig_out;
jorick92 0:73dcc8fb9900 97
jorick92 0:73dcc8fb9900 98 // eerst variabelen definieren
jorick92 0:73dcc8fb9900 99
jorick92 0:73dcc8fb9900 100 //biceps
jorick92 0:73dcc8fb9900 101 //filter 1
jorick92 0:73dcc8fb9900 102 float in0_biceps =0;
jorick92 0:73dcc8fb9900 103 static float in1_biceps =0, in2_biceps = 0, in3_biceps = 0, in4_biceps = 0;
jorick92 0:73dcc8fb9900 104 static float out0_biceps = 0, out1_biceps = 0 , out2_biceps = 0, out3_biceps = 0, out4_biceps = 0;
jorick92 0:73dcc8fb9900 105 //filter 2
jorick92 0:73dcc8fb9900 106 float in0_2_biceps =0;
jorick92 0:73dcc8fb9900 107 static float in1_2_biceps =0, in2_2_biceps = 0, in3_2_biceps = 0, in4_2_biceps = 0;
jorick92 0:73dcc8fb9900 108 static float out0_2_biceps = 0, out1_2_biceps = 0 , out2_2_biceps = 0, out3_2_biceps = 0, out4_2_biceps = 0;
jorick92 0:73dcc8fb9900 109 //filter 3
jorick92 0:73dcc8fb9900 110 float in0_3_biceps =0;
jorick92 0:73dcc8fb9900 111 static float in1_3_biceps =0, in2_3_biceps = 0, in3_3_biceps = 0, in4_3_biceps = 0;
jorick92 0:73dcc8fb9900 112 static float out0_3_biceps = 0, out1_3_biceps = 0 , out2_3_biceps = 0, out3_3_biceps = 0, out4_3_biceps = 0;
jorick92 0:73dcc8fb9900 113
jorick92 0:73dcc8fb9900 114 //triceps
jorick92 0:73dcc8fb9900 115 //filter 1
jorick92 0:73dcc8fb9900 116 float in0_triceps =0;
jorick92 0:73dcc8fb9900 117 static float in1_triceps =0, in2_triceps = 0, in3_triceps = 0, in4_triceps = 0;
jorick92 0:73dcc8fb9900 118 static float out0_triceps = 0, out1_triceps = 0 , out2_triceps = 0, out3_triceps = 0, out4_triceps = 0;
jorick92 0:73dcc8fb9900 119 //filter 2
jorick92 0:73dcc8fb9900 120 float in0_2_triceps =0;
jorick92 0:73dcc8fb9900 121 static float in1_2_triceps =0, in2_2_triceps = 0, in3_2_triceps = 0, in4_2_triceps = 0;
jorick92 0:73dcc8fb9900 122 static float out0_2_triceps = 0, out1_2_triceps = 0 , out2_2_triceps = 0, out3_2_triceps = 0, out4_2_triceps = 0;
jorick92 0:73dcc8fb9900 123 //filter 3
jorick92 0:73dcc8fb9900 124 float in0_3_triceps =0;
jorick92 0:73dcc8fb9900 125 static float in1_3_triceps =0, in2_3_triceps = 0, in3_3_triceps = 0, in4_3_triceps = 0;
jorick92 0:73dcc8fb9900 126 static float out0_3_triceps = 0, out1_3_triceps = 0 , out2_3_triceps = 0, out3_3_triceps = 0, out4_3_triceps = 0;
jorick92 0:73dcc8fb9900 127
jorick92 0:73dcc8fb9900 128 //flexoren
jorick92 0:73dcc8fb9900 129 //filter 1
jorick92 0:73dcc8fb9900 130 float in0_flexoren =0;
jorick92 0:73dcc8fb9900 131 static float in1_flexoren =0, in2_flexoren = 0, in3_flexoren = 0, in4_flexoren = 0;
jorick92 0:73dcc8fb9900 132 static float out0_flexoren = 0, out1_flexoren = 0 , out2_flexoren = 0, out3_flexoren = 0, out4_flexoren = 0;
jorick92 0:73dcc8fb9900 133 //filter 2
jorick92 0:73dcc8fb9900 134 float in0_2_flexoren =0;
jorick92 0:73dcc8fb9900 135 static float in1_2_flexoren =0, in2_2_flexoren = 0, in3_2_flexoren = 0, in4_2_flexoren = 0;
jorick92 0:73dcc8fb9900 136 static float out0_2_flexoren = 0, out1_2_flexoren = 0 , out2_2_flexoren = 0, out3_2_flexoren = 0, out4_2_flexoren = 0;
jorick92 0:73dcc8fb9900 137 //filter 3
jorick92 0:73dcc8fb9900 138 float in0_3_flexoren =0;
jorick92 0:73dcc8fb9900 139 static float in1_3_flexoren =0, in2_3_flexoren = 0, in3_3_flexoren = 0, in4_3_flexoren = 0;
jorick92 0:73dcc8fb9900 140 static float out0_3_flexoren = 0, out1_3_flexoren = 0 , out2_3_flexoren = 0, out3_3_flexoren = 0, out4_3_flexoren = 0;
jorick92 0:73dcc8fb9900 141
jorick92 0:73dcc8fb9900 142 //extensoren
jorick92 0:73dcc8fb9900 143 //filter 1
jorick92 0:73dcc8fb9900 144 float in0_extensoren =0;
jorick92 0:73dcc8fb9900 145 static float in1_extensoren =0, in2_extensoren = 0, in3_extensoren = 0, in4_extensoren = 0;
jorick92 0:73dcc8fb9900 146 static float out0_extensoren = 0, out1_extensoren = 0 , out2_extensoren = 0, out3_extensoren = 0, out4_extensoren = 0;
jorick92 0:73dcc8fb9900 147 //filter 2
jorick92 0:73dcc8fb9900 148 float in0_2_extensoren =0;
jorick92 0:73dcc8fb9900 149 static float in1_2_extensoren =0, in2_2_extensoren = 0, in3_2_extensoren = 0, in4_2_extensoren = 0;
jorick92 0:73dcc8fb9900 150 static float out0_2_extensoren = 0, out1_2_extensoren = 0 , out2_2_extensoren = 0, out3_2_extensoren = 0, out4_2_extensoren = 0;
jorick92 0:73dcc8fb9900 151 //filter 3
jorick92 0:73dcc8fb9900 152 float in0_3_extensoren =0;
jorick92 0:73dcc8fb9900 153 static float in1_3_extensoren =0, in2_3_extensoren = 0, in3_3_extensoren = 0, in4_3_extensoren = 0;
jorick92 0:73dcc8fb9900 154 static float out0_3_extensoren = 0, out1_3_extensoren = 0 , out2_3_extensoren = 0, out3_3_extensoren = 0, out4_3_extensoren = 0;
jorick92 0:73dcc8fb9900 155
jorick92 0:73dcc8fb9900 156
jorick92 0:73dcc8fb9900 157 switch(sig_number){
jorick92 0:73dcc8fb9900 158 case 1:
jorick92 0:73dcc8fb9900 159 // signaal filteren op 15 Hz HIGHPASS
jorick92 0:73dcc8fb9900 160 in4_biceps = in3_biceps; in3_biceps = in2_biceps; in2_biceps = in1_biceps; in1_biceps = in0_biceps;
jorick92 0:73dcc8fb9900 161 in0_biceps = emg_biceps.read();
jorick92 0:73dcc8fb9900 162 out4_biceps = out3_biceps; out3_biceps = out2_biceps; out2_biceps = out1_biceps; out1_biceps = out0_biceps;
jorick92 0:73dcc8fb9900 163 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;
jorick92 0:73dcc8fb9900 164
jorick92 0:73dcc8fb9900 165 //signaal filteren op 40 HZ LOWPASS
jorick92 0:73dcc8fb9900 166 in4_2_biceps = in3_2_biceps; in3_2_biceps = in2_2_biceps; in2_2_biceps = in1_2_biceps; in1_2_biceps = in0_2_biceps;
jorick92 0:73dcc8fb9900 167 in0_2_biceps = out0_biceps;
jorick92 0:73dcc8fb9900 168 out4_2_biceps = out3_2_biceps; out3_2_biceps = out2_2_biceps; out2_2_biceps = out1_2_biceps; out1_2_biceps = out0_2_biceps;
jorick92 0:73dcc8fb9900 169 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;
jorick92 0:73dcc8fb9900 170
jorick92 0:73dcc8fb9900 171 //signaal filteren op 5Hz LOWPASS
jorick92 0:73dcc8fb9900 172 in4_3_biceps = in3_3_biceps; in3_3_biceps = in2_3_biceps; in2_3_biceps = in1_3_biceps; in1_3_biceps = in0_3_biceps;
jorick92 0:73dcc8fb9900 173 in0_3_biceps = abs(out0_2_biceps);
jorick92 0:73dcc8fb9900 174 out4_3_biceps = out3_3_biceps; out3_3_biceps = out2_3_biceps; out2_3_biceps = out1_3_biceps; out1_3_biceps = out0_3_biceps;
jorick92 0:73dcc8fb9900 175 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 176 sig_out = out0_3_biceps;
jorick92 0:73dcc8fb9900 177 break;
jorick92 0:73dcc8fb9900 178 case 2:
jorick92 0:73dcc8fb9900 179 // signaal filteren op 15 Hz HIGHPASS
jorick92 0:73dcc8fb9900 180 in4_triceps = in3_triceps; in3_triceps = in2_triceps; in2_triceps = in1_triceps; in1_triceps = in0_triceps;
jorick92 0:73dcc8fb9900 181 in0_triceps = emg_triceps.read();
jorick92 0:73dcc8fb9900 182 out4_triceps = out3_triceps; out3_triceps = out2_triceps; out2_triceps = out1_triceps; out1_triceps = out0_triceps;
jorick92 0:73dcc8fb9900 183 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;
jorick92 0:73dcc8fb9900 184
jorick92 0:73dcc8fb9900 185 //signaal filteren op 40 HZ LOWPASS
jorick92 0:73dcc8fb9900 186 in4_2_triceps = in3_2_triceps; in3_2_triceps = in2_2_triceps; in2_2_triceps = in1_2_triceps; in1_2_triceps = in0_2_triceps;
jorick92 0:73dcc8fb9900 187 in0_2_triceps = out0_triceps;
jorick92 0:73dcc8fb9900 188 out4_2_triceps = out3_2_triceps; out3_2_triceps = out2_2_triceps; out2_2_triceps = out1_2_triceps; out1_2_triceps = out0_2_triceps;
jorick92 0:73dcc8fb9900 189 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;
jorick92 0:73dcc8fb9900 190
jorick92 0:73dcc8fb9900 191 //signaal filteren op 5Hz LOWPASS
jorick92 0:73dcc8fb9900 192 in4_3_triceps = in3_3_triceps; in3_3_triceps = in2_3_triceps; in2_3_triceps = in1_3_triceps; in1_3_triceps = in0_3_triceps;
jorick92 0:73dcc8fb9900 193 in0_3_triceps = abs(out0_2_triceps);
jorick92 0:73dcc8fb9900 194 out4_3_triceps = out3_3_triceps; out3_3_triceps = out2_3_triceps; out2_3_triceps = out1_3_triceps; out1_3_triceps = out0_3_triceps;
jorick92 0:73dcc8fb9900 195 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 196 sig_out = out0_3_triceps;
jorick92 0:73dcc8fb9900 197 break;
jorick92 0:73dcc8fb9900 198 case 3:
jorick92 0:73dcc8fb9900 199 // signaal filteren op 15 Hz HIGHPASS
jorick92 0:73dcc8fb9900 200 in4_flexoren = in3_flexoren; in3_flexoren = in2_flexoren; in2_flexoren = in1_flexoren; in1_flexoren = in0_flexoren;
jorick92 0:73dcc8fb9900 201 in0_flexoren = emg_flexoren.read();
jorick92 0:73dcc8fb9900 202 out4_flexoren = out3_flexoren; out3_flexoren = out2_flexoren; out2_flexoren = out1_flexoren; out1_flexoren = out0_flexoren;
jorick92 0:73dcc8fb9900 203 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;
jorick92 0:73dcc8fb9900 204
jorick92 0:73dcc8fb9900 205 //signaal filteren op 40 HZ LOWPASS
jorick92 0:73dcc8fb9900 206 in4_2_flexoren = in3_2_flexoren; in3_2_flexoren = in2_2_flexoren; in2_2_flexoren = in1_2_flexoren; in1_2_flexoren = in0_2_flexoren;
jorick92 0:73dcc8fb9900 207 in0_2_flexoren = out0_flexoren;
jorick92 0:73dcc8fb9900 208 out4_2_flexoren = out3_2_flexoren; out3_2_flexoren = out2_2_flexoren; out2_2_flexoren = out1_2_flexoren; out1_2_flexoren = out0_2_flexoren;
jorick92 0:73dcc8fb9900 209 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;
jorick92 0:73dcc8fb9900 210
jorick92 0:73dcc8fb9900 211 //signaal filteren op 5Hz LOWPASS
jorick92 0:73dcc8fb9900 212 in4_3_flexoren = in3_3_flexoren; in3_3_flexoren = in2_3_flexoren; in2_3_flexoren = in1_3_flexoren; in1_3_flexoren = in0_3_flexoren;
jorick92 0:73dcc8fb9900 213 in0_3_flexoren = abs(out0_2_flexoren);
jorick92 0:73dcc8fb9900 214 out4_3_flexoren = out3_3_flexoren; out3_3_flexoren = out2_3_flexoren; out2_3_flexoren = out1_3_flexoren; out1_3_flexoren = out0_3_flexoren;
jorick92 0:73dcc8fb9900 215 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 216 sig_out = out0_3_flexoren;
jorick92 0:73dcc8fb9900 217 break;
jorick92 0:73dcc8fb9900 218 case 4:
jorick92 0:73dcc8fb9900 219 // signaal filteren op 15 Hz HIGHPASS
jorick92 0:73dcc8fb9900 220 in4_extensoren = in3_extensoren; in3_extensoren = in2_extensoren; in2_extensoren = in1_extensoren; in1_extensoren = in0_extensoren;
jorick92 0:73dcc8fb9900 221 in0_extensoren = emg_extensoren.read();
jorick92 0:73dcc8fb9900 222 out4_extensoren = out3_extensoren; out3_extensoren = out2_extensoren; out2_extensoren = out1_extensoren; out1_extensoren = out0_extensoren;
jorick92 0:73dcc8fb9900 223 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;
jorick92 0:73dcc8fb9900 224
jorick92 0:73dcc8fb9900 225 //signaal filteren op 40 HZ LOWPASS
jorick92 0:73dcc8fb9900 226 in4_2_extensoren = in3_2_extensoren; in3_2_extensoren = in2_2_extensoren; in2_2_extensoren = in1_2_extensoren; in1_2_extensoren = in0_2_extensoren;
jorick92 0:73dcc8fb9900 227 in0_2_extensoren = out0_extensoren;
jorick92 0:73dcc8fb9900 228 out4_2_extensoren = out3_2_extensoren; out3_2_extensoren = out2_2_extensoren; out2_2_extensoren = out1_2_extensoren; out1_2_extensoren = out0_2_extensoren;
jorick92 0:73dcc8fb9900 229 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;
jorick92 0:73dcc8fb9900 230
jorick92 0:73dcc8fb9900 231 //signaal filteren op 5Hz LOWPASS
jorick92 0:73dcc8fb9900 232 in4_3_extensoren = in3_3_extensoren; in3_3_extensoren = in2_3_extensoren; in2_3_extensoren = in1_3_extensoren; in1_3_extensoren = in0_3_extensoren;
jorick92 0:73dcc8fb9900 233 in0_3_extensoren = abs(out0_2_extensoren);
jorick92 0:73dcc8fb9900 234 out4_3_extensoren = out3_3_extensoren; out3_3_extensoren = out2_3_extensoren; out2_3_extensoren = out1_3_extensoren; out1_3_extensoren = out0_3_extensoren;
jorick92 0:73dcc8fb9900 235 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 236 sig_out = out0_3_extensoren;
jorick92 0:73dcc8fb9900 237 break;
jorick92 0:73dcc8fb9900 238 }
jorick92 0:73dcc8fb9900 239 return sig_out;
jorick92 0:73dcc8fb9900 240 }
jorick92 0:73dcc8fb9900 241
jorick92 0:73dcc8fb9900 242 int main()
jorick92 0:73dcc8fb9900 243 {
jorick92 0:73dcc8fb9900 244 //LOCAL VARIABLES
jorick92 0:73dcc8fb9900 245 /*Potmeter input*/
jorick92 0:73dcc8fb9900 246 AnalogIn potmeterA(PTC2);
jorick92 0:73dcc8fb9900 247 AnalogIn potmeterB(PTB2);
jorick92 0:73dcc8fb9900 248 /* Encoder, using my encoder library */
jorick92 0:73dcc8fb9900 249 /* First pin should be PTDx or PTAx */
jorick92 0:73dcc8fb9900 250 /* because those pins can be used as */
jorick92 0:73dcc8fb9900 251 /* InterruptIn */
jorick92 0:73dcc8fb9900 252 Encoder motorA(PTD4,PTC8);
jorick92 0:73dcc8fb9900 253 Encoder motorB(PTD0,PTD2);
jorick92 0:73dcc8fb9900 254 /* MODSERIAL to get non-blocking Serial*/
jorick92 0:73dcc8fb9900 255 MODSERIAL pc(USBTX,USBRX);
jorick92 0:73dcc8fb9900 256 /* PWM control to motor */
jorick92 0:73dcc8fb9900 257 PwmOut pwm_motorA(PTA12);
jorick92 0:73dcc8fb9900 258 PwmOut pwm_motorB(PTA5);
jorick92 0:73dcc8fb9900 259 /* Direction pin */
jorick92 0:73dcc8fb9900 260 DigitalOut motordirA(PTD3);
jorick92 0:73dcc8fb9900 261 DigitalOut motordirB(PTD1);
jorick92 0:73dcc8fb9900 262 /* variable to store setpoint in */
jorick92 0:73dcc8fb9900 263 float setpointA;
jorick92 0:73dcc8fb9900 264 float setpointB;
jorick92 0:73dcc8fb9900 265 float setpoint_beginA;
jorick92 0:73dcc8fb9900 266 float setpoint_beginB;
jorick92 0:73dcc8fb9900 267 float setpoint_rechtsonderA;
jorick92 0:73dcc8fb9900 268 float setpoint_rechtsonderB;
jorick92 0:73dcc8fb9900 269
jorick92 0:73dcc8fb9900 270 /* variable to store pwm value in*/
jorick92 0:73dcc8fb9900 271 float pwm_to_motorA;
jorick92 0:73dcc8fb9900 272 float pwm_to_begin_motorA = 0;
jorick92 0:73dcc8fb9900 273 float pwm_to_begin_motorB = 0;
jorick92 0:73dcc8fb9900 274 float pwm_to_motorB;
jorick92 0:73dcc8fb9900 275 float pwm_to_rechtsonder_motorA;
jorick92 0:73dcc8fb9900 276 float pwm_to_rechtsonder_motorB;
jorick92 0:73dcc8fb9900 277
jorick92 0:73dcc8fb9900 278 const float dt = 0.002;
jorick92 0:73dcc8fb9900 279 float Kp = 0.001; //0.0113
jorick92 0:73dcc8fb9900 280 float Ki = 0.0759;
jorick92 0:73dcc8fb9900 281 float Kd = 0.0004342;
jorick92 0:73dcc8fb9900 282 float error_t0_A = 0;
jorick92 0:73dcc8fb9900 283 float error_t0_B = 0;
jorick92 0:73dcc8fb9900 284 float error_ti_A;
jorick92 0:73dcc8fb9900 285 float error_ti_B;
jorick92 0:73dcc8fb9900 286 float error_t_1_A;
jorick92 0:73dcc8fb9900 287 float error_t_1_B;
jorick92 0:73dcc8fb9900 288 float P_regelaar_A;
jorick92 0:73dcc8fb9900 289 float P_regelaar_B;
jorick92 0:73dcc8fb9900 290 float I_regelaar_A;
jorick92 0:73dcc8fb9900 291 float I_regelaar_B;
jorick92 0:73dcc8fb9900 292 float D_regelaar_A;
jorick92 0:73dcc8fb9900 293 float D_regelaar_B;
jorick92 0:73dcc8fb9900 294 float output_regelaar_A;
jorick92 0:73dcc8fb9900 295 float output_regelaar_B;
jorick92 0:73dcc8fb9900 296 float integral_i_A;
jorick92 0:73dcc8fb9900 297 float integral_i_B;
jorick92 0:73dcc8fb9900 298 float integral_0_A = 0;
jorick92 0:73dcc8fb9900 299 float integral_0_B = 0;
jorick92 0:73dcc8fb9900 300
jorick92 0:73dcc8fb9900 301 int32_t positionmotorA_t0;
jorick92 0:73dcc8fb9900 302 int32_t positionmotorB_t0;
jorick92 0:73dcc8fb9900 303 int32_t positionmotorA_t_1;
jorick92 0:73dcc8fb9900 304 int32_t positionmotorB_t_1;
jorick92 0:73dcc8fb9900 305 int32_t positiondifference_motorA;
jorick92 0:73dcc8fb9900 306 int32_t positiondifference_motorB;
jorick92 0:73dcc8fb9900 307
jorick92 0:73dcc8fb9900 308 //START OF CODE
jorick92 0:73dcc8fb9900 309
jorick92 0:73dcc8fb9900 310 while(1) {
jorick92 0:73dcc8fb9900 311 while(!toggle);
jorick92 0:73dcc8fb9900 312 { // wait while toggle == 0
jorick92 0:73dcc8fb9900 313 toggle_on();
jorick92 0:73dcc8fb9900 314
jorick92 0:73dcc8fb9900 315 /*Set the baudrate (use this number in RealTerm too!) */
jorick92 0:73dcc8fb9900 316 pc.baud(115200);
jorick92 0:73dcc8fb9900 317
jorick92 0:73dcc8fb9900 318 // 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.
jorick92 0:73dcc8fb9900 319 // motor B zit onder en motor A zit boven en dus op zijn kop (en dus setpoint moet - zijn).
jorick92 0:73dcc8fb9900 320
jorick92 0:73dcc8fb9900 321 motordirB.write(0);
jorick92 0:73dcc8fb9900 322 pwm_motorB.write(.08);
jorick92 0:73dcc8fb9900 323 positionmotorB_t0 = motorB.getPosition();
jorick92 0:73dcc8fb9900 324 do {
jorick92 0:73dcc8fb9900 325 wait(0.2);
jorick92 0:73dcc8fb9900 326 positionmotorB_t_1 = positionmotorB_t0 ;
jorick92 0:73dcc8fb9900 327 positionmotorB_t0 = motorB.getPosition();
jorick92 0:73dcc8fb9900 328 positiondifference_motorB = abs(positionmotorB_t0 - positionmotorB_t_1);
jorick92 0:73dcc8fb9900 329 } while(positiondifference_motorB > 10);
jorick92 0:73dcc8fb9900 330 motorB.setPosition(0);
jorick92 0:73dcc8fb9900 331 pwm_motorB.write(0);
jorick92 0:73dcc8fb9900 332
jorick92 0:73dcc8fb9900 333 wait(1); // willen nu even dat tussen ene actie en andere actie 1 seconde wacht.
jorick92 0:73dcc8fb9900 334
jorick92 0:73dcc8fb9900 335 motordirA.write(1);
jorick92 0:73dcc8fb9900 336 pwm_motorA.write(.08);
jorick92 0:73dcc8fb9900 337 positionmotorA_t0 = motorA.getPosition();
jorick92 0:73dcc8fb9900 338 do {
jorick92 0:73dcc8fb9900 339 wait(0.2);
jorick92 0:73dcc8fb9900 340 positionmotorA_t_1 = positionmotorA_t0 ;
jorick92 0:73dcc8fb9900 341 positionmotorA_t0 = motorA.getPosition();
jorick92 0:73dcc8fb9900 342 positiondifference_motorA = abs(positionmotorA_t0 - positionmotorA_t_1);
jorick92 0:73dcc8fb9900 343 } while(positiondifference_motorA > 10);
jorick92 0:73dcc8fb9900 344 motorA.setPosition(0);
jorick92 0:73dcc8fb9900 345 pwm_motorA.write(0);
jorick92 0:73dcc8fb9900 346
jorick92 0:73dcc8fb9900 347 wait(1); // willen nu even dat tussen ene actie en andere actie 1 seconde wacht.
jorick92 0:73dcc8fb9900 348
jorick92 0:73dcc8fb9900 349 // 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 350
jorick92 0:73dcc8fb9900 351 motordirA.write(0);
jorick92 0:73dcc8fb9900 352 pwm_motorA.write(.08);
jorick92 0:73dcc8fb9900 353 do {
jorick92 0:73dcc8fb9900 354 setpoint_beginA = -63; // x-as
jorick92 0:73dcc8fb9900 355 pwm_to_begin_motorA = abs((setpoint_beginA + motorA.getPosition()) *.001); // + omdat men met een negatieve hoekverdraaiing werkt.
jorick92 0:73dcc8fb9900 356 wait(0.2);
jorick92 0:73dcc8fb9900 357 keep_in_range(&pwm_to_begin_motorA, -1, 1 );
jorick92 0:73dcc8fb9900 358 motordirA.write(0);
jorick92 0:73dcc8fb9900 359 pwm_motorA.write(pwm_to_begin_motorA);
jorick92 0:73dcc8fb9900 360 } while(pwm_to_begin_motorA <= 0);
jorick92 0:73dcc8fb9900 361 motorA.setPosition(0);
jorick92 0:73dcc8fb9900 362 pwm_motorA.write(0);
jorick92 0:73dcc8fb9900 363
jorick92 0:73dcc8fb9900 364 wait(1); // willen nu even dat tussen ene actie en andere actie 1 seconde wacht.
jorick92 0:73dcc8fb9900 365
jorick92 0:73dcc8fb9900 366 // hierna moet motor A naar de rechtsonder A4. Motor A 532.
jorick92 0:73dcc8fb9900 367
jorick92 0:73dcc8fb9900 368 motordirA.write(0);
jorick92 0:73dcc8fb9900 369 pwm_motorA.write(0.08);
jorick92 0:73dcc8fb9900 370 do {
jorick92 0:73dcc8fb9900 371 setpoint_beginA = -532; // rechtsonder positie A4
jorick92 0:73dcc8fb9900 372 pwm_to_begin_motorA = abs((setpoint_beginA + motorA.getPosition()) *.001);
jorick92 0:73dcc8fb9900 373 wait(0.2);
jorick92 0:73dcc8fb9900 374 keep_in_range(&pwm_to_begin_motorA, -1, 1 );
jorick92 0:73dcc8fb9900 375 motordirA.write(0);
jorick92 0:73dcc8fb9900 376 pwm_motorA.write(pwm_to_begin_motorA);
jorick92 0:73dcc8fb9900 377 } while(pwm_to_begin_motorA <= 0);
jorick92 0:73dcc8fb9900 378 pwm_motorA.write(0);
jorick92 0:73dcc8fb9900 379
jorick92 0:73dcc8fb9900 380 wait(1);
jorick92 0:73dcc8fb9900 381
jorick92 0:73dcc8fb9900 382 // Hierna moet motor B 21.6 (192) graden naar links om naar x-as te gaan.
jorick92 0:73dcc8fb9900 383
jorick92 0:73dcc8fb9900 384 motordirB.write(1);
jorick92 0:73dcc8fb9900 385 pwm_motorB.write(.08);
jorick92 0:73dcc8fb9900 386 do {
jorick92 0:73dcc8fb9900 387 setpoint_beginB = 192; // x-as
jorick92 0:73dcc8fb9900 388 pwm_to_begin_motorB = abs((setpoint_beginB - motorB.getPosition()) *.001);
jorick92 0:73dcc8fb9900 389 wait(0.2);
jorick92 0:73dcc8fb9900 390 keep_in_range(&pwm_to_begin_motorB, -1, 1 );
jorick92 0:73dcc8fb9900 391 motordirB.write(1);
jorick92 0:73dcc8fb9900 392 pwm_motorB.write(pwm_to_begin_motorB);
jorick92 0:73dcc8fb9900 393 } while(pwm_to_begin_motorB <= 0);
jorick92 0:73dcc8fb9900 394 motorB.setPosition(0);
jorick92 0:73dcc8fb9900 395 pwm_motorB.write(0);
jorick92 0:73dcc8fb9900 396
jorick92 0:73dcc8fb9900 397 wait(1); // willen nu even dat tussen ene actie en andere actie 1 seconde wacht.
jorick92 0:73dcc8fb9900 398
jorick92 0:73dcc8fb9900 399 // Hierna moet motor B van x-as naar de rechtsonder A4 positie. Motor B 460.
jorick92 0:73dcc8fb9900 400
jorick92 0:73dcc8fb9900 401 motordirB.write(1);
jorick92 0:73dcc8fb9900 402 pwm_motorB.write(0.08);
jorick92 0:73dcc8fb9900 403 do {
jorick92 0:73dcc8fb9900 404 setpoint_beginB = 460; // rechtsonder positie A4
jorick92 0:73dcc8fb9900 405 pwm_to_begin_motorB = abs((setpoint_beginB - motorB.getPosition()) *.001);
jorick92 0:73dcc8fb9900 406 wait(0.2);
jorick92 0:73dcc8fb9900 407 keep_in_range(&pwm_to_begin_motorB, -1, 1 );
jorick92 0:73dcc8fb9900 408 motordirB.write(1);
jorick92 0:73dcc8fb9900 409 pwm_motorB.write(pwm_to_begin_motorB);
jorick92 0:73dcc8fb9900 410 } while(pwm_to_begin_motorB <= 0);
jorick92 0:73dcc8fb9900 411 pwm_motorB.write(0);
jorick92 0:73dcc8fb9900 412
jorick92 0:73dcc8fb9900 413 wait(1);
jorick92 0:73dcc8fb9900 414
jorick92 0:73dcc8fb9900 415 // Nu zijn de motoren gekalibreed en staan ze op de startpositie.
jorick92 0:73dcc8fb9900 416 // Hierna het script dat EMG wordt omgezet in een positie verandering
jorick92 0:73dcc8fb9900 417
jorick92 0:73dcc8fb9900 418 /*Create a ticker, and let it call the */
jorick92 0:73dcc8fb9900 419 /*function 'setlooptimerflag' every 0.01s */
jorick92 0:73dcc8fb9900 420 Ticker looptimer;
jorick92 0:73dcc8fb9900 421 looptimer.attach(setlooptimerflag,0.01);
jorick92 0:73dcc8fb9900 422
jorick92 0:73dcc8fb9900 423 //INFINITE LOOP
jorick92 0:73dcc8fb9900 424 while(1) {
jorick92 0:73dcc8fb9900 425
jorick92 0:73dcc8fb9900 426 while(looptimerflag != true);
jorick92 0:73dcc8fb9900 427 looptimerflag = false;
jorick92 0:73dcc8fb9900 428
jorick92 0:73dcc8fb9900 429 // HIER EMG!!
jorick92 0:73dcc8fb9900 430 float emg_value_biceps;
jorick92 0:73dcc8fb9900 431 float emg_value_triceps;
jorick92 0:73dcc8fb9900 432 float emg_value_flexoren;
jorick92 0:73dcc8fb9900 433 float emg_value_extensoren;
jorick92 0:73dcc8fb9900 434 float dy;
jorick92 0:73dcc8fb9900 435 emg_value_biceps = ((100*(filter(1))-0.18)/0.49);
jorick92 0:73dcc8fb9900 436 emg_value_triceps = ((100*(filter(2))-0.18)/0.35);
jorick92 0:73dcc8fb9900 437 //emg_value_flexoren = 100*filter(3);
jorick92 0:73dcc8fb9900 438 //emg_value_extensoren = 100*filter(4);
jorick92 0:73dcc8fb9900 439
jorick92 0:73dcc8fb9900 440 if(emg_value_biceps < 0.10){
jorick92 0:73dcc8fb9900 441 emg_value_biceps=0;
jorick92 0:73dcc8fb9900 442 }
jorick92 0:73dcc8fb9900 443 else {
jorick92 0:73dcc8fb9900 444 emg_value_biceps = emg_value_biceps;
jorick92 0:73dcc8fb9900 445 }
jorick92 0:73dcc8fb9900 446 if(emg_value_triceps < 0.20){
jorick92 0:73dcc8fb9900 447 emg_value_triceps=0;
jorick92 0:73dcc8fb9900 448 }
jorick92 0:73dcc8fb9900 449 else {
jorick92 0:73dcc8fb9900 450 emg_value_triceps=emg_value_triceps;
jorick92 0:73dcc8fb9900 451 }
jorick92 0:73dcc8fb9900 452
jorick92 0:73dcc8fb9900 453
jorick92 0:73dcc8fb9900 454
jorick92 0:73dcc8fb9900 455 dy = emg_value_biceps-emg_value_triceps;
jorick92 0:73dcc8fb9900 456 dy=dy*10;
jorick92 0:73dcc8fb9900 457 if(pc.rxBufferGetSize(0)-pc.rxBufferGetCount() > 30)
jorick92 0:73dcc8fb9900 458 pc.printf("%.6f\n",dy);
jorick92 0:73dcc8fb9900 459
jorick92 0:73dcc8fb9900 460
jorick92 0:73dcc8fb9900 461
jorick92 0:73dcc8fb9900 462
jorick92 0:73dcc8fb9900 463 //setpointA = (potmeterA.read()-0.09027)*(631); // bereik van 71 graden dit afhankelijk van waar nul punt zit en waar heel wil. Dus afh. van EMG lezen bij EMG wordt 0.5 - 0.09027
jorick92 0:73dcc8fb9900 464 //setpointB = (potmeterB.read())*(415); // bereik van 46.7 graden
jorick92 0:73dcc8fb9900 465 //pc.printf("s: %f, %d ", setpointA, motorA.getPosition());
jorick92 0:73dcc8fb9900 466 //pc.printf("s: %f, %d ", setpointB, motorB.getPosition());
jorick92 0:73dcc8fb9900 467
jorick92 0:73dcc8fb9900 468 setpointB = (dy);
jorick92 0:73dcc8fb9900 469 //setpointB = (potmeterB.read() - 0.5) * (871/2);
jorick92 0:73dcc8fb9900 470 // motor A moet de hoek altijd binnen 53.4 tot en met 124.3 graden liggen
jorick92 0:73dcc8fb9900 471 // motor B moet de hoek altijd binnen 30.2 tot en met -16.5 graden liggen
jorick92 0:73dcc8fb9900 472 keep_in_range(&setpointA, -1105, -474); // voor motor moet bereik zijn -1105 tot -474
jorick92 0:73dcc8fb9900 473 keep_in_range(&setpointB, -147, 269); // voor motor moet bereik zijn -147 tot 269
jorick92 0:73dcc8fb9900 474
jorick92 0:73dcc8fb9900 475 // PID regelaar voor motor A
jorick92 0:73dcc8fb9900 476 //wait(dt);
jorick92 0:73dcc8fb9900 477 //error_ti_A = setpointA - motorA.getPosition();
jorick92 0:73dcc8fb9900 478 //P_regelaar_A = Kp * error_ti_A;
jorick92 0:73dcc8fb9900 479 //D_regelaar_A = Kd * ((error_ti_A - error_t0_A) / dt);
jorick92 0:73dcc8fb9900 480 //integral_i_A = integral_0_A + (error_ti_A * dt);
jorick92 0:73dcc8fb9900 481 //I_regelaar_A = Ki * integral_i_A;
jorick92 0:73dcc8fb9900 482 //integral_0_A = integral_i_A;
jorick92 0:73dcc8fb9900 483 //error_t0_A = error_ti_A;
jorick92 0:73dcc8fb9900 484 //output_regelaar_A = P_regelaar_A;
jorick92 0:73dcc8fb9900 485
jorick92 0:73dcc8fb9900 486 // PID regelaar voor motor B
jorick92 0:73dcc8fb9900 487 //wait(dt);
jorick92 0:73dcc8fb9900 488 //error_ti_B = setpointB - motorB.getPosition();
jorick92 0:73dcc8fb9900 489 //P_regelaar_B = Kp * error_ti_B;
jorick92 0:73dcc8fb9900 490 //D_regelaar_B = Kd * ((error_ti_B - error_t0_B) / dt);
jorick92 0:73dcc8fb9900 491 //integral_i_B = integral_0_B + (error_ti_B * dt);
jorick92 0:73dcc8fb9900 492 //I_regelaar_B = Ki * integral_i_B;
jorick92 0:73dcc8fb9900 493 //integral_0_B = integral_i_B;
jorick92 0:73dcc8fb9900 494 //error_t0_B = error_ti_B;
jorick92 0:73dcc8fb9900 495 //output_regelaar_B = P_regelaar_B;
jorick92 0:73dcc8fb9900 496
jorick92 0:73dcc8fb9900 497 /* This is a PID-action! store in pwm_to_motor */
jorick92 0:73dcc8fb9900 498 pwm_to_motorA = (setpointA - motorA.getPosition())*.001; //output_regelaar_A;
jorick92 0:73dcc8fb9900 499 pwm_to_motorB = (setpointB); //- motorB.getPosition())*.001; //output_regelaar_B;
jorick92 0:73dcc8fb9900 500
jorick92 0:73dcc8fb9900 501 keep_in_range(&pwm_to_motorA, -1,1);
jorick92 0:73dcc8fb9900 502 keep_in_range(&pwm_to_motorB, -1,1);
jorick92 0:73dcc8fb9900 503
jorick92 0:73dcc8fb9900 504 if(pwm_to_motorA > 0)
jorick92 0:73dcc8fb9900 505 motordirA.write(1);
jorick92 0:73dcc8fb9900 506 else
jorick92 0:73dcc8fb9900 507 motordirA.write(0);
jorick92 0:73dcc8fb9900 508 if(pwm_to_motorB > 0)
jorick92 0:73dcc8fb9900 509 motordirB.write(1);
jorick92 0:73dcc8fb9900 510 else
jorick92 0:73dcc8fb9900 511 motordirB.write(0);
jorick92 0:73dcc8fb9900 512
jorick92 0:73dcc8fb9900 513 pwm_motorA.write(abs(pwm_to_motorA));
jorick92 0:73dcc8fb9900 514 pwm_motorB.write(abs(pwm_to_motorB));
jorick92 0:73dcc8fb9900 515 }
jorick92 0:73dcc8fb9900 516 }
jorick92 0:73dcc8fb9900 517 while(toggle);
jorick92 0:73dcc8fb9900 518 { // wait while toggle == 1
jorick92 0:73dcc8fb9900 519 toggle_off();
jorick92 0:73dcc8fb9900 520 pwm_motorA.write(0);
jorick92 0:73dcc8fb9900 521 pwm_motorB.write(0);
jorick92 0:73dcc8fb9900 522 }
jorick92 0:73dcc8fb9900 523 }
jorick92 0:73dcc8fb9900 524 }
jorick92 0:73dcc8fb9900 525
jorick92 0:73dcc8fb9900 526
jorick92 0:73dcc8fb9900 527 void keep_in_range(float * in, float min, float max)
jorick92 0:73dcc8fb9900 528 {
jorick92 0:73dcc8fb9900 529 *in > min ? *in < max? : *in = max: *in = min;
jorick92 0:73dcc8fb9900 530 }
jorick92 0:73dcc8fb9900 531
jorick92 0:73dcc8fb9900 532
jorick92 0:73dcc8fb9900 533
jorick92 0:73dcc8fb9900 534
jorick92 0:73dcc8fb9900 535
jorick92 0:73dcc8fb9900 536