BMT M9 Groep01 / Mbed 2 deprecated Robot1_11

robot

Dependencies:   Encoder HIDScope MODSERIAL TouchButton mbed-dsp mbed

Fork of Robot2false by BMT M9 Groep01

main.cpp@57:4b682f92ab51, 2014-11-03 (annotated)

Committer:
Tanja2211
Date:
Mon Nov 03 18:06:35 2014 +0000
Revision:
57:4b682f92ab51
Parent:
56:29c31b83c4ce
Child:
58:9a56581d5298
pidpositie verbeterd

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Tanja2211 0:1594bb11fa13 1 #include "mbed.h"
Tanja2211 0:1594bb11fa13 2 #include "encoder.h"
Tanja2211 0:1594bb11fa13 3 #include "HIDScope.h"
Tanja2211 0:1594bb11fa13 4 #include "MODSERIAL.h"
Tanja2211 0:1594bb11fa13 5 #include "arm_math.h"
Tanja2211 0:1594bb11fa13 6 #include "TouchButton.h"
Tanja2211 0:1594bb11fa13 7
Tanja2211 0:1594bb11fa13 8 #define K_P (0.5)
Tanja2211 57:4b682f92ab51 9 #define K_I (0.0 *TSAMP1)
Tanja2211 0:1594bb11fa13 10 #define K_D (0 /TSAMP1)
Tanja2211 0:1594bb11fa13 11 #define I_LIMIT 1.
Tanja2211 55:4eb229a35859 12 #define K_Pp (0.003) //-------------------------------Deze drie waarden moeten anders!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
Tanja2211 54:2b54283b3b47 13 #define K_Ip (0.0 *TSAMP2)//-----------------------------TSAMP1 veranderd naar TSAMP2
Tanja2211 55:4eb229a35859 14 #define K_Dp (0.0000003 /TSAMP2)//--------------------------------TSAMP1 veranderd naar TSAMP2
Tanja2211 0:1594bb11fa13 15
Tanja2211 57:4b682f92ab51 16 #define TSAMP1 0.01
Tanja2211 0:1594bb11fa13 17 #define TSAMP2 0.01
Tanja2211 0:1594bb11fa13 18 #define WACHTEN 1
Tanja2211 0:1594bb11fa13 19 #define SLAAN 2
Tanja2211 0:1594bb11fa13 20 #define TERUGKEREN 3
Tanja2211 54:2b54283b3b47 21 #define ANGLEMAX -50
Tanja2211 35:39e6e9941ce4 22 #define ANGLEMIN 0
Tanja2211 0:1594bb11fa13 23
Tanja2211 0:1594bb11fa13 24 //initiating functions
Tanja2211 0:1594bb11fa13 25 void Triceps();
Tanja2211 0:1594bb11fa13 26 void Biceps();
Tanja2211 0:1594bb11fa13 27 void Calibratie_Triceps();
Tanja2211 0:1594bb11fa13 28 void Calibratie_Biceps();
Tanja2211 7:b40bbf5be443 29 float pid(float setspeed, float measurement, bool reset = false);
Tanja2211 54:2b54283b3b47 30 float pidpositie(float setposition, float measurement, bool reset =false);
Tanja2211 0:1594bb11fa13 31 void motor2aansturing();
Tanja2211 0:1594bb11fa13 32 void motor1aansturing();
Jolein 17:71c5c9bfb7ba 33 void motor1aansturingdeel2();
Tanja2211 0:1594bb11fa13 34
Tanja2211 0:1594bb11fa13 35 //alle initiaties voor EMG
Tanja2211 0:1594bb11fa13 36 MODSERIAL pc(USBTX,USBRX);
Tanja2211 0:1594bb11fa13 37
Tanja2211 55:4eb229a35859 38 HIDScope scope(6);
Tanja2211 0:1594bb11fa13 39
Tanja2211 0:1594bb11fa13 40 AnalogIn emgB(PTB1); //biceps
Tanja2211 0:1594bb11fa13 41 AnalogIn emgT(PTB2); //tricep
Tanja2211 0:1594bb11fa13 42
Tanja2211 0:1594bb11fa13 43 //*** OBJECTS ***
Tanja2211 0:1594bb11fa13 44 //bicep
Tanja2211 0:1594bb11fa13 45 uint16_t emg_valueB;
Tanja2211 0:1594bb11fa13 46 float emg_value_f32B;
Tanja2211 0:1594bb11fa13 47 float filtered_emgB;
Tanja2211 0:1594bb11fa13 48 float drempelwaardeB1, drempelwaardeB2, drempelwaardeB3;//B1=snelheidsstand 1, B2=snelheidsstand 2, B3=snelheidsstand 3
Tanja2211 0:1594bb11fa13 49 int yB1, yB2, yB3;
Tanja2211 0:1594bb11fa13 50 float B0, B1, B2, B3, B4, B5, B6, B7, B8, B9, B10, B11, B12, B13, B14, B15, B16, B17, B18, B19, B20, B21, B22, B23, B24, B25, B26, B27, B28, B29, MOVAVG_B; //moving average objects
Tanja2211 0:1594bb11fa13 51 float B30, B31, B32, B33, B34, B35, B36, B37, B38, B39, B40, B41, B42, B43, B44, B45, B46, B47, B48, B49, B50, B51, B52, B53, B54, B55, B56, B57, B58, B59;
Tanja2211 0:1594bb11fa13 52 int snelheidsstand;
Tanja2211 0:1594bb11fa13 53 //tricep
Tanja2211 0:1594bb11fa13 54 uint16_t emg_valueT;
Tanja2211 0:1594bb11fa13 55 float emg_value_f32T;
Tanja2211 0:1594bb11fa13 56 float filtered_emgT;
Tanja2211 0:1594bb11fa13 57 float drempelwaardeT;
Tanja2211 0:1594bb11fa13 58 int yT1, yT2;
Tanja2211 0:1594bb11fa13 59 float T0, T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28, T29,MOVAVG_T; //moving average objects
Tanja2211 0:1594bb11fa13 60 float MOVAVG_Positie1, MOVAVG_Positie2;
Tanja2211 0:1594bb11fa13 61 int positie;
Tanja2211 0:1594bb11fa13 62
Tanja2211 0:1594bb11fa13 63 //*** FILTERS ***
Tanja2211 0:1594bb11fa13 64 arm_biquad_casd_df1_inst_f32 notchT;
Tanja2211 0:1594bb11fa13 65 arm_biquad_casd_df1_inst_f32 notchB;
Tanja2211 0:1594bb11fa13 66 //constants for 50Hz
Tanja2211 0:1594bb11fa13 67 float notch_const[]= {0.5857841106784856, -1.3007020142696517e-16, 0.5857841106784856, 1.3007020142696517e-16, -0.17156822135697122}; //{a0 a1 a2 -b1 -b2}
Tanja2211 0:1594bb11fa13 68 float notch_states[4];
Tanja2211 0:1594bb11fa13 69
Tanja2211 0:1594bb11fa13 70 arm_biquad_casd_df1_inst_f32 lowpassT;
Tanja2211 0:1594bb11fa13 71 arm_biquad_casd_df1_inst_f32 lowpassB;
Tanja2211 0:1594bb11fa13 72 //constants for 60Hz lowpass
Tanja2211 0:1594bb11fa13 73 float lowpass_const[] = {0.39133426347022965, 0.7826685269404593, 0.39133426347022965, -0.3695259524151476, -0.19581110146577096};//{a0 a1 a2 -b1 -b2} van online calculator
Tanja2211 0:1594bb11fa13 74 float lowpass_states[4];
Tanja2211 0:1594bb11fa13 75
Tanja2211 0:1594bb11fa13 76 arm_biquad_casd_df1_inst_f32 highpassT;
Tanja2211 0:1594bb11fa13 77 arm_biquad_casd_df1_inst_f32 highpassB;
Tanja2211 0:1594bb11fa13 78 //constants for 20Hz highpass
Tanja2211 0:1594bb11fa13 79 float highpass_const[] = {0.6389437261127494, -1.2778874522254988, 0.6389437261127494, 1.1429772843080923, -0.41279762014290533};//{a0 a1 a2 -b1 -b2}
Tanja2211 0:1594bb11fa13 80 float highpass_states[4];
Tanja2211 0:1594bb11fa13 81
Jolein 46:8b30338f3f9b 82 /*bool stop;*/
Tanja2211 33:2f9c21ac9175 83 float pwm;
Jolein 48:5b1b90260239 84 float new_pwmpos;
Tanja2211 0:1594bb11fa13 85 float new_pwm;
Jolein 53:bdcbf365a9aa 86 float PWM2 = 0.3; //PWM voor instellen hoek batje, kan waarschijnlijk een stuk langzamer
Jolein 24:a165dcd86710 87 int toestand = WACHTEN; //terugkeren?
Tanja2211 55:4eb229a35859 88 float setspeed = 0, V3=80, V2=40, V1 =30;//V in counts/s
Tanja2211 0:1594bb11fa13 89
Tanja2211 0:1594bb11fa13 90
Tanja2211 55:4eb229a35859 91 Encoder motor1(PTD5,PTD3, true);
Jolein 24:a165dcd86710 92 Encoder motor2(PTD0,PTD2);
Jolein 24:a165dcd86710 93
Tanja2211 0:1594bb11fa13 94 DigitalOut motordir1(PTA4);
Tanja2211 0:1594bb11fa13 95 DigitalOut motordir2(PTC9);
Tanja2211 0:1594bb11fa13 96 PwmOut pwm_motor1(PTA5);
Tanja2211 0:1594bb11fa13 97 PwmOut pwm_motor2(PTC8);
Tanja2211 57:4b682f92ab51 98 float prevgetpos=0;
Tanja2211 57:4b682f92ab51 99 float omega;
Tanja2211 57:4b682f92ab51 100 float deltacounts;
Tanja2211 0:1594bb11fa13 101
Tanja2211 0:1594bb11fa13 102 DigitalOut myled1(LED1);//red
Tanja2211 0:1594bb11fa13 103 DigitalOut myled2(LED2);//green
Tanja2211 0:1594bb11fa13 104 DigitalOut myled3(LED3);//blue
Tanja2211 0:1594bb11fa13 105
s1340735 36:0c8d4397c02f 106 DigitalOut rood(PTD1);
s1340735 36:0c8d4397c02f 107 DigitalOut groen(PTA13);
Tanja2211 54:2b54283b3b47 108 DigitalOut blauw(PTD4);
Tanja2211 54:2b54283b3b47 109 DigitalOut wit(PTA12);
s1340735 36:0c8d4397c02f 110
Tanja2211 0:1594bb11fa13 111 /* FRDM-KL25Z built-in touch slider
Tanja2211 0:1594bb11fa13 112 *******************
Tanja2211 0:1594bb11fa13 113 * * * *
Tanja2211 0:1594bb11fa13 114 * 1 * 2 * 3 *
Tanja2211 0:1594bb11fa13 115 * * * *
Tanja2211 0:1594bb11fa13 116 *******************
Tanja2211 0:1594bb11fa13 117 * key 1 will light Red LED -> CALIBRATIE TRICEPS
Tanja2211 0:1594bb11fa13 118 * key 2 will light Green LED -> CALIBRATIE BICEPS
Tanja2211 0:1594bb11fa13 119 * key 3 will light Blue LED -> START*/
Tanja2211 0:1594bb11fa13 120
Tanja2211 0:1594bb11fa13 121
Tanja2211 0:1594bb11fa13 122 enum standen {STAND1=0, STAND2=1, STAND3=2};
Tanja2211 0:1594bb11fa13 123 standen hoek2 = STAND1;
Tanja2211 0:1594bb11fa13 124
Tanja2211 0:1594bb11fa13 125 int main ()
Tanja2211 22:f3a827faa135 126 {
Tanja2211 0:1594bb11fa13 127 pc.baud(115200);
Tanja2211 0:1594bb11fa13 128
Tanja2211 0:1594bb11fa13 129 drempelwaardeT=0;
Tanja2211 0:1594bb11fa13 130 drempelwaardeB1=0;
Tanja2211 0:1594bb11fa13 131 drempelwaardeB2=0;
Tanja2211 0:1594bb11fa13 132 drempelwaardeB3=0;
Tanja2211 0:1594bb11fa13 133
Tanja2211 0:1594bb11fa13 134 TouchButton TButton;
Tanja2211 0:1594bb11fa13 135
Tanja2211 0:1594bb11fa13 136 myled1=1;
Tanja2211 0:1594bb11fa13 137 myled2=1;
Tanja2211 0:1594bb11fa13 138 myled3=1;
Tanja2211 0:1594bb11fa13 139
Tanja2211 0:1594bb11fa13 140 int key=0;
Tanja2211 0:1594bb11fa13 141
Tanja2211 0:1594bb11fa13 142 pc.printf("key 1 calibratie triceps\n");
Tanja2211 0:1594bb11fa13 143 pc.printf("key 2 caliratie biceps\n");
Tanja2211 0:1594bb11fa13 144 pc.printf("key 3 START\n");
Tanja2211 0:1594bb11fa13 145
Tanja2211 0:1594bb11fa13 146 while(true) {
Tanja2211 0:1594bb11fa13 147
Tanja2211 0:1594bb11fa13 148 key = TButton.PressedButton();
Tanja2211 0:1594bb11fa13 149
Tanja2211 0:1594bb11fa13 150 if (key==1) {
Tanja2211 0:1594bb11fa13 151 //rood
Tanja2211 0:1594bb11fa13 152 myled1 = 0;
Tanja2211 0:1594bb11fa13 153 myled2 = 1;
Tanja2211 0:1594bb11fa13 154 myled3 = 1;
s1340735 36:0c8d4397c02f 155 rood=1;
Tanja2211 0:1594bb11fa13 156
s1340735 21:b7fb79882cb8 157 /*pc.printf("calibratie tricep aan\n");
Tanja2211 0:1594bb11fa13 158 wait(2);
Tanja2211 0:1594bb11fa13 159
Tanja2211 0:1594bb11fa13 160 Calibratie_Triceps();
Tanja2211 0:1594bb11fa13 161 drempelwaardeT=MOVAVG_T-1;
Tanja2211 0:1594bb11fa13 162 pc.printf("drempelwaarde triceps is %f\r\n", drempelwaardeT);
Tanja2211 0:1594bb11fa13 163
Tanja2211 0:1594bb11fa13 164 pc.printf("calibratie tricep klaar,\n");
Tanja2211 0:1594bb11fa13 165 myled1 = 0;
Tanja2211 0:1594bb11fa13 166 myled2 = 0;
Tanja2211 0:1594bb11fa13 167 myled3 = 0;
s1340735 36:0c8d4397c02f 168 wit=1;
s1340735 36:0c8d4397c02f 169
Tanja2211 0:1594bb11fa13 170 wait(2);
Tanja2211 0:1594bb11fa13 171
Tanja2211 0:1594bb11fa13 172 myled1=1;
Tanja2211 0:1594bb11fa13 173 myled2=1;
s1340735 36:0c8d4397c02f 174 myled3=1;
s1340735 36:0c8d4397c02f 175 rood=0;
s1340735 36:0c8d4397c02f 176 wit=0;*/
Tanja2211 0:1594bb11fa13 177 }
Tanja2211 0:1594bb11fa13 178 if (key==2) {
Tanja2211 0:1594bb11fa13 179 //green
Tanja2211 0:1594bb11fa13 180 myled1 = 1;
Tanja2211 0:1594bb11fa13 181 myled2 = 0;
Tanja2211 0:1594bb11fa13 182 myled3 = 1;
s1340735 36:0c8d4397c02f 183 groen=1;
Tanja2211 0:1594bb11fa13 184
s1340735 21:b7fb79882cb8 185 /*pc.printf("calibratie bicep snelheid 1 aan\n");
Tanja2211 0:1594bb11fa13 186 wait(2);
Tanja2211 0:1594bb11fa13 187
Tanja2211 0:1594bb11fa13 188 Calibratie_Biceps();
Tanja2211 0:1594bb11fa13 189 drempelwaardeB1=MOVAVG_B-1;
Tanja2211 0:1594bb11fa13 190 pc.printf("drempelwaarde snelheid 1 is %f\r\n", drempelwaardeB1);
Tanja2211 0:1594bb11fa13 191 myled1 = 0;
Tanja2211 0:1594bb11fa13 192 myled2 = 0;
Tanja2211 0:1594bb11fa13 193 myled3 = 0;
s1340735 36:0c8d4397c02f 194 wit=1;
Tanja2211 0:1594bb11fa13 195 wait(2);
Tanja2211 0:1594bb11fa13 196
s1340735 36:0c8d4397c02f 197 wit=0;
Tanja2211 0:1594bb11fa13 198 myled1 = 1;
Tanja2211 0:1594bb11fa13 199 myled2 = 0;
Tanja2211 0:1594bb11fa13 200 myled3 = 1;
Tanja2211 0:1594bb11fa13 201
Tanja2211 0:1594bb11fa13 202 pc.printf("calibratie biceps snelheid 2 aan\n");
Tanja2211 0:1594bb11fa13 203 wait(2);
Tanja2211 0:1594bb11fa13 204
Tanja2211 0:1594bb11fa13 205 Calibratie_Biceps();
Tanja2211 0:1594bb11fa13 206 drempelwaardeB2=MOVAVG_B-1;
Tanja2211 0:1594bb11fa13 207 pc.printf("drempelwaarde snelheid 2 is %f\r\n", drempelwaardeB2);
Tanja2211 0:1594bb11fa13 208 myled1 = 0;
Tanja2211 0:1594bb11fa13 209 myled2 = 0;
Tanja2211 0:1594bb11fa13 210 myled3 = 0;
s1340735 36:0c8d4397c02f 211 wit=1;
s1340735 36:0c8d4397c02f 212
Tanja2211 0:1594bb11fa13 213 wait(2);
Tanja2211 0:1594bb11fa13 214
s1340735 36:0c8d4397c02f 215 wit=0;
Tanja2211 0:1594bb11fa13 216 myled1 = 1;
Tanja2211 0:1594bb11fa13 217 myled2 = 0;
Tanja2211 0:1594bb11fa13 218 myled3 = 1;
Tanja2211 0:1594bb11fa13 219
Tanja2211 0:1594bb11fa13 220 pc.printf("calibratie biceps snelheid 3 aan\n");
Tanja2211 0:1594bb11fa13 221 wait(2);
Tanja2211 0:1594bb11fa13 222 Calibratie_Biceps();
Tanja2211 0:1594bb11fa13 223 drempelwaardeB3=MOVAVG_B-1;
Tanja2211 0:1594bb11fa13 224 pc.printf("drempelwaarde snelheid 3 is %f\r\n", drempelwaardeB3);
Tanja2211 0:1594bb11fa13 225 myled1 = 0;
Tanja2211 0:1594bb11fa13 226 myled2 = 0;
Tanja2211 0:1594bb11fa13 227 myled3 = 0;
s1340735 36:0c8d4397c02f 228 wit=1;
s1340735 36:0c8d4397c02f 229
Tanja2211 0:1594bb11fa13 230 wait(2);
Tanja2211 0:1594bb11fa13 231
Tanja2211 0:1594bb11fa13 232 pc.printf("caliratie biceps is klaar\n");
s1340735 36:0c8d4397c02f 233 wit=0;
s1340735 36:0c8d4397c02f 234 groen=0;
Tanja2211 0:1594bb11fa13 235 myled1=1;
Tanja2211 0:1594bb11fa13 236 myled2=1;
s1340735 21:b7fb79882cb8 237 myled3=1;*/
Tanja2211 0:1594bb11fa13 238 }
Tanja2211 0:1594bb11fa13 239
Tanja2211 0:1594bb11fa13 240 if (key==3) {
Tanja2211 0:1594bb11fa13 241 //blue
Tanja2211 0:1594bb11fa13 242 myled1 = 1;
Tanja2211 0:1594bb11fa13 243 myled2 = 1;
Tanja2211 0:1594bb11fa13 244 myled3 = 0;
s1340735 36:0c8d4397c02f 245 blauw=1;
s1340735 21:b7fb79882cb8 246 /*wait(3);
Tanja2211 0:1594bb11fa13 247
Tanja2211 0:1594bb11fa13 248 if(drempelwaardeT==0) {
Tanja2211 0:1594bb11fa13 249 pc.printf("geen waarde calibratie TRICEPS \n");
Tanja2211 0:1594bb11fa13 250 myled1 = 0;
Tanja2211 0:1594bb11fa13 251 myled2 = 0;
Tanja2211 0:1594bb11fa13 252 myled3 = 0;
s1340735 36:0c8d4397c02f 253 wit=1;
s1340735 36:0c8d4397c02f 254 rood=1;
Tanja2211 0:1594bb11fa13 255 }
Tanja2211 0:1594bb11fa13 256 if (drempelwaardeB1==0) {
Tanja2211 0:1594bb11fa13 257 pc.printf("geen waarde calibratie BICEPS 1 \n");
Tanja2211 0:1594bb11fa13 258 myled1 = 0;
Tanja2211 0:1594bb11fa13 259 myled2 = 0;
Tanja2211 0:1594bb11fa13 260 myled3 = 0;
s1340735 36:0c8d4397c02f 261 wit=1;
s1340735 36:0c8d4397c02f 262 groen=1;
Tanja2211 0:1594bb11fa13 263 }
Tanja2211 0:1594bb11fa13 264 if (drempelwaardeB2==0) {
Tanja2211 0:1594bb11fa13 265 pc.printf("geen waarde calibratie BICEPS 2 \n");
Tanja2211 0:1594bb11fa13 266 myled1 = 0;
Tanja2211 0:1594bb11fa13 267 myled2 = 0;
Tanja2211 0:1594bb11fa13 268 myled3 = 0;
s1340735 36:0c8d4397c02f 269 wit=1;
s1340735 36:0c8d4397c02f 270 groen=1;
Tanja2211 0:1594bb11fa13 271 }
Tanja2211 0:1594bb11fa13 272 if (drempelwaardeB3==0) {
Tanja2211 0:1594bb11fa13 273 pc.printf("geen waarde calibratie BICEPS 3 \n");
Tanja2211 0:1594bb11fa13 274 myled1 = 0;
Tanja2211 0:1594bb11fa13 275 myled2 = 0;
Tanja2211 0:1594bb11fa13 276 myled3 = 0;
s1340735 36:0c8d4397c02f 277 wit=1;
s1340735 36:0c8d4397c02f 278 groen=1;
s1340735 21:b7fb79882cb8 279 } else {*/
Tanja2211 0:1594bb11fa13 280
Tanja2211 22:f3a827faa135 281 /*pc.printf("eerst positie dan snelheid aangeven /n");
Tanja2211 0:1594bb11fa13 282
Tanja2211 22:f3a827faa135 283 //bepaling van positie met triceps 1
Tanja2211 22:f3a827faa135 284 Ticker log_timerT1;
Tanja2211 22:f3a827faa135 285
Tanja2211 22:f3a827faa135 286 arm_biquad_cascade_df1_init_f32(&notchT,1,notch_const,notch_states);
Tanja2211 22:f3a827faa135 287 arm_biquad_cascade_df1_init_f32(&lowpassT,1,lowpass_const,lowpass_states);
Tanja2211 22:f3a827faa135 288 arm_biquad_cascade_df1_init_f32(&highpassT,1,highpass_const,highpass_states);
Tanja2211 0:1594bb11fa13 289
Tanja2211 22:f3a827faa135 290 myled1 = 0;
Tanja2211 22:f3a827faa135 291 myled2 = 1;
Tanja2211 22:f3a827faa135 292 myled3 = 1;
s1340735 36:0c8d4397c02f 293 rood=1;
Tanja2211 22:f3a827faa135 294
Tanja2211 22:f3a827faa135 295 log_timerT1.attach(Triceps, 0.005);
Tanja2211 22:f3a827faa135 296 wait(2);
Tanja2211 22:f3a827faa135 297 log_timerT1.detach();
Tanja2211 0:1594bb11fa13 298
Tanja2211 22:f3a827faa135 299 // positie van batje met behulp van Triceps
Tanja2211 0:1594bb11fa13 300
Tanja2211 22:f3a827faa135 301 if (MOVAVG_T >= drempelwaardeT) {
Tanja2211 22:f3a827faa135 302 yT1=1;
Tanja2211 22:f3a827faa135 303 } else {
Tanja2211 22:f3a827faa135 304 yT1=0;
Tanja2211 22:f3a827faa135 305 }
Tanja2211 0:1594bb11fa13 306
Tanja2211 22:f3a827faa135 307 pc.printf("Triceps meting 1 is klaar.\n");
Tanja2211 22:f3a827faa135 308 myled1 = 1;
Tanja2211 22:f3a827faa135 309 myled2 = 1;
Tanja2211 22:f3a827faa135 310 myled3 = 0;
s1340735 36:0c8d4397c02f 311 rood=0;
s1340735 36:0c8d4397c02f 312
Tanja2211 22:f3a827faa135 313 wait(3);
Tanja2211 0:1594bb11fa13 314
Tanja2211 22:f3a827faa135 315 //bepaling van positie met tricep 2
Tanja2211 22:f3a827faa135 316 Ticker log_timerT2;
Tanja2211 22:f3a827faa135 317
Tanja2211 22:f3a827faa135 318 arm_biquad_cascade_df1_init_f32(&notchT,1,notch_const,notch_states);
Tanja2211 22:f3a827faa135 319 arm_biquad_cascade_df1_init_f32(&lowpassT,1,lowpass_const,lowpass_states);
Tanja2211 22:f3a827faa135 320 arm_biquad_cascade_df1_init_f32(&highpassT,1,highpass_const,highpass_states);
Tanja2211 22:f3a827faa135 321
Tanja2211 22:f3a827faa135 322 myled1 = 0;
Tanja2211 22:f3a827faa135 323 myled2 = 1;
Tanja2211 22:f3a827faa135 324 myled3 = 1;
s1340735 36:0c8d4397c02f 325 rood=1;
Tanja2211 0:1594bb11fa13 326
Tanja2211 22:f3a827faa135 327 log_timerT2.attach(Triceps, 0.005);
Tanja2211 22:f3a827faa135 328 wait(2);
Tanja2211 22:f3a827faa135 329 log_timerT2.detach();
Tanja2211 0:1594bb11fa13 330
Tanja2211 22:f3a827faa135 331 if (MOVAVG_T >= drempelwaardeT) {
Tanja2211 22:f3a827faa135 332 yT2=1;
Tanja2211 22:f3a827faa135 333 } else {
Tanja2211 22:f3a827faa135 334 yT2=0;
Tanja2211 22:f3a827faa135 335 }
Tanja2211 22:f3a827faa135 336
Tanja2211 22:f3a827faa135 337 pc.printf("Triceps meting 2 is klaar.\n");
Tanja2211 22:f3a827faa135 338 myled1 = 1;
Tanja2211 22:f3a827faa135 339 myled2 = 1;
Tanja2211 22:f3a827faa135 340 myled3 = 0;
s1340735 36:0c8d4397c02f 341 rood=0;
Tanja2211 22:f3a827faa135 342
Tanja2211 22:f3a827faa135 343 //*** INPUT MOTOR 2 ***
Tanja2211 56:29c31b83c4ce 344 positie=yT1+yT2; */
Tanja2211 56:29c31b83c4ce 345
Tanja2211 57:4b682f92ab51 346 /* //controle positie op scherm
Tanja2211 22:f3a827faa135 347 if (positie==0) {
Tanja2211 22:f3a827faa135 348 pc.printf("Motor 2 blijft op stand 1\n");
Tanja2211 22:f3a827faa135 349 } else {
Tanja2211 22:f3a827faa135 350 if (positie==1) {
Tanja2211 22:f3a827faa135 351 pc.printf("Motor 2 gaat naar stand 2\n");
Tanja2211 22:f3a827faa135 352 } else {
Tanja2211 22:f3a827faa135 353 if (positie==2) {
Tanja2211 22:f3a827faa135 354 pc.printf("Motor 2 gaat naar stand 3\n");
Tanja2211 22:f3a827faa135 355 }
Tanja2211 22:f3a827faa135 356 }
Tanja2211 22:f3a827faa135 357 }
Tanja2211 0:1594bb11fa13 358
Tanja2211 22:f3a827faa135 359 Ticker looptimer2;
Tanja2211 22:f3a827faa135 360 looptimer2.attach(motor2aansturing,TSAMP1);
Tanja2211 22:f3a827faa135 361 wait(8);
Tanja2211 22:f3a827faa135 362 looptimer2.detach();
Tanja2211 55:4eb229a35859 363 pc.printf("Detach Motor 1\n");
Tanja2211 57:4b682f92ab51 364 */
Tanja2211 22:f3a827faa135 365
Tanja2211 57:4b682f92ab51 366 // ------------------------------------------------------------------------------------------------------------------------------- eind aansturing motor 2
Tanja2211 22:f3a827faa135 367 wait(2);
Tanja2211 22:f3a827faa135 368 /* Ticker log_timerB;
Tanja2211 0:1594bb11fa13 369
Tanja2211 22:f3a827faa135 370 arm_biquad_cascade_df1_init_f32(&notchB,1,notch_const,notch_states);
Tanja2211 22:f3a827faa135 371 arm_biquad_cascade_df1_init_f32(&lowpassB,1,lowpass_const,lowpass_states);
Tanja2211 22:f3a827faa135 372 arm_biquad_cascade_df1_init_f32(&highpassB,1,highpass_const,highpass_states);
Tanja2211 22:f3a827faa135 373
Tanja2211 22:f3a827faa135 374 myled1 = 1;
Tanja2211 22:f3a827faa135 375 myled2 = 0;
Tanja2211 22:f3a827faa135 376 myled3 = 1;
s1340735 36:0c8d4397c02f 377 groen=1;
Tanja2211 22:f3a827faa135 378
Tanja2211 22:f3a827faa135 379 log_timerB.attach(Biceps,0.005);
Tanja2211 22:f3a827faa135 380 wait(2);
Tanja2211 22:f3a827faa135 381 log_timerB.detach();
Tanja2211 22:f3a827faa135 382
Tanja2211 22:f3a827faa135 383 //bepaling van snelheidsstand met biceps
Tanja2211 0:1594bb11fa13 384
Tanja2211 22:f3a827faa135 385 if (MOVAVG_B >= drempelwaardeB1) {
Tanja2211 22:f3a827faa135 386 yB1=1;
Tanja2211 22:f3a827faa135 387 if (MOVAVG_B >= drempelwaardeB2) {
Tanja2211 22:f3a827faa135 388 yB2=1;
Tanja2211 22:f3a827faa135 389 if (MOVAVG_B >= drempelwaardeB3) {
Tanja2211 22:f3a827faa135 390 yB3=1;
Tanja2211 22:f3a827faa135 391 } else {
Tanja2211 22:f3a827faa135 392 yB3=0;
Tanja2211 22:f3a827faa135 393 }
Tanja2211 22:f3a827faa135 394 } else {
Tanja2211 22:f3a827faa135 395 yB2=0;
Tanja2211 22:f3a827faa135 396 }
Tanja2211 22:f3a827faa135 397 } else {
Tanja2211 22:f3a827faa135 398 yB1=0;
Tanja2211 22:f3a827faa135 399 }
Tanja2211 0:1594bb11fa13 400
Tanja2211 22:f3a827faa135 401 pc.printf("Biceps meting is klaar.\n");
Tanja2211 22:f3a827faa135 402 myled1 = 1;
Tanja2211 22:f3a827faa135 403 myled2 = 1;
Tanja2211 22:f3a827faa135 404 myled3 = 0;
s1340735 36:0c8d4397c02f 405 groen=0;
Tanja2211 22:f3a827faa135 406
Tanja2211 22:f3a827faa135 407 //*** INPUT MOTOR 1 ***
Tanja2211 22:f3a827faa135 408 snelheidsstand=yB1+yB2+yB3; */
Tanja2211 22:f3a827faa135 409
Tanja2211 57:4b682f92ab51 410 snelheidsstand=3;
Tanja2211 22:f3a827faa135 411
Tanja2211 22:f3a827faa135 412 //controle snelheidsstand op scherm
Tanja2211 22:f3a827faa135 413 if (snelheidsstand==0) {
Tanja2211 22:f3a827faa135 414 pc.printf("Motor 1 beweegt niet \n");
Tanja2211 22:f3a827faa135 415 } else {
Tanja2211 22:f3a827faa135 416 if (snelheidsstand==1) {
Tanja2211 22:f3a827faa135 417 pc.printf("Motor 1 beweegt met snelheid 1 \n");
Tanja2211 0:1594bb11fa13 418 } else {
Tanja2211 22:f3a827faa135 419 if (snelheidsstand==2) {
Tanja2211 22:f3a827faa135 420 pc.printf("Motor 1 beweegt met snelheid 2 \n");
Tanja2211 0:1594bb11fa13 421 } else {
Tanja2211 22:f3a827faa135 422 if (snelheidsstand==3) {
Tanja2211 22:f3a827faa135 423 pc.printf("Motor 1 beweegt met snelheid 3 \n");
Tanja2211 0:1594bb11fa13 424 }
Tanja2211 0:1594bb11fa13 425 }
Tanja2211 0:1594bb11fa13 426 }
Tanja2211 22:f3a827faa135 427 }
Tanja2211 0:1594bb11fa13 428
Tanja2211 34:688120048afb 429 Ticker looptimer1;
Tanja2211 22:f3a827faa135 430 //pwm_motor1.write(0.3);
Tanja2211 22:f3a827faa135 431 motordir1 = 1;
Jolein 46:8b30338f3f9b 432 //stop = 0;
Tanja2211 22:f3a827faa135 433 looptimer1.attach(motor1aansturing,TSAMP1);
Tanja2211 29:f26796cca47f 434 wait(2); ////is aan te passen (tijd die nodig is om balletje te slaan
Tanja2211 22:f3a827faa135 435 looptimer1.detach();
Tanja2211 55:4eb229a35859 436 pc.printf("detachMotor1\n");
s1340735 36:0c8d4397c02f 437
s1340735 36:0c8d4397c02f 438 pid(0,0,true);
Tanja2211 55:4eb229a35859 439
Tanja2211 22:f3a827faa135 440 Ticker looptimer3;
Jolein 40:207eb8ab507a 441 looptimer3.attach(motor1aansturingdeel2,TSAMP1); //of TSAMP2?....
Tanja2211 55:4eb229a35859 442 wait(4); ////is aan te passen (tijd die nodig is om balletje te slaan
Tanja2211 22:f3a827faa135 443 looptimer3.detach();
Tanja2211 22:f3a827faa135 444 pc.printf("detachMotor1\n");
Tanja2211 55:4eb229a35859 445
Tanja2211 54:2b54283b3b47 446 pidpositie(0,0,true);
Tanja2211 22:f3a827faa135 447 pwm_motor1.write(0);
Jolein 48:5b1b90260239 448 toestand = WACHTEN; //hierheen verplaatst vanaf motor1aansturingdeel2. Belangrijk!! niet weghalen!!
Tanja2211 57:4b682f92ab51 449
Tanja2211 0:1594bb11fa13 450
Tanja2211 22:f3a827faa135 451 myled1=1;
Tanja2211 22:f3a827faa135 452 myled2=1;
Tanja2211 22:f3a827faa135 453 myled3=1;
s1340735 36:0c8d4397c02f 454 groen=0;
s1340735 36:0c8d4397c02f 455 blauw=0;
Tanja2211 55:4eb229a35859 456
s1340735 36:0c8d4397c02f 457 }
s1340735 36:0c8d4397c02f 458 }
Tanja2211 22:f3a827faa135 459 //}
Tanja2211 0:1594bb11fa13 460 }//end int main
Tanja2211 0:1594bb11fa13 461
Tanja2211 0:1594bb11fa13 462
Tanja2211 9:ba7f541cef3a 463 float pid(float setspeed, float measurement, bool reset )
Tanja2211 0:1594bb11fa13 464 {
Tanja2211 57:4b682f92ab51 465
Tanja2211 57:4b682f92ab51 466
Tanja2211 0:1594bb11fa13 467 float error;
Tanja2211 0:1594bb11fa13 468 static float prev_error = 0;
Tanja2211 0:1594bb11fa13 469 float out_p = 0;
Tanja2211 0:1594bb11fa13 470 static float out_i = 0;
Tanja2211 0:1594bb11fa13 471 float out_d = 0;
Tanja2211 22:f3a827faa135 472 if(reset==true) {
Jolein 13:722697791695 473 out_i = 0;
Jolein 13:722697791695 474 prev_error = 0;
Jolein 13:722697791695 475 }
Tanja2211 0:1594bb11fa13 476 error = setspeed-measurement;
Tanja2211 0:1594bb11fa13 477 out_p = error*K_P;
Tanja2211 0:1594bb11fa13 478 out_i += error*K_I;
Tanja2211 0:1594bb11fa13 479 out_d = (error-prev_error)*K_D;
Tanja2211 0:1594bb11fa13 480 prev_error = error;
Tanja2211 0:1594bb11fa13 481 return out_p + out_i + out_d;
Tanja2211 0:1594bb11fa13 482 }
Tanja2211 0:1594bb11fa13 483
Tanja2211 54:2b54283b3b47 484 float pidpositie(float setposition, float measurement, bool reset)
Jolein 24:a165dcd86710 485 {
Jolein 24:a165dcd86710 486 float error;
Jolein 24:a165dcd86710 487 static float prev_error = 0;
Jolein 24:a165dcd86710 488 float out_p = 0;
Jolein 24:a165dcd86710 489 static float out_i = 0;
Jolein 24:a165dcd86710 490 float out_d = 0;
Tanja2211 54:2b54283b3b47 491 if(reset==true) {
Tanja2211 54:2b54283b3b47 492 out_i = 0;
Tanja2211 54:2b54283b3b47 493 prev_error = 0;
Tanja2211 54:2b54283b3b47 494 }
Tanja2211 27:9dea548d6d6c 495 error = setposition-measurement;
Jolein 24:a165dcd86710 496 out_p = error*K_Pp;
Jolein 24:a165dcd86710 497 out_i += error*K_Ip;
Jolein 24:a165dcd86710 498 out_d = (error-prev_error)*K_Dp;
Jolein 24:a165dcd86710 499 prev_error = error;
Tanja2211 57:4b682f92ab51 500 return out_p + out_i + out_d;
Jolein 24:a165dcd86710 501 }
Jolein 24:a165dcd86710 502
Tanja2211 0:1594bb11fa13 503 void Triceps()
Tanja2211 0:1594bb11fa13 504 {
Tanja2211 0:1594bb11fa13 505 //Triceps lezen
Tanja2211 0:1594bb11fa13 506 emg_valueT = emgT.read_u16();
Tanja2211 0:1594bb11fa13 507 emg_value_f32T = emgT.read();
Tanja2211 0:1594bb11fa13 508
Tanja2211 0:1594bb11fa13 509 //Triceps filteren
Tanja2211 0:1594bb11fa13 510 arm_biquad_cascade_df1_f32(&notchT, &emg_value_f32T, &filtered_emgT, 1);
Tanja2211 0:1594bb11fa13 511 arm_biquad_cascade_df1_f32(&lowpassT, &filtered_emgT, &filtered_emgT, 1 );
Tanja2211 0:1594bb11fa13 512 filtered_emgT = fabs(filtered_emgT);
Tanja2211 0:1594bb11fa13 513 arm_biquad_cascade_df1_f32(&highpassT, &filtered_emgT, &filtered_emgT, 1 );
Tanja2211 0:1594bb11fa13 514 filtered_emgT = fabs(filtered_emgT);
Tanja2211 0:1594bb11fa13 515
Tanja2211 0:1594bb11fa13 516 //Triceps moving average
Tanja2211 0:1594bb11fa13 517 T0=filtered_emgT*1000;
Tanja2211 0:1594bb11fa13 518 MOVAVG_T=T0*0.03333+T1*0.03333+T2*0.03333+T3*0.03333+T4*0.03333+T5*0.03333+T6*0.03333+T7*0.03333+T8*0.03333+T9*0.03333+T10*0.03333+T11*0.03333+T12*0.03333+T13*0.03333+T14*0.03333+T15*0.03333+T16*0.03333+T17*0.03333+T18*0.03333+T19*0.03333+T20*0.03333+T21*0.03333+T22*0.03333+T23*0.03333+T24*0.03333+T25*0.03333+T26*0.03333+T27*0.03333+T28*0.03333+T29*0.03333;
Tanja2211 0:1594bb11fa13 519 T29=T28;
Tanja2211 0:1594bb11fa13 520 T28=T27;
Tanja2211 0:1594bb11fa13 521 T27=T26;
Tanja2211 0:1594bb11fa13 522 T26=T25;
Tanja2211 0:1594bb11fa13 523 T25=T24;
Tanja2211 0:1594bb11fa13 524 T24=T23;
Tanja2211 0:1594bb11fa13 525 T23=T22;
Tanja2211 0:1594bb11fa13 526 T22=T21;
Tanja2211 0:1594bb11fa13 527 T21=T20;
Tanja2211 0:1594bb11fa13 528 T20=T19;
Tanja2211 0:1594bb11fa13 529 T19=T18;
Tanja2211 0:1594bb11fa13 530 T18=T17;
Tanja2211 0:1594bb11fa13 531 T17=T16;
Tanja2211 0:1594bb11fa13 532 T16=T15;
Tanja2211 0:1594bb11fa13 533 T15=T14;
Tanja2211 0:1594bb11fa13 534 T14=T13;
Tanja2211 0:1594bb11fa13 535 T13=T12;
Tanja2211 0:1594bb11fa13 536 T12=T11;
Tanja2211 0:1594bb11fa13 537 T11=T10;
Tanja2211 0:1594bb11fa13 538 T10=T9;
Tanja2211 0:1594bb11fa13 539 T9=T8;
Tanja2211 0:1594bb11fa13 540 T8=T7;
Tanja2211 0:1594bb11fa13 541 T7=T6;
Tanja2211 0:1594bb11fa13 542 T6=T5;
Tanja2211 0:1594bb11fa13 543 T5=T4;
Tanja2211 0:1594bb11fa13 544 T4=T3;
Tanja2211 0:1594bb11fa13 545 T3=T2;
Tanja2211 0:1594bb11fa13 546 T2=T1;
Tanja2211 0:1594bb11fa13 547 T1=T0;
Tanja2211 0:1594bb11fa13 548
Tanja2211 0:1594bb11fa13 549 //sturen naar scherm (Realterm)
Tanja2211 0:1594bb11fa13 550 pc.printf("Moving average T %f\r\n",MOVAVG_T);
Tanja2211 0:1594bb11fa13 551
Tanja2211 0:1594bb11fa13 552 //sturen naar HID Scope
Tanja2211 20:99a8e9da2d6d 553 /*scope.set(0,emg_valueT); //ruwe data
Tanja2211 0:1594bb11fa13 554 scope.set(1,filtered_emgT); //filtered
Tanja2211 20:99a8e9da2d6d 555 scope.send();*/
Tanja2211 0:1594bb11fa13 556 }
Tanja2211 0:1594bb11fa13 557
Tanja2211 0:1594bb11fa13 558 void Biceps()
Tanja2211 0:1594bb11fa13 559 {
Tanja2211 0:1594bb11fa13 560 //Biceps lezen
Tanja2211 0:1594bb11fa13 561 emg_valueB = emgB.read_u16(); // read direct ADC result, converted to 16 bit integer (0..2^16 = 0..65536 = 0..3.3V)
Tanja2211 0:1594bb11fa13 562 emg_value_f32B = emgB.read();
Tanja2211 0:1594bb11fa13 563
Tanja2211 0:1594bb11fa13 564 //Biceps filteren
Tanja2211 0:1594bb11fa13 565 arm_biquad_cascade_df1_f32(&notchB, &emg_value_f32B, &filtered_emgB, 1 );
Tanja2211 0:1594bb11fa13 566 arm_biquad_cascade_df1_f32(&lowpassB, &filtered_emgB, &filtered_emgB, 1 );
Tanja2211 0:1594bb11fa13 567 filtered_emgB = fabs(filtered_emgB);
Tanja2211 0:1594bb11fa13 568 arm_biquad_cascade_df1_f32(&highpassB, &filtered_emgB, &filtered_emgB, 1 );
Tanja2211 0:1594bb11fa13 569 filtered_emgB = fabs(filtered_emgB);
Tanja2211 0:1594bb11fa13 570
Tanja2211 0:1594bb11fa13 571 //Biceps moving average
Tanja2211 0:1594bb11fa13 572 B0=filtered_emgB*1000;
Tanja2211 0:1594bb11fa13 573 MOVAVG_B=B0*0.01667+B1*0.01667+B2*0.01667+B3*0.01667+B4*0.01667+B5*0.01667+B6*0.01667+B7*0.01667+B8*0.01667+B9*0.01667+B10*0.01667+B11*0.01667+B12*0.01667+B13*0.01667+B14*0.01667+B15*0.01667+B16*0.01667+B17*0.01667+B18*0.01667+B19*0.01667+B20*0.01667+B21*0.01667+B22*0.01667+B23*0.01667+B24*0.01667+B25*0.01667+B26*0.01667+B27*0.01667+B28*0.01667+B29*0.01667+B30*0.01667+B31*0.01667+B32*0.01667+B33*0.01667+B34*0.01667+B35*0.01667+B36*0.01667+B37*0.01667+B38*0.01667+B39*0.01667+B40*0.01667+B41*0.01667+B42*0.01667+B43*0.01667+B44*0.01667+B45*0.01667+B46*0.01667+B47*0.01667+B48*0.01667+B49*0.01667+B50*0.01667+B51*0.01667+B52*0.01667+B53*0.01667+B54*0.01667+B55*0.01667+B56*0.01667+B57*0.01667+B58*0.01667+B59*0.01667;
Tanja2211 0:1594bb11fa13 574 B59=B58;
Tanja2211 0:1594bb11fa13 575 B58=B57;
Tanja2211 0:1594bb11fa13 576 B57=B56;
Tanja2211 0:1594bb11fa13 577 B56=B55;
Tanja2211 0:1594bb11fa13 578 B55=B54;
Tanja2211 0:1594bb11fa13 579 B54=B53;
Tanja2211 0:1594bb11fa13 580 B53=B52;
Tanja2211 0:1594bb11fa13 581 B52=B51;
Tanja2211 0:1594bb11fa13 582 B51=B50;
Tanja2211 0:1594bb11fa13 583 B50=B48;
Tanja2211 0:1594bb11fa13 584 B49=B49;
Tanja2211 0:1594bb11fa13 585 B48=B47;
Tanja2211 0:1594bb11fa13 586 B47=B46;
Tanja2211 0:1594bb11fa13 587 B46=B45;
Tanja2211 0:1594bb11fa13 588 B45=B44;
Tanja2211 0:1594bb11fa13 589 B44=B43;
Tanja2211 0:1594bb11fa13 590 B43=B42;
Tanja2211 0:1594bb11fa13 591 B42=B41;
Tanja2211 0:1594bb11fa13 592 B41=B40;
Tanja2211 0:1594bb11fa13 593 B40=B39;
Tanja2211 0:1594bb11fa13 594 B39=B38;
Tanja2211 0:1594bb11fa13 595 B38=B37;
Tanja2211 0:1594bb11fa13 596 B37=B36;
Tanja2211 0:1594bb11fa13 597 B36=B35;
Tanja2211 0:1594bb11fa13 598 B35=B34;
Tanja2211 0:1594bb11fa13 599 B34=B33;
Tanja2211 0:1594bb11fa13 600 B33=B32;
Tanja2211 0:1594bb11fa13 601 B32=B31;
Tanja2211 0:1594bb11fa13 602 B31=B30;
Tanja2211 0:1594bb11fa13 603 B30=B29;
Tanja2211 0:1594bb11fa13 604 B29=B28;
Tanja2211 0:1594bb11fa13 605 B28=B27;
Tanja2211 0:1594bb11fa13 606 B27=B26;
Tanja2211 0:1594bb11fa13 607 B26=B25;
Tanja2211 0:1594bb11fa13 608 B25=B24;
Tanja2211 0:1594bb11fa13 609 B24=B23;
Tanja2211 0:1594bb11fa13 610 B23=B22;
Tanja2211 0:1594bb11fa13 611 B22=B21;
Tanja2211 0:1594bb11fa13 612 B21=B20;
Tanja2211 0:1594bb11fa13 613 B20=B19;
Tanja2211 0:1594bb11fa13 614 B19=B18;
Tanja2211 0:1594bb11fa13 615 B18=B17;
Tanja2211 0:1594bb11fa13 616 B17=B16;
Tanja2211 0:1594bb11fa13 617 B16=B15;
Tanja2211 0:1594bb11fa13 618 B15=B14;
Tanja2211 0:1594bb11fa13 619 B14=B13;
Tanja2211 0:1594bb11fa13 620 B13=B12;
Tanja2211 0:1594bb11fa13 621 B12=B11;
Tanja2211 0:1594bb11fa13 622 B11=B10;
Tanja2211 0:1594bb11fa13 623 B10=B9;
Tanja2211 0:1594bb11fa13 624 B9=B8;
Tanja2211 0:1594bb11fa13 625 B8=B7;
Tanja2211 0:1594bb11fa13 626 B7=B6;
Tanja2211 0:1594bb11fa13 627 B6=B5;
Tanja2211 0:1594bb11fa13 628 B5=B4;
Tanja2211 0:1594bb11fa13 629 B4=B3;
Tanja2211 0:1594bb11fa13 630 B3=B2;
Tanja2211 0:1594bb11fa13 631 B2=B1;
Tanja2211 0:1594bb11fa13 632 B1=B0;
Tanja2211 0:1594bb11fa13 633
Tanja2211 0:1594bb11fa13 634 //sturen naar scherm
Tanja2211 0:1594bb11fa13 635 pc.printf("Moving average B %f\r\n",MOVAVG_B);
Tanja2211 0:1594bb11fa13 636
Tanja2211 0:1594bb11fa13 637 //naar HID Scope
Tanja2211 20:99a8e9da2d6d 638 /*scope.set(2,emg_valueB); //ruwe data
Tanja2211 0:1594bb11fa13 639 scope.set(3,filtered_emgB); //filtered
Tanja2211 20:99a8e9da2d6d 640 scope.send();*/
Tanja2211 0:1594bb11fa13 641 }
Tanja2211 0:1594bb11fa13 642
Tanja2211 0:1594bb11fa13 643 void Calibratie_Triceps()
Tanja2211 0:1594bb11fa13 644 {
Tanja2211 0:1594bb11fa13 645 Ticker log_timerT;
Tanja2211 0:1594bb11fa13 646
Tanja2211 0:1594bb11fa13 647 arm_biquad_cascade_df1_init_f32(&notchT,1,notch_const,notch_states);
Tanja2211 0:1594bb11fa13 648 arm_biquad_cascade_df1_init_f32(&lowpassT,1,lowpass_const,lowpass_states);
Tanja2211 0:1594bb11fa13 649 arm_biquad_cascade_df1_init_f32(&highpassT,1,highpass_const,highpass_states);
Tanja2211 0:1594bb11fa13 650
Tanja2211 0:1594bb11fa13 651 log_timerT.attach(Triceps, 0.005);
Tanja2211 0:1594bb11fa13 652 wait(2);
Tanja2211 0:1594bb11fa13 653 log_timerT.detach();
Tanja2211 0:1594bb11fa13 654 }
Tanja2211 0:1594bb11fa13 655
Tanja2211 0:1594bb11fa13 656 void Calibratie_Biceps()
Tanja2211 0:1594bb11fa13 657 {
Tanja2211 0:1594bb11fa13 658 Ticker log_timerB;
Tanja2211 0:1594bb11fa13 659
Tanja2211 0:1594bb11fa13 660 arm_biquad_cascade_df1_init_f32(&notchB,1,notch_const,notch_states);
Tanja2211 0:1594bb11fa13 661 arm_biquad_cascade_df1_init_f32(&lowpassB,1,lowpass_const,lowpass_states);
Tanja2211 0:1594bb11fa13 662 arm_biquad_cascade_df1_init_f32(&highpassB,1,highpass_const,highpass_states);
Tanja2211 0:1594bb11fa13 663
Tanja2211 0:1594bb11fa13 664 log_timerB.attach(Biceps, 0.005);
Tanja2211 0:1594bb11fa13 665 wait(2);
Tanja2211 0:1594bb11fa13 666 log_timerB.detach();
Tanja2211 0:1594bb11fa13 667 }
Tanja2211 0:1594bb11fa13 668
Tanja2211 0:1594bb11fa13 669 void motor2aansturing()
Tanja2211 0:1594bb11fa13 670 {
Tanja2211 54:2b54283b3b47 671 switch(positie) {
Jolein 52:b4af2a4ce38e 672 case 0:
Jolein 52:b4af2a4ce38e 673 if (motor2.getPosition()>= 0) {
Jolein 53:bdcbf365a9aa 674 motordir2 = 1;
Jolein 53:bdcbf365a9aa 675 pwm_motor2.write(PWM2);
Jolein 52:b4af2a4ce38e 676 } else {
Jolein 52:b4af2a4ce38e 677 pwm_motor2.write(0);
Jolein 52:b4af2a4ce38e 678 }
Jolein 52:b4af2a4ce38e 679 break;
Jolein 52:b4af2a4ce38e 680 case 1:
Jolein 52:b4af2a4ce38e 681 if (motor2.getPosition()>= 2) {
Jolein 53:bdcbf365a9aa 682 motordir2 = 1;
Jolein 53:bdcbf365a9aa 683 pwm_motor2.write(PWM2);
Jolein 52:b4af2a4ce38e 684 }
Jolein 52:b4af2a4ce38e 685 if (motor2.getPosition()<= 2) {
Jolein 52:b4af2a4ce38e 686 motordir2 = 0;
Jolein 52:b4af2a4ce38e 687 pwm_motor2.write(PWM2);
Jolein 52:b4af2a4ce38e 688 } else {
Jolein 52:b4af2a4ce38e 689 pwm_motor2.write(0);
Jolein 52:b4af2a4ce38e 690 }
Jolein 52:b4af2a4ce38e 691 break;
Jolein 52:b4af2a4ce38e 692 case 2:
Jolein 52:b4af2a4ce38e 693 if (motor2.getPosition()<= 4) {
Jolein 53:bdcbf365a9aa 694 motordir2 = 0;
Jolein 53:bdcbf365a9aa 695 pwm_motor2.write(PWM2);
Jolein 52:b4af2a4ce38e 696 } else {
Jolein 52:b4af2a4ce38e 697 pwm_motor2.write(0);
Jolein 52:b4af2a4ce38e 698 }
Jolein 52:b4af2a4ce38e 699 break;
Tanja2211 0:1594bb11fa13 700 }
Tanja2211 0:1594bb11fa13 701 }
Tanja2211 0:1594bb11fa13 702
Tanja2211 0:1594bb11fa13 703 void motor1aansturing()
Tanja2211 0:1594bb11fa13 704 {
Tanja2211 57:4b682f92ab51 705 deltacounts = motor1.getPosition()- prevgetpos;
Tanja2211 57:4b682f92ab51 706 prevgetpos=motor1.getPosition();
Tanja2211 57:4b682f92ab51 707 omega=(deltacounts/TSAMP1);
Tanja2211 57:4b682f92ab51 708
Jolein 40:207eb8ab507a 709 switch(toestand) {
Jolein 38:cc14faf38326 710 case SLAAN:
Jolein 44:130fa54388ef 711 pc.printf("SLAAN\n");
Tanja2211 57:4b682f92ab51 712 new_pwm = pid(setspeed, omega);
Jolein 38:cc14faf38326 713 pwm_motor1.write(new_pwm);
Jolein 38:cc14faf38326 714 motordir1 = 1;
Jolein 44:130fa54388ef 715 if (motor1.getPosition() <= ANGLEMAX) {
Jolein 44:130fa54388ef 716 toestand = TERUGKEREN;
Tanja2211 54:2b54283b3b47 717 pwm_motor1.write(0.0);//arvid had hier 0,0 gezet?!
Jolein 45:d359aad22a8a 718 pc.printf("toestand = terugkeren, wacht tot 2e ticker\n\r");
Jolein 46:8b30338f3f9b 719 //stop = 1;
Tanja2211 54:2b54283b3b47 720
Tanja2211 55:4eb229a35859 721 }
Jolein 40:207eb8ab507a 722 break;
Tanja2211 55:4eb229a35859 723
Jolein 38:cc14faf38326 724 case WACHTEN:
Jolein 38:cc14faf38326 725 pidpositie(ANGLEMIN,motor1.getPosition()); //dit is nu alleen een waarde, moet vervolgens in een functie terugkomen toch
Jolein 38:cc14faf38326 726 //?? motor1.getPosition(nieuwe positie);
Jolein 38:cc14faf38326 727 pwm_motor1.write(0);
Jolein 38:cc14faf38326 728 pc.printf("ifwachten\n");
Jolein 46:8b30338f3f9b 729 if (snelheidsstand != 0 /*&& stop == 0*/) {
Jolein 40:207eb8ab507a 730 toestand = SLAAN;
Jolein 40:207eb8ab507a 731 pc.printf("slaan \n");
Jolein 41:f2478822cee9 732 switch(snelheidsstand) {
Jolein 41:f2478822cee9 733 case 1:
Jolein 41:f2478822cee9 734 setspeed = V1;
Jolein 41:f2478822cee9 735 pc.printf("Snel 1 \n");
Jolein 41:f2478822cee9 736 break;
Jolein 41:f2478822cee9 737 case 2:
Jolein 41:f2478822cee9 738 setspeed = V2;
Jolein 41:f2478822cee9 739 pc.printf("Snel 2\n");
Jolein 41:f2478822cee9 740 break;
Jolein 41:f2478822cee9 741 case 3:
Jolein 41:f2478822cee9 742 setspeed = V3;
Jolein 41:f2478822cee9 743 pc.printf("Snel 3 \n");
Jolein 41:f2478822cee9 744 break;
Jolein 41:f2478822cee9 745 }//end switch
Jolein 40:207eb8ab507a 746 } //end if
Jolein 40:207eb8ab507a 747 break;
Tanja2211 55:4eb229a35859 748
Tanja2211 54:2b54283b3b47 749 /*case TERUGKEREN:
Tanja2211 54:2b54283b3b47 750 if (motor1.getPosition()<=ANGLEMAX) {
Tanja2211 54:2b54283b3b47 751 new_pwmpos = pidpositie(ANGLEMAX, motor1.getPosition());
Tanja2211 54:2b54283b3b47 752 pwm_motor1.write(new_pwmpos); //mogelijk moet dit -new_pwm zijn???
Tanja2211 54:2b54283b3b47 753 motordir1 = 0;
Tanja2211 54:2b54283b3b47 754 }
Tanja2211 54:2b54283b3b47 755 if (motor1.getPosition()>= ANGLEMAX) {
Tanja2211 54:2b54283b3b47 756 new_pwmpos = pidpositie(ANGLEMAX, motor1.getPosition());
Tanja2211 54:2b54283b3b47 757 pwm_motor1.write(new_pwmpos); //mogelijk moet dit -new_pwm zijn???
Tanja2211 54:2b54283b3b47 758 motordir1 = 1;
Tanja2211 54:2b54283b3b47 759 }
Tanja2211 54:2b54283b3b47 760 break;*///overbodig!!!
Jolein 41:f2478822cee9 761 } //end switch
Tanja2211 57:4b682f92ab51 762
Jolein 39:7d36e1219707 763 scope.set(0,motor1.getPosition());
Tanja2211 55:4eb229a35859 764 scope.set(1,ANGLEMAX);
Tanja2211 57:4b682f92ab51 765 scope.set(3,omega);
Tanja2211 57:4b682f92ab51 766 scope.set(4, setspeed);
Jolein 39:7d36e1219707 767 scope.send();
Jolein 17:71c5c9bfb7ba 768 }
Jolein 17:71c5c9bfb7ba 769
Jolein 17:71c5c9bfb7ba 770 void motor1aansturingdeel2()
Jolein 17:71c5c9bfb7ba 771 {
Tanja2211 57:4b682f92ab51 772 deltacounts = motor1.getPosition()- prevgetpos;
Tanja2211 57:4b682f92ab51 773 prevgetpos=motor1.getPosition();
Tanja2211 57:4b682f92ab51 774 omega=(deltacounts/TSAMP1);
Tanja2211 57:4b682f92ab51 775
Jolein 40:207eb8ab507a 776 switch(toestand) {
Jolein 48:5b1b90260239 777 case TERUGKEREN: //deze case moet blijven ookal is het de enige case
Tanja2211 57:4b682f92ab51 778 new_pwmpos = pidpositie(ANGLEMIN, motor1.getPosition());//new_PWM benaming zorgt mogelijk voor problemen
Tanja2211 57:4b682f92ab51 779 if (new_pwmpos > 0) {
Jolein 48:5b1b90260239 780 motordir1 = 0;
Jolein 48:5b1b90260239 781 //pc.printf("motor2.getPosition %d\r\n", motor2.getPosition());
Tanja2211 57:4b682f92ab51 782 } else {
Jolein 48:5b1b90260239 783 motordir1 = 1;
Jolein 43:af480e6823ab 784 //pc.printf("if2\n");
Jolein 43:af480e6823ab 785 }
Tanja2211 57:4b682f92ab51 786 pwm_motor1.write(fabs(new_pwmpos)); //mogelijk moet dit -new_pwm zijn???
Jolein 40:207eb8ab507a 787 break;
Jolein 44:130fa54388ef 788 } //end switch
Tanja2211 57:4b682f92ab51 789
Jolein 39:7d36e1219707 790 scope.set(0,motor1.getPosition()); //ruwe data
Tanja2211 55:4eb229a35859 791 scope.set(2,ANGLEMIN);
Tanja2211 57:4b682f92ab51 792 scope.set(3, omega);
Tanja2211 57:4b682f92ab51 793 scope.set(4, setspeed);
Jolein 39:7d36e1219707 794 scope.send();
Jolein 48:5b1b90260239 795 }//let op. Geen pidposition(0,0,true) deze moet zelf zorgen dat hij 0 wordt, en daar genoeg tijd voor hebben!!!