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@66:c922e18c9217, 2014-11-05 (annotated)

Committer:
Tanja2211
Date:
Wed Nov 05 20:19:47 2014 +0000
Revision:
66:c922e18c9217
Parent:
65:e1bc4d5cb5d2 
laatste commit 2

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