Muhabbat Komil / Mbed 2 deprecated Robot1_1

final

Dependencies:   Encoder HIDScope MODSERIAL TouchButton mbed-dsp mbed

Fork of Robot1_11 by BMT M9 Groep01

main.cpp@23:8f7ce4894c58, 2014-10-31 (annotated)

Committer:
Tanja2211
Date:
Fri Oct 31 14:33:13 2014 +0000
Revision:
23:8f7ce4894c58
Parent:
22:f3a827faa135
Child:
24:a165dcd86710
pid op wachten moet toegevoed worden

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