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@34:688120048afb, 2014-11-01 (annotated)

Committer:
Tanja2211
Date:
Sat Nov 01 15:28:52 2014 +0000
Revision:
34:688120048afb
Parent:
33:2f9c21ac9175
Child:
35:39e6e9941ce4
terugkeren alleen werkt

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