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@63:90cc947c3d77, 2014-11-04 (annotated)

Committer:
Tanja2211
Date:
Tue Nov 04 08:48:21 2014 +0000
Revision:
63:90cc947c3d77
Parent:
62:bdcc3328b13e
Child:
64:21e3e832f127
snelheden aanpassen

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