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@31:f26796cca47f, 2014-11-01 (annotated)

Committer:
Tanja2211
Date:
Sat Nov 01 14:11:01 2014 +0000
Revision:
31:f26796cca47f
Parent:
30:488e24ed1cb1 
pid positie in slaan en terugkeren

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