BMT M9 Groep01 / Mbed 2 deprecated Robot2false

alles in elkaar met de mooie manier van de regelaar

Dependencies:   Encoder HIDScope MODSERIAL TouchButton mbed-dsp mbed

Fork of Robot2mooiemanier by BMT M9 Groep01

main.cpp@21:b7fb79882cb8, 2014-10-31 (annotated)

Committer:
s1340735
Date:
Fri Oct 31 13:30:21 2014 +0000
Revision:
21:b7fb79882cb8
Parent:
20:99a8e9da2d6d
Child:
22:f3a827faa135
motor 1 zonder shizzle

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