gokmen ascioglu / hakantez_V10

f

Dependencies:   hakan SDFileSystem mbed-src

main.cpp@1:e31dea65b070, 2016-09-19 (annotated)

Committer:
gokmenascioglu
Date:
Mon Sep 19 21:53:53 2016 +0000
Revision:
1:e31dea65b070
Parent:
0:f7e6527f4d8a 
bbbb

Who changed what in which revision?

UserRevisionLine numberNew contents of line
gokmenascioglu 0:f7e6527f4d8a 1 #include "mbed.h"
gokmenascioglu 0:f7e6527f4d8a 2 #include "MPU6050.h"
gokmenascioglu 0:f7e6527f4d8a 3
gokmenascioglu 0:f7e6527f4d8a 4 DigitalOut led(LED1);
gokmenascioglu 0:f7e6527f4d8a 5 DigitalOut led1(LED2);
gokmenascioglu 0:f7e6527f4d8a 6 Serial pc(USBTX, USBRX);
gokmenascioglu 0:f7e6527f4d8a 7 MPU6050 mpu;
gokmenascioglu 0:f7e6527f4d8a 8 Timer timer;
gokmenascioglu 0:f7e6527f4d8a 9 Timer timer1;
gokmenascioglu 0:f7e6527f4d8a 10 Timer timer2;
gokmenascioglu 0:f7e6527f4d8a 11 Timer timer3;
gokmenascioglu 0:f7e6527f4d8a 12
gokmenascioglu 0:f7e6527f4d8a 13 int16_t ax, ay, az;
gokmenascioglu 0:f7e6527f4d8a 14 int16_t gx, gy, gz, buff_x[1000],buff_y[1000],buff_z[1000];
gokmenascioglu 0:f7e6527f4d8a 15 int16_t i=1,ym=5;
gokmenascioglu 0:f7e6527f4d8a 16 int32_t toplam_x=0,toplam_y=0,toplam_z=0;
gokmenascioglu 0:f7e6527f4d8a 17 float x,y,z,k,l,m,k1,l1,x1,y1;
gokmenascioglu 0:f7e6527f4d8a 18 float begin, end,end1,end2, ve, s,t,u,s1,t1,u1;
gokmenascioglu 0:f7e6527f4d8a 19
gokmenascioglu 0:f7e6527f4d8a 20 void accelerometer(void)
gokmenascioglu 0:f7e6527f4d8a 21 {
gokmenascioglu 0:f7e6527f4d8a 22 mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
gokmenascioglu 0:f7e6527f4d8a 23
gokmenascioglu 0:f7e6527f4d8a 24 ax=(ax-toplam_x);
gokmenascioglu 0:f7e6527f4d8a 25 ay=ay-toplam_y;
gokmenascioglu 0:f7e6527f4d8a 26 az=az+toplam_z;
gokmenascioglu 0:f7e6527f4d8a 27
gokmenascioglu 0:f7e6527f4d8a 28 ax=ax-toplam_x;
gokmenascioglu 0:f7e6527f4d8a 29 ay=ay-toplam_y;
gokmenascioglu 0:f7e6527f4d8a 30 az=az-toplam_z;
gokmenascioglu 0:f7e6527f4d8a 31
gokmenascioglu 0:f7e6527f4d8a 32 x=ax/16383.00;
gokmenascioglu 0:f7e6527f4d8a 33 y=ay/16383.00;
gokmenascioglu 0:f7e6527f4d8a 34 z=az/16383.00;
gokmenascioglu 0:f7e6527f4d8a 35 x1=x;
gokmenascioglu 0:f7e6527f4d8a 36 y1=y;
gokmenascioglu 0:f7e6527f4d8a 37
gokmenascioglu 0:f7e6527f4d8a 38 }
gokmenascioglu 0:f7e6527f4d8a 39
gokmenascioglu 0:f7e6527f4d8a 40 void accelerometer1(void)
gokmenascioglu 0:f7e6527f4d8a 41 {
gokmenascioglu 0:f7e6527f4d8a 42 mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
gokmenascioglu 0:f7e6527f4d8a 43
gokmenascioglu 0:f7e6527f4d8a 44 ax=(ax-toplam_x);
gokmenascioglu 0:f7e6527f4d8a 45 ay=ay-toplam_y;
gokmenascioglu 0:f7e6527f4d8a 46 az=az+toplam_z;
gokmenascioglu 0:f7e6527f4d8a 47
gokmenascioglu 0:f7e6527f4d8a 48 ax=ax-toplam_x;
gokmenascioglu 0:f7e6527f4d8a 49 ay=ay-toplam_y;
gokmenascioglu 0:f7e6527f4d8a 50 az=az-toplam_z;
gokmenascioglu 0:f7e6527f4d8a 51
gokmenascioglu 0:f7e6527f4d8a 52 s=ax/16383.00;
gokmenascioglu 0:f7e6527f4d8a 53 t=ay/16383.00;
gokmenascioglu 0:f7e6527f4d8a 54 u=az/16383.00;
gokmenascioglu 0:f7e6527f4d8a 55
gokmenascioglu 0:f7e6527f4d8a 56 }
gokmenascioglu 0:f7e6527f4d8a 57
gokmenascioglu 0:f7e6527f4d8a 58 void accelerometer2(void)
gokmenascioglu 0:f7e6527f4d8a 59 {
gokmenascioglu 0:f7e6527f4d8a 60 mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
gokmenascioglu 0:f7e6527f4d8a 61
gokmenascioglu 0:f7e6527f4d8a 62 ax=(ax-toplam_x);
gokmenascioglu 0:f7e6527f4d8a 63 ay=ay-toplam_y;
gokmenascioglu 0:f7e6527f4d8a 64 az=az+toplam_z;
gokmenascioglu 0:f7e6527f4d8a 65
gokmenascioglu 0:f7e6527f4d8a 66 ax=ax-toplam_x;
gokmenascioglu 0:f7e6527f4d8a 67 ay=ay-toplam_y;
gokmenascioglu 0:f7e6527f4d8a 68 az=az-toplam_z;
gokmenascioglu 0:f7e6527f4d8a 69
gokmenascioglu 0:f7e6527f4d8a 70 s1=ax/16383.00;
gokmenascioglu 0:f7e6527f4d8a 71 t1=ay/16383.00;
gokmenascioglu 0:f7e6527f4d8a 72 u1=az/16383.00;
gokmenascioglu 0:f7e6527f4d8a 73
gokmenascioglu 0:f7e6527f4d8a 74 }
gokmenascioglu 0:f7e6527f4d8a 75
gokmenascioglu 0:f7e6527f4d8a 76 int main()
gokmenascioglu 0:f7e6527f4d8a 77 {
gokmenascioglu 0:f7e6527f4d8a 78 pc.printf("MPU6050 testConnection\n");
gokmenascioglu 0:f7e6527f4d8a 79
gokmenascioglu 0:f7e6527f4d8a 80 bool mpu6050TestResult = mpu.testConnection();
gokmenascioglu 0:f7e6527f4d8a 81
gokmenascioglu 0:f7e6527f4d8a 82 if(mpu6050TestResult) {
gokmenascioglu 0:f7e6527f4d8a 83 pc.printf("MPU6050 test passed \n");
gokmenascioglu 0:f7e6527f4d8a 84 } else {
gokmenascioglu 0:f7e6527f4d8a 85 pc.printf("MPU6050 test failed \n");
gokmenascioglu 0:f7e6527f4d8a 86 }
gokmenascioglu 0:f7e6527f4d8a 87
gokmenascioglu 0:f7e6527f4d8a 88 mpu.initialize();
gokmenascioglu 0:f7e6527f4d8a 89 mpu.setDLPFMode(1);
gokmenascioglu 0:f7e6527f4d8a 90 pc.printf("DLPF=%d\n\r",mpu.getDLPFMode());
gokmenascioglu 0:f7e6527f4d8a 91
gokmenascioglu 0:f7e6527f4d8a 92
gokmenascioglu 0:f7e6527f4d8a 93 for(i=0; i<500; i++) {
gokmenascioglu 0:f7e6527f4d8a 94 mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
gokmenascioglu 0:f7e6527f4d8a 95 buff_x[i]=ax;
gokmenascioglu 0:f7e6527f4d8a 96 toplam_x=toplam_x+buff_x[i];
gokmenascioglu 0:f7e6527f4d8a 97 buff_y[i]=ay;
gokmenascioglu 0:f7e6527f4d8a 98 toplam_y=toplam_y+buff_y[i];
gokmenascioglu 0:f7e6527f4d8a 99 buff_z[i]=az;
gokmenascioglu 0:f7e6527f4d8a 100 toplam_z=toplam_z+buff_z[i];
gokmenascioglu 0:f7e6527f4d8a 101 }
gokmenascioglu 0:f7e6527f4d8a 102
gokmenascioglu 0:f7e6527f4d8a 103 toplam_x=toplam_x/500;
gokmenascioglu 0:f7e6527f4d8a 104 toplam_y=toplam_y/500;
gokmenascioglu 0:f7e6527f4d8a 105 toplam_z=toplam_z/500;
gokmenascioglu 0:f7e6527f4d8a 106 toplam_z=16383-toplam_z;
gokmenascioglu 0:f7e6527f4d8a 107
gokmenascioglu 0:f7e6527f4d8a 108 ax=(ax-toplam_x);
gokmenascioglu 0:f7e6527f4d8a 109 ay=ay-toplam_y;
gokmenascioglu 0:f7e6527f4d8a 110 az=az+toplam_z;
gokmenascioglu 0:f7e6527f4d8a 111
gokmenascioglu 0:f7e6527f4d8a 112 ax=ax-toplam_x;
gokmenascioglu 0:f7e6527f4d8a 113 ay=ay-toplam_y;
gokmenascioglu 0:f7e6527f4d8a 114 az=az-toplam_z;
gokmenascioglu 0:f7e6527f4d8a 115
gokmenascioglu 0:f7e6527f4d8a 116 k=ax/16383.00;
gokmenascioglu 0:f7e6527f4d8a 117 l=ay/16383.00;
gokmenascioglu 0:f7e6527f4d8a 118 m=az/16383.00;
gokmenascioglu 0:f7e6527f4d8a 119
gokmenascioglu 0:f7e6527f4d8a 120 i=0;
gokmenascioglu 0:f7e6527f4d8a 121 led=1;
gokmenascioglu 0:f7e6527f4d8a 122
gokmenascioglu 0:f7e6527f4d8a 123 k1=k;
gokmenascioglu 0:f7e6527f4d8a 124 l1=l;
gokmenascioglu 0:f7e6527f4d8a 125
gokmenascioglu 0:f7e6527f4d8a 126 while(1) {
gokmenascioglu 0:f7e6527f4d8a 127 k=k1;
gokmenascioglu 0:f7e6527f4d8a 128 l=l1;
gokmenascioglu 0:f7e6527f4d8a 129 timer.start();
gokmenascioglu 0:f7e6527f4d8a 130 begin = timer.read();
gokmenascioglu 0:f7e6527f4d8a 131
gokmenascioglu 0:f7e6527f4d8a 132 accelerometer();
gokmenascioglu 0:f7e6527f4d8a 133 wait_ms(10);
gokmenascioglu 0:f7e6527f4d8a 134 i++;
gokmenascioglu 0:f7e6527f4d8a 135 pc.printf("Beklemede\n\r");
gokmenascioglu 0:f7e6527f4d8a 136
gokmenascioglu 0:f7e6527f4d8a 137 /* HEAD FRONT MOVEMENT */
gokmenascioglu 0:f7e6527f4d8a 138 timer1.reset();
gokmenascioglu 0:f7e6527f4d8a 139 if ((y+2) < (l+1.7)) {
gokmenascioglu 0:f7e6527f4d8a 140
gokmenascioglu 0:f7e6527f4d8a 141 timer1.start();
gokmenascioglu 0:f7e6527f4d8a 142 accelerometer1();
gokmenascioglu 0:f7e6527f4d8a 143 wait_ms(10);
gokmenascioglu 0:f7e6527f4d8a 144
gokmenascioglu 0:f7e6527f4d8a 145 while((t+2)<(l+1.85)) {
gokmenascioglu 0:f7e6527f4d8a 146
gokmenascioglu 0:f7e6527f4d8a 147 end=timer1.read();
gokmenascioglu 0:f7e6527f4d8a 148 accelerometer1();
gokmenascioglu 0:f7e6527f4d8a 149 wait_ms(10);
gokmenascioglu 0:f7e6527f4d8a 150
gokmenascioglu 0:f7e6527f4d8a 151 if (end>1.5) {
gokmenascioglu 0:f7e6527f4d8a 152 while (t<-0.15) { accelerometer1(); }
gokmenascioglu 0:f7e6527f4d8a 153 y=5.0;
gokmenascioglu 0:f7e6527f4d8a 154 t=7.0;
gokmenascioglu 0:f7e6527f4d8a 155 end=0;
gokmenascioglu 0:f7e6527f4d8a 156 timer1.reset();
gokmenascioglu 0:f7e6527f4d8a 157 break;
gokmenascioglu 0:f7e6527f4d8a 158 }
gokmenascioglu 0:f7e6527f4d8a 159
gokmenascioglu 0:f7e6527f4d8a 160 }
gokmenascioglu 0:f7e6527f4d8a 161
gokmenascioglu 0:f7e6527f4d8a 162 if ( (y+2)< (l+1.7) ) {
gokmenascioglu 0:f7e6527f4d8a 163
gokmenascioglu 0:f7e6527f4d8a 164 accelerometer2();
gokmenascioglu 0:f7e6527f4d8a 165 timer2.reset();
gokmenascioglu 0:f7e6527f4d8a 166 timer2.start();
gokmenascioglu 0:f7e6527f4d8a 167 while((t1+2)>(l+1.85)) {
gokmenascioglu 0:f7e6527f4d8a 168
gokmenascioglu 0:f7e6527f4d8a 169 accelerometer2();
gokmenascioglu 0:f7e6527f4d8a 170 wait_ms(10);
gokmenascioglu 0:f7e6527f4d8a 171 end1=timer2.read();
gokmenascioglu 0:f7e6527f4d8a 172 ym=1;
gokmenascioglu 0:f7e6527f4d8a 173 if (end1>1.5) {
gokmenascioglu 0:f7e6527f4d8a 174 y=5;
gokmenascioglu 0:f7e6527f4d8a 175 ym=5;
gokmenascioglu 0:f7e6527f4d8a 176 l=5;
gokmenascioglu 0:f7e6527f4d8a 177 break;
gokmenascioglu 0:f7e6527f4d8a 178 }
gokmenascioglu 0:f7e6527f4d8a 179 }
gokmenascioglu 0:f7e6527f4d8a 180
gokmenascioglu 0:f7e6527f4d8a 181 }
gokmenascioglu 0:f7e6527f4d8a 182 }
gokmenascioglu 0:f7e6527f4d8a 183
gokmenascioglu 0:f7e6527f4d8a 184
gokmenascioglu 0:f7e6527f4d8a 185 /* HEAD back MOVEMENT */
gokmenascioglu 0:f7e6527f4d8a 186 if ((y1+1.7) > (l+2)) {
gokmenascioglu 0:f7e6527f4d8a 187 timer1.reset();
gokmenascioglu 0:f7e6527f4d8a 188 timer1.start();
gokmenascioglu 0:f7e6527f4d8a 189 accelerometer1();
gokmenascioglu 0:f7e6527f4d8a 190 wait_ms(10);
gokmenascioglu 0:f7e6527f4d8a 191
gokmenascioglu 0:f7e6527f4d8a 192 while((t+1.85)>(l+2)) {
gokmenascioglu 0:f7e6527f4d8a 193 end=timer1.read();
gokmenascioglu 0:f7e6527f4d8a 194 accelerometer1();
gokmenascioglu 0:f7e6527f4d8a 195 wait_ms(10);
gokmenascioglu 0:f7e6527f4d8a 196
gokmenascioglu 0:f7e6527f4d8a 197 if (end>1.5) {
gokmenascioglu 0:f7e6527f4d8a 198 while (t>0.3) { accelerometer1(); }
gokmenascioglu 0:f7e6527f4d8a 199 y1=-5.0;
gokmenascioglu 0:f7e6527f4d8a 200 t=-7.0;
gokmenascioglu 0:f7e6527f4d8a 201 end=0;
gokmenascioglu 0:f7e6527f4d8a 202 timer1.reset();
gokmenascioglu 0:f7e6527f4d8a 203 break;
gokmenascioglu 0:f7e6527f4d8a 204 }
gokmenascioglu 0:f7e6527f4d8a 205
gokmenascioglu 0:f7e6527f4d8a 206 }
gokmenascioglu 0:f7e6527f4d8a 207
gokmenascioglu 0:f7e6527f4d8a 208
gokmenascioglu 0:f7e6527f4d8a 209 if ( (y1+1.7)>(l+2.0) ) {
gokmenascioglu 0:f7e6527f4d8a 210
gokmenascioglu 0:f7e6527f4d8a 211 accelerometer2();
gokmenascioglu 0:f7e6527f4d8a 212 timer2.reset();
gokmenascioglu 0:f7e6527f4d8a 213 timer2.start();
gokmenascioglu 0:f7e6527f4d8a 214
gokmenascioglu 0:f7e6527f4d8a 215 while((t1+1.85)<(l+2)) {
gokmenascioglu 0:f7e6527f4d8a 216 accelerometer2();
gokmenascioglu 0:f7e6527f4d8a 217 wait_ms(10);
gokmenascioglu 0:f7e6527f4d8a 218 end1=timer2.read();
gokmenascioglu 0:f7e6527f4d8a 219 ym=2;
gokmenascioglu 0:f7e6527f4d8a 220 if (end1>1.5) {
gokmenascioglu 0:f7e6527f4d8a 221 y1=-5;
gokmenascioglu 0:f7e6527f4d8a 222 ym=5;
gokmenascioglu 0:f7e6527f4d8a 223 t1=5;
gokmenascioglu 0:f7e6527f4d8a 224 break;
gokmenascioglu 0:f7e6527f4d8a 225 }
gokmenascioglu 0:f7e6527f4d8a 226 }
gokmenascioglu 0:f7e6527f4d8a 227
gokmenascioglu 0:f7e6527f4d8a 228 }
gokmenascioglu 0:f7e6527f4d8a 229 }
gokmenascioglu 0:f7e6527f4d8a 230
gokmenascioglu 0:f7e6527f4d8a 231 /* HEAD left MOVEMENT */
gokmenascioglu 0:f7e6527f4d8a 232 if ((x1+1.7) > (k+2)) {
gokmenascioglu 0:f7e6527f4d8a 233 timer1.reset();
gokmenascioglu 0:f7e6527f4d8a 234 timer1.start();
gokmenascioglu 0:f7e6527f4d8a 235 accelerometer1();
gokmenascioglu 0:f7e6527f4d8a 236 wait_ms(10);
gokmenascioglu 0:f7e6527f4d8a 237
gokmenascioglu 0:f7e6527f4d8a 238 while((s+1.85)>(k+2)) {
gokmenascioglu 0:f7e6527f4d8a 239 end=timer1.read();
gokmenascioglu 0:f7e6527f4d8a 240 accelerometer1();
gokmenascioglu 0:f7e6527f4d8a 241 wait_ms(10);
gokmenascioglu 0:f7e6527f4d8a 242
gokmenascioglu 0:f7e6527f4d8a 243 if (end>1.5) {
gokmenascioglu 0:f7e6527f4d8a 244 while (s>0.3) { accelerometer1(); }
gokmenascioglu 0:f7e6527f4d8a 245 x1=-5.0;
gokmenascioglu 0:f7e6527f4d8a 246 s=-7.0;
gokmenascioglu 0:f7e6527f4d8a 247 end=0;
gokmenascioglu 0:f7e6527f4d8a 248 timer1.reset();
gokmenascioglu 0:f7e6527f4d8a 249 break;
gokmenascioglu 0:f7e6527f4d8a 250 }
gokmenascioglu 0:f7e6527f4d8a 251
gokmenascioglu 0:f7e6527f4d8a 252 }
gokmenascioglu 0:f7e6527f4d8a 253
gokmenascioglu 0:f7e6527f4d8a 254
gokmenascioglu 0:f7e6527f4d8a 255 if ( (x1+1.7)>(k+2.0) ) {
gokmenascioglu 0:f7e6527f4d8a 256
gokmenascioglu 0:f7e6527f4d8a 257 accelerometer2();
gokmenascioglu 0:f7e6527f4d8a 258 timer2.reset();
gokmenascioglu 0:f7e6527f4d8a 259 timer2.start();
gokmenascioglu 0:f7e6527f4d8a 260 while((s1+1.85)<(k+2)) {
gokmenascioglu 0:f7e6527f4d8a 261 accelerometer2();
gokmenascioglu 0:f7e6527f4d8a 262 wait_ms(10);
gokmenascioglu 0:f7e6527f4d8a 263 end1=timer2.read();
gokmenascioglu 0:f7e6527f4d8a 264 ym=4;
gokmenascioglu 0:f7e6527f4d8a 265 if (end1>1.5) {
gokmenascioglu 0:f7e6527f4d8a 266 x1=-5;
gokmenascioglu 0:f7e6527f4d8a 267 ym=5;
gokmenascioglu 0:f7e6527f4d8a 268 s1=5;
gokmenascioglu 0:f7e6527f4d8a 269 break;
gokmenascioglu 0:f7e6527f4d8a 270 }
gokmenascioglu 0:f7e6527f4d8a 271 }
gokmenascioglu 0:f7e6527f4d8a 272
gokmenascioglu 0:f7e6527f4d8a 273 }
gokmenascioglu 0:f7e6527f4d8a 274 }
gokmenascioglu 0:f7e6527f4d8a 275
gokmenascioglu 0:f7e6527f4d8a 276 /* HEAD right MOVEMENT */
gokmenascioglu 0:f7e6527f4d8a 277 timer1.reset();
gokmenascioglu 0:f7e6527f4d8a 278 if ((x+2) < (k+1.7)) {
gokmenascioglu 0:f7e6527f4d8a 279
gokmenascioglu 0:f7e6527f4d8a 280 timer1.start();
gokmenascioglu 0:f7e6527f4d8a 281 accelerometer1();
gokmenascioglu 0:f7e6527f4d8a 282 wait_ms(10);
gokmenascioglu 0:f7e6527f4d8a 283
gokmenascioglu 0:f7e6527f4d8a 284 while((s+2)<(k+1.85)) {
gokmenascioglu 0:f7e6527f4d8a 285 end=timer1.read();
gokmenascioglu 0:f7e6527f4d8a 286 accelerometer1();
gokmenascioglu 0:f7e6527f4d8a 287 wait_ms(10);
gokmenascioglu 0:f7e6527f4d8a 288 if (end>1.5) {
gokmenascioglu 0:f7e6527f4d8a 289 while (s<-0.15) { accelerometer1(); }
gokmenascioglu 0:f7e6527f4d8a 290 x=5.0;
gokmenascioglu 0:f7e6527f4d8a 291 s=7.0;
gokmenascioglu 0:f7e6527f4d8a 292 end=0;
gokmenascioglu 0:f7e6527f4d8a 293 timer1.reset();
gokmenascioglu 0:f7e6527f4d8a 294 break;
gokmenascioglu 0:f7e6527f4d8a 295 }
gokmenascioglu 0:f7e6527f4d8a 296
gokmenascioglu 0:f7e6527f4d8a 297 }
gokmenascioglu 0:f7e6527f4d8a 298
gokmenascioglu 0:f7e6527f4d8a 299 if ( (x+2)< (k+1.7) ) {
gokmenascioglu 0:f7e6527f4d8a 300
gokmenascioglu 0:f7e6527f4d8a 301 accelerometer2();
gokmenascioglu 0:f7e6527f4d8a 302 timer2.reset();
gokmenascioglu 0:f7e6527f4d8a 303 timer2.start();
gokmenascioglu 0:f7e6527f4d8a 304
gokmenascioglu 0:f7e6527f4d8a 305 while((s1+2)>(k+1.85)) {
gokmenascioglu 0:f7e6527f4d8a 306 accelerometer2();
gokmenascioglu 0:f7e6527f4d8a 307 wait_ms(10);
gokmenascioglu 0:f7e6527f4d8a 308 end1=timer2.read();
gokmenascioglu 0:f7e6527f4d8a 309 ym=3;
gokmenascioglu 0:f7e6527f4d8a 310 if (end1>1.5) {
gokmenascioglu 0:f7e6527f4d8a 311 x=5;
gokmenascioglu 0:f7e6527f4d8a 312 ym=5;
gokmenascioglu 0:f7e6527f4d8a 313 k=5;
gokmenascioglu 0:f7e6527f4d8a 314 break;
gokmenascioglu 0:f7e6527f4d8a 315 }
gokmenascioglu 0:f7e6527f4d8a 316 }
gokmenascioglu 0:f7e6527f4d8a 317
gokmenascioglu 0:f7e6527f4d8a 318 }
gokmenascioglu 0:f7e6527f4d8a 319 }
gokmenascioglu 0:f7e6527f4d8a 320
gokmenascioglu 0:f7e6527f4d8a 321 if (ym==1){ pc.printf("head infront\n\r"); ym=5; wait(2.0); }
gokmenascioglu 0:f7e6527f4d8a 322 if (ym==2){ pc.printf("head back\n\r"); ym=5; wait(2.0); }
gokmenascioglu 0:f7e6527f4d8a 323 if (ym==3){ pc.printf("head right\n\r"); ym=5; wait(2.0); }
gokmenascioglu 0:f7e6527f4d8a 324 if (ym==4){ pc.printf("head left\n\r"); ym=5; wait(2.0); }
gokmenascioglu 0:f7e6527f4d8a 325
gokmenascioglu 0:f7e6527f4d8a 326
gokmenascioglu 0:f7e6527f4d8a 327 if (i==2000) {
gokmenascioglu 0:f7e6527f4d8a 328 led=0;
gokmenascioglu 0:f7e6527f4d8a 329 break;
gokmenascioglu 0:f7e6527f4d8a 330 }
gokmenascioglu 0:f7e6527f4d8a 331
gokmenascioglu 0:f7e6527f4d8a 332 }
gokmenascioglu 0:f7e6527f4d8a 333
gokmenascioglu 0:f7e6527f4d8a 334 }
gokmenascioglu 0:f7e6527f4d8a 335