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@37:35fda673beb3, 2014-11-02 (annotated)

Committer:
Jolein
Date:
Sun Nov 02 12:17:33 2014 +0000
Revision:
37:35fda673beb3
Parent:
36:0c8d4397c02f
Child:
38:cc14faf38326
TSAMP2 en reset voor pidposition toegevoegd

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