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@24:a165dcd86710, 2014-10-31 (annotated)

Committer:
Jolein
Date:
Fri Oct 31 14:54:49 2014 +0000
Revision:
24:a165dcd86710
Parent:
23:8f7ce4894c58
Child:
25:b58e10cbd35b
Child:
30:28be2bf11ad7
met pidposition!

Who changed what in which revision?

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