f
Dependencies: hakan SDFileSystem mbed-src
main.cpp@0:f7e6527f4d8a, 2016-09-19 (annotated)
- Committer:
- gokmenascioglu
- Date:
- Mon Sep 19 21:35:10 2016 +0000
- Revision:
- 0:f7e6527f4d8a
i need mpu6050 sl
Who changed what in which revision?
User | Revision | Line number | New 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 |