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@15:737f5f448eb7, 2014-10-31 (annotated)

Committer:
Jolein
Date:
Fri Oct 31 09:40:30 2014 +0000
Revision:
15:737f5f448eb7
Parent:
14:1ff917c6ac45
Child:
16:8e56aa6f4b7a
volgorde voor duidelijkheid veranderd

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