FRA221_2015 / Mbed 2 deprecated 10A_ClassProject_MegnaticGame

hmc5883l

Dependencies:   HMC5883L LCD4884 MFRC522 eeprom mbed

project.cpp@13:9ad2742de34f, 2015-12-09 (annotated)

Committer:
1918june
Date:
Wed Dec 09 09:03:26 2015 +0000
Revision:
13:9ad2742de34f
Parent:
12:7fb5636ca854 
june

Who changed what in which revision?

UserRevisionLine numberNew contents of line
1918june 0:930d2bc16788 1 #include "mbed.h"
1918june 0:930d2bc16788 2 #include "LCD4884.h"
1918june 0:930d2bc16788 3 #include "MFRC522.h"
1918june 0:930d2bc16788 4 #include "eeprom.h"
1918june 0:930d2bc16788 5 #include "string.h"
1918june 5:4af4865f462b 6 #include "HMC5883L.h"
1918june 0:930d2bc16788 7
1918june 0:930d2bc16788 8
1918june 0:930d2bc16788 9 // KL25Z Pin for MFRC522 reset
1918june 0:930d2bc16788 10 #define MF_RESET D9
1918june 9:b83b9198988d 11 #define POINT_June 100
1918june 9:b83b9198988d 12 #define POINT_O 101
1918june 0:930d2bc16788 13
1918june 0:930d2bc16788 14
1918june 0:930d2bc16788 15 EEPROM eeprom(I2C_SDA,I2C_SCL,0);
1918june 0:930d2bc16788 16 Serial pc(SERIAL_TX,SERIAL_RX);
1918june 0:930d2bc16788 17 LCD4884 lcd;
1918june 0:930d2bc16788 18 MFRC522 RFID(D11, D12, D13, D10, MF_RESET);
1918june 3:987e54d48f77 19 InterruptIn Sw1(PC_5);
1918june 5:4af4865f462b 20 AnalogIn Rnd(A2);
1918june 3:987e54d48f77 21 DigitalIn Switch1(PC_5);
1918june 3:987e54d48f77 22 DigitalIn Switch2(PC_6);
1918june 3:987e54d48f77 23 InterruptIn Sw2(PC_6);
1918june 5:4af4865f462b 24 HMC5883L compass1(D14,D15);
1918june 5:4af4865f462b 25 HMC5883L compass2(D3,D6);
1918june 5:4af4865f462b 26 HMC5883L compass3(D5,D7);
1918june 5:4af4865f462b 27 DigitalOut servoPin(D4); //servo
1918june 11:d55f0cc16089 28
1918june 11:d55f0cc16089 29 DigitalOut m1inA1(PB_13); //Motor 1 //
1918june 11:d55f0cc16089 30 DigitalOut m1inA2(PB_14);//
1918june 11:d55f0cc16089 31 DigitalOut m1inB1(PB_15);//
1918june 11:d55f0cc16089 32 DigitalOut m1inB2(PB_12);//
1918june 11:d55f0cc16089 33
1918june 11:d55f0cc16089 34 DigitalOut m2inA1(PB_5); //Motor 2 //
1918june 9:b83b9198988d 35 DigitalOut m2inA2(A5);
1918june 9:b83b9198988d 36 DigitalOut m2inB1(A4);
1918june 11:d55f0cc16089 37 DigitalOut m2inB2(PB_1);
1918june 11:d55f0cc16089 38
1918june 5:4af4865f462b 39 AnalogIn X(A0); // Joystick
1918june 5:4af4865f462b 40 AnalogIn Y(A1);
1918june 5:4af4865f462b 41 int16_t data[12] = {0};
1918june 3:987e54d48f77 42 uint8_t mode=0;
1918june 8:b12138d249ba 43 uint8_t selectitem=0,place=0;
1918june 9:b83b9198988d 44 uint8_t point=0,user=0;
1918june 5:4af4865f462b 45
1918june 5:4af4865f462b 46 void m1step1()
1918june 5:4af4865f462b 47 {
1918june 5:4af4865f462b 48 m1inA1 = 0;
1918june 5:4af4865f462b 49 m1inA2 = 1;
1918june 5:4af4865f462b 50 m1inB1 = 1;
1918june 5:4af4865f462b 51 m1inB2 = 0;
1918june 11:d55f0cc16089 52 wait_ms(100);
1918june 5:4af4865f462b 53 }
1918june 5:4af4865f462b 54 void m1step2()
1918june 5:4af4865f462b 55 {
1918june 5:4af4865f462b 56 m1inA1 = 0;
1918june 5:4af4865f462b 57 m1inA2 = 1;
1918june 5:4af4865f462b 58 m1inB1 = 0;
1918june 5:4af4865f462b 59 m1inB2 = 1;
1918june 11:d55f0cc16089 60 wait_ms(100);
1918june 5:4af4865f462b 61 }
1918june 5:4af4865f462b 62 void m1step3()
1918june 5:4af4865f462b 63 {
1918june 5:4af4865f462b 64 m1inA1 = 1;
1918june 5:4af4865f462b 65 m1inA2 = 0;
1918june 5:4af4865f462b 66 m1inB1 = 0;
1918june 5:4af4865f462b 67 m1inB2 = 1;
1918june 11:d55f0cc16089 68 wait_ms(100);
1918june 5:4af4865f462b 69 }
1918june 5:4af4865f462b 70 void m1step4()
1918june 5:4af4865f462b 71 {
1918june 5:4af4865f462b 72 m1inA1 = 1;
1918june 5:4af4865f462b 73 m1inA2 = 0;
1918june 5:4af4865f462b 74 m1inB1 = 1;
1918june 5:4af4865f462b 75 m1inB2 = 0;
1918june 11:d55f0cc16089 76 wait_ms(100);
1918june 5:4af4865f462b 77 }
1918june 5:4af4865f462b 78 void m1stopMotor()
1918june 5:4af4865f462b 79 {
1918june 5:4af4865f462b 80 m1inA1 = 0;
1918june 5:4af4865f462b 81 m1inA2 = 0;
1918june 5:4af4865f462b 82 m1inB1 = 0;
1918june 5:4af4865f462b 83 m1inB2 = 0;
1918june 5:4af4865f462b 84 }
1918june 5:4af4865f462b 85
1918june 5:4af4865f462b 86 void m2step1()
1918june 5:4af4865f462b 87 {
1918june 5:4af4865f462b 88 m2inA1 = 0;
1918june 5:4af4865f462b 89 m2inA2 = 1;
1918june 5:4af4865f462b 90 m2inB1 = 1;
1918june 5:4af4865f462b 91 m2inB2 = 0;
1918june 11:d55f0cc16089 92 wait_ms(1);
1918june 0:930d2bc16788 93
1918june 5:4af4865f462b 94 }
1918june 5:4af4865f462b 95 void m2step2()
1918june 5:4af4865f462b 96 {
1918june 5:4af4865f462b 97 m2inA1 = 0;
1918june 5:4af4865f462b 98 m2inA2 = 1;
1918june 5:4af4865f462b 99 m2inB1 = 0;
1918june 5:4af4865f462b 100 m2inB2 = 1;
1918june 11:d55f0cc16089 101 wait_ms(1);
1918june 5:4af4865f462b 102 }
1918june 5:4af4865f462b 103 void m2step3()
1918june 5:4af4865f462b 104 {
1918june 5:4af4865f462b 105 m2inA1 = 1;
1918june 5:4af4865f462b 106 m2inA2 = 0;
1918june 5:4af4865f462b 107 m2inB1 = 0;
1918june 5:4af4865f462b 108 m2inB2 = 1;
1918june 11:d55f0cc16089 109 wait_ms(1);
1918june 5:4af4865f462b 110 }
1918june 5:4af4865f462b 111 void m2step4()
1918june 5:4af4865f462b 112 {
1918june 5:4af4865f462b 113 m2inA1 = 1;
1918june 5:4af4865f462b 114 m2inA2 = 0;
1918june 5:4af4865f462b 115 m2inB1 = 1;
1918june 5:4af4865f462b 116 m2inB2 = 0;
1918june 11:d55f0cc16089 117 wait_ms(1);
1918june 5:4af4865f462b 118 }
1918june 5:4af4865f462b 119 void m2stopMotor()
1918june 5:4af4865f462b 120 {
1918june 5:4af4865f462b 121 m2inA1 = 0;
1918june 5:4af4865f462b 122 m2inA2 = 0;
1918june 5:4af4865f462b 123 m2inB1 = 0;
1918june 5:4af4865f462b 124 m2inB2 = 0;
1918june 5:4af4865f462b 125 }
1918june 5:4af4865f462b 126 //PUT
1918june 5:4af4865f462b 127 void Down()
1918june 5:4af4865f462b 128 {
1918june 5:4af4865f462b 129 m1step1();
1918june 5:4af4865f462b 130 m1step2();
1918june 5:4af4865f462b 131 m1step3();
1918june 5:4af4865f462b 132 m1step4();
1918june 5:4af4865f462b 133 }
1918june 5:4af4865f462b 134 void Up()
1918june 5:4af4865f462b 135 {
1918june 5:4af4865f462b 136 m1step3();
1918june 5:4af4865f462b 137 m1step2();
1918june 5:4af4865f462b 138 m1step1();
1918june 5:4af4865f462b 139 m1step4();
1918june 5:4af4865f462b 140 }
1918june 5:4af4865f462b 141 void Left()
1918june 5:4af4865f462b 142 {
1918june 5:4af4865f462b 143 m2step1();
1918june 5:4af4865f462b 144 m2step2();
1918june 5:4af4865f462b 145 m2step3();
1918june 5:4af4865f462b 146 m2step4();
1918june 5:4af4865f462b 147 }
1918june 5:4af4865f462b 148 void Right()
1918june 5:4af4865f462b 149 {
1918june 5:4af4865f462b 150 m2step3();
1918june 5:4af4865f462b 151 m2step2();
1918june 5:4af4865f462b 152 m2step1();
1918june 5:4af4865f462b 153 m2step4();
1918june 5:4af4865f462b 154 }
1918june 5:4af4865f462b 155 void DownRight()
1918june 5:4af4865f462b 156 {
1918june 5:4af4865f462b 157 m1step1();
1918june 5:4af4865f462b 158 m1step2();
1918june 5:4af4865f462b 159 m1step3();
1918june 5:4af4865f462b 160 m1step4();
1918june 5:4af4865f462b 161 m2step3();
1918june 5:4af4865f462b 162 m2step2();
1918june 5:4af4865f462b 163 m2step1();
1918june 5:4af4865f462b 164 m2step4();
1918june 5:4af4865f462b 165 }
1918june 5:4af4865f462b 166 void DownLeft()
1918june 5:4af4865f462b 167 {
1918june 5:4af4865f462b 168 m1step1();
1918june 5:4af4865f462b 169 m1step2();
1918june 5:4af4865f462b 170 m1step3();
1918june 5:4af4865f462b 171 m1step4();
1918june 5:4af4865f462b 172 m2step1();
1918june 5:4af4865f462b 173 m2step2();
1918june 5:4af4865f462b 174 m2step3();
1918june 5:4af4865f462b 175 m2step4();
1918june 5:4af4865f462b 176 }
1918june 5:4af4865f462b 177 void UpRight()
1918june 5:4af4865f462b 178 {
1918june 5:4af4865f462b 179 m1step3();
1918june 5:4af4865f462b 180 m1step2();
1918june 5:4af4865f462b 181 m1step1();
1918june 5:4af4865f462b 182 m1step4();
1918june 5:4af4865f462b 183 m2step3();
1918june 5:4af4865f462b 184 m2step2();
1918june 5:4af4865f462b 185 m2step1();
1918june 5:4af4865f462b 186 m2step4();
1918june 5:4af4865f462b 187 }
1918june 5:4af4865f462b 188 void UpLeft()
1918june 5:4af4865f462b 189 {
1918june 5:4af4865f462b 190 m1step3();
1918june 5:4af4865f462b 191 m1step2();
1918june 5:4af4865f462b 192 m1step1();
1918june 5:4af4865f462b 193 m1step4();
1918june 5:4af4865f462b 194 m2step1();
1918june 5:4af4865f462b 195 m2step2();
1918june 5:4af4865f462b 196 m2step3();
1918june 5:4af4865f462b 197 m2step4();
1918june 5:4af4865f462b 198 }
1918june 5:4af4865f462b 199 void calibrate()
1918june 5:4af4865f462b 200 {
1918june 9:b83b9198988d 201 for(int i=0; i<=4; i++) {
1918june 9:b83b9198988d 202 servoPin = 1;
1918june 9:b83b9198988d 203 wait_us(700);
1918june 9:b83b9198988d 204 servoPin = 0;
1918june 9:b83b9198988d 205 wait(1);
1918june 9:b83b9198988d 206 }
1918june 9:b83b9198988d 207 //อ่านค่าSensor*************************
1918june 9:b83b9198988d 208 for(int i=0; i<=6; i++) {
1918june 9:b83b9198988d 209 servoPin = 1;
1918june 9:b83b9198988d 210 wait_us(2500);
1918june 9:b83b9198988d 211 servoPin = 0;
1918june 9:b83b9198988d 212 wait(1);
1918june 9:b83b9198988d 213 }
1918june 9:b83b9198988d 214
1918june 9:b83b9198988d 215 for (int i=0; i<=125; i++) {
1918june 9:b83b9198988d 216 m1step1();
1918june 9:b83b9198988d 217 m1step2();
1918june 9:b83b9198988d 218 m1step3();
1918june 9:b83b9198988d 219 m1step4();
1918june 9:b83b9198988d 220 }
1918june 9:b83b9198988d 221
1918june 9:b83b9198988d 222 m1stopMotor();
1918june 9:b83b9198988d 223 wait(1);
1918june 9:b83b9198988d 224
1918june 9:b83b9198988d 225 for(int i=0; i<=4; i++) {
1918june 9:b83b9198988d 226 servoPin = 1;
1918june 9:b83b9198988d 227 wait_us(700);
1918june 9:b83b9198988d 228 servoPin = 0;
1918june 9:b83b9198988d 229 wait(1);
1918june 9:b83b9198988d 230 }
1918june 9:b83b9198988d 231 //อ่านค่าSensor*************************
1918june 9:b83b9198988d 232 for(int i=0; i<=6; i++) {
1918june 9:b83b9198988d 233 servoPin = 1;
1918june 9:b83b9198988d 234 wait_us(2500);
1918june 9:b83b9198988d 235 servoPin = 0;
1918june 9:b83b9198988d 236 wait(1);
1918june 9:b83b9198988d 237 }
1918june 9:b83b9198988d 238
1918june 9:b83b9198988d 239 for (int i=0; i<=125; i++) {
1918june 9:b83b9198988d 240 m1step1();
1918june 9:b83b9198988d 241 m1step2();
1918june 9:b83b9198988d 242 m1step3();
1918june 9:b83b9198988d 243 m1step4();
1918june 9:b83b9198988d 244 }
1918june 9:b83b9198988d 245
1918june 9:b83b9198988d 246 m1stopMotor();
1918june 9:b83b9198988d 247 wait(1);
1918june 9:b83b9198988d 248
1918june 9:b83b9198988d 249 for(int i=0; i<=4; i++) {
1918june 9:b83b9198988d 250 servoPin = 1;
1918june 9:b83b9198988d 251 wait_us(700);
1918june 9:b83b9198988d 252 servoPin = 0;
1918june 9:b83b9198988d 253 wait(1);
1918june 9:b83b9198988d 254 }
1918june 9:b83b9198988d 255 //อ่านค่าSensor*************************
1918june 9:b83b9198988d 256 for(int i=0; i<=6; i++) {
1918june 9:b83b9198988d 257 servoPin = 1;
1918june 9:b83b9198988d 258 wait_us(2500);
1918june 9:b83b9198988d 259 servoPin = 0;
1918june 9:b83b9198988d 260 wait(1);
1918june 9:b83b9198988d 261 }
1918june 9:b83b9198988d 262
1918june 9:b83b9198988d 263 for (int i=0; i<=150; i++) {
1918june 9:b83b9198988d 264 m2step1();
1918june 9:b83b9198988d 265 m2step2();
1918june 9:b83b9198988d 266 m2step3();
1918june 9:b83b9198988d 267 m2step4();
1918june 9:b83b9198988d 268 }
1918june 9:b83b9198988d 269
1918june 9:b83b9198988d 270 m2stopMotor();
1918june 9:b83b9198988d 271 wait(1);
1918june 9:b83b9198988d 272
1918june 9:b83b9198988d 273 for(int i=0; i<=4; i++) {
1918june 9:b83b9198988d 274 servoPin = 1;
1918june 9:b83b9198988d 275 wait_us(700);
1918june 9:b83b9198988d 276 servoPin = 0;
1918june 9:b83b9198988d 277 wait(1);
1918june 9:b83b9198988d 278 }
1918june 9:b83b9198988d 279 //อ่านค่าSensor*************************
1918june 9:b83b9198988d 280 for(int i=0; i<=6; i++) {
1918june 9:b83b9198988d 281 servoPin = 1;
1918june 9:b83b9198988d 282 wait_us(2500);
1918june 9:b83b9198988d 283 servoPin = 0;
1918june 9:b83b9198988d 284 wait(1);
1918june 9:b83b9198988d 285 }
1918june 9:b83b9198988d 286
1918june 9:b83b9198988d 287 for (int i=0; i<=125; i++) {
1918june 9:b83b9198988d 288 m1step3();
1918june 9:b83b9198988d 289 m1step2();
1918june 9:b83b9198988d 290 m1step1();
1918june 9:b83b9198988d 291 m1step4();
1918june 9:b83b9198988d 292 }
1918june 9:b83b9198988d 293
1918june 9:b83b9198988d 294 m1stopMotor();
1918june 9:b83b9198988d 295 wait(1);
1918june 9:b83b9198988d 296
1918june 9:b83b9198988d 297 for(int i=0; i<=4; i++) {
1918june 9:b83b9198988d 298 servoPin = 1;
1918june 9:b83b9198988d 299 wait_us(700);
1918june 9:b83b9198988d 300 servoPin = 0;
1918june 9:b83b9198988d 301 wait(1);
1918june 9:b83b9198988d 302 }
1918june 9:b83b9198988d 303 //อ่านค่าSensor*************************
1918june 9:b83b9198988d 304 for(int i=0; i<=6; i++) {
1918june 9:b83b9198988d 305 servoPin = 1;
1918june 9:b83b9198988d 306 wait_us(2500);
1918june 9:b83b9198988d 307 servoPin = 0;
1918june 9:b83b9198988d 308 wait(1);
1918june 9:b83b9198988d 309 }
1918june 9:b83b9198988d 310
1918june 9:b83b9198988d 311 for (int i=0; i<=125; i++) {
1918june 9:b83b9198988d 312 m1step3();
1918june 9:b83b9198988d 313 m1step2();
1918june 9:b83b9198988d 314 m1step1();
1918june 9:b83b9198988d 315 m1step4();
1918june 9:b83b9198988d 316 }
1918june 9:b83b9198988d 317
1918june 9:b83b9198988d 318 m1stopMotor();
1918june 9:b83b9198988d 319 wait(1);
1918june 9:b83b9198988d 320
1918june 9:b83b9198988d 321 for(int i=0; i<=4; i++) {
1918june 9:b83b9198988d 322 servoPin = 1;
1918june 9:b83b9198988d 323 wait_us(700);
1918june 9:b83b9198988d 324 servoPin = 0;
1918june 9:b83b9198988d 325 wait(1);
1918june 9:b83b9198988d 326 }
1918june 9:b83b9198988d 327 //อ่านค่าSensor*************************
1918june 9:b83b9198988d 328 for(int i=0; i<=6; i++) {
1918june 9:b83b9198988d 329 servoPin = 1;
1918june 9:b83b9198988d 330 wait_us(2500);
1918june 9:b83b9198988d 331 servoPin = 0;
1918june 9:b83b9198988d 332 wait(1);
1918june 9:b83b9198988d 333 }
1918june 9:b83b9198988d 334
1918june 9:b83b9198988d 335 for (int i=0; i<=150; i++) {
1918june 9:b83b9198988d 336 m2step1();
1918june 9:b83b9198988d 337 m2step2();
1918june 9:b83b9198988d 338 m2step3();
1918june 9:b83b9198988d 339 m2step4();
1918june 9:b83b9198988d 340 }
1918june 9:b83b9198988d 341
1918june 9:b83b9198988d 342 m2stopMotor();
1918june 9:b83b9198988d 343 wait(1);
1918june 9:b83b9198988d 344
1918june 9:b83b9198988d 345 for(int i=0; i<=4; i++) {
1918june 9:b83b9198988d 346 servoPin = 1;
1918june 9:b83b9198988d 347 wait_us(700);
1918june 9:b83b9198988d 348 servoPin = 0;
1918june 9:b83b9198988d 349 wait(1);
1918june 9:b83b9198988d 350 }
1918june 9:b83b9198988d 351 //อ่านค่าSensor*************************
1918june 9:b83b9198988d 352 for(int i=0; i<=6; i++) {
1918june 9:b83b9198988d 353 servoPin = 1;
1918june 9:b83b9198988d 354 wait_us(2500);
1918june 9:b83b9198988d 355 servoPin = 0;
1918june 9:b83b9198988d 356 wait(1);
1918june 9:b83b9198988d 357 }
1918june 9:b83b9198988d 358
1918june 9:b83b9198988d 359 for (int i=0; i<=125; i++) {
1918june 9:b83b9198988d 360 m1step1();
1918june 9:b83b9198988d 361 m1step2();
1918june 9:b83b9198988d 362 m1step3();
1918june 9:b83b9198988d 363 m1step4();
1918june 9:b83b9198988d 364 }
1918june 9:b83b9198988d 365
1918june 9:b83b9198988d 366 m1stopMotor();
1918june 9:b83b9198988d 367 wait(1);
1918june 9:b83b9198988d 368
1918june 9:b83b9198988d 369 for(int i=0; i<=4; i++) {
1918june 9:b83b9198988d 370 servoPin = 1;
1918june 9:b83b9198988d 371 wait_us(700);
1918june 9:b83b9198988d 372 servoPin = 0;
1918june 9:b83b9198988d 373 wait(1);
1918june 9:b83b9198988d 374 }
1918june 9:b83b9198988d 375 //อ่านค่าSensor*************************
1918june 9:b83b9198988d 376 for(int i=0; i<=6; i++) {
1918june 9:b83b9198988d 377 servoPin = 1;
1918june 9:b83b9198988d 378 wait_us(2500);
1918june 9:b83b9198988d 379 servoPin = 0;
1918june 9:b83b9198988d 380 wait(1);
1918june 9:b83b9198988d 381 }
1918june 9:b83b9198988d 382
1918june 9:b83b9198988d 383 for (int i=0; i<=125; i++) {
1918june 9:b83b9198988d 384 m1step1();
1918june 9:b83b9198988d 385 m1step2();
1918june 9:b83b9198988d 386 m1step3();
1918june 9:b83b9198988d 387 m1step4();
1918june 9:b83b9198988d 388 }
1918june 9:b83b9198988d 389
1918june 9:b83b9198988d 390 m1stopMotor();
1918june 9:b83b9198988d 391 wait(1);
1918june 9:b83b9198988d 392
1918june 9:b83b9198988d 393 for(int i=0; i<=4; i++) {
1918june 9:b83b9198988d 394 servoPin = 1;
1918june 9:b83b9198988d 395 wait_us(700);
1918june 9:b83b9198988d 396 servoPin = 0;
1918june 9:b83b9198988d 397 wait(1);
1918june 9:b83b9198988d 398 }
1918june 9:b83b9198988d 399 //อ่านค่าSensor*************************
1918june 9:b83b9198988d 400 for(int i=0; i<=6; i++) {
1918june 9:b83b9198988d 401 servoPin = 1;
1918june 9:b83b9198988d 402 wait_us(2500);
1918june 9:b83b9198988d 403 servoPin = 0;
1918june 9:b83b9198988d 404 wait(1);
1918june 9:b83b9198988d 405 }
1918june 5:4af4865f462b 406
1918june 5:4af4865f462b 407 }
1918june 9:b83b9198988d 408
1918june 9:b83b9198988d 409
1918june 5:4af4865f462b 410 void control()
1918june 5:4af4865f462b 411 {
1918june 9:b83b9198988d 412 float x,y;
1918june 5:4af4865f462b 413 x = X.read()*360;
1918june 5:4af4865f462b 414 y = Y.read()*360;
1918june 12:7fb5636ca854 415 // printf("X = %f.2\tY = %f.2\n",x,y);
1918june 5:4af4865f462b 416 if(x > 200) {
1918june 5:4af4865f462b 417 Up();
1918june 5:4af4865f462b 418 }
1918june 5:4af4865f462b 419 if(x < 160) {
1918june 5:4af4865f462b 420 Down();
1918june 5:4af4865f462b 421 }
1918june 5:4af4865f462b 422 if(y > 200) {
1918june 5:4af4865f462b 423 Right();
1918june 5:4af4865f462b 424 }
1918june 5:4af4865f462b 425 if(y < 160) {
1918june 5:4af4865f462b 426 Left();
1918june 5:4af4865f462b 427 }
1918june 5:4af4865f462b 428
1918june 5:4af4865f462b 429 }
1918june 5:4af4865f462b 430 void servodown()
1918june 5:4af4865f462b 431 {
1918june 5:4af4865f462b 432 for(int i=0; i<=6; i++) {
1918june 5:4af4865f462b 433 servoPin = 1;
1918june 5:4af4865f462b 434 wait_us(500);
1918june 5:4af4865f462b 435 servoPin = 0;
1918june 5:4af4865f462b 436 wait(1);
1918june 5:4af4865f462b 437 }
1918june 5:4af4865f462b 438 }
1918june 5:4af4865f462b 439 void servoup()
1918june 5:4af4865f462b 440 {
1918june 5:4af4865f462b 441 for(int i=0; i<=6; i++) {
1918june 5:4af4865f462b 442 servoPin = 1;
1918june 5:4af4865f462b 443 wait_us(2500);
1918june 5:4af4865f462b 444 servoPin = 0;
1918june 5:4af4865f462b 445 wait(1);
1918june 5:4af4865f462b 446 }
1918june 5:4af4865f462b 447 }
1918june 5:4af4865f462b 448 void read(int16_t data[9])
1918june 5:4af4865f462b 449 {
1918june 5:4af4865f462b 450 int16_t a[3],b[3],c[3];
1918june 5:4af4865f462b 451
1918june 5:4af4865f462b 452 int w=0;
1918june 5:4af4865f462b 453
1918june 5:4af4865f462b 454 compass1.getXYZ(a);
1918june 5:4af4865f462b 455 compass2.getXYZ(b);
1918june 5:4af4865f462b 456 compass3.getXYZ(c);
1918june 5:4af4865f462b 457 for(int i=0; i<3; i++) {
1918june 5:4af4865f462b 458 data[w]=a[i]/10;
1918june 5:4af4865f462b 459 w++;
1918june 5:4af4865f462b 460 }
1918june 5:4af4865f462b 461
1918june 5:4af4865f462b 462 for(int i=0; i<3; i++) {
1918june 5:4af4865f462b 463 data[w]=b[i]/10;
1918june 5:4af4865f462b 464 w++;
1918june 5:4af4865f462b 465 }
1918june 5:4af4865f462b 466 for(int i=0; i<3; i++) {
1918june 5:4af4865f462b 467 data[w]=c[i]/10;
1918june 5:4af4865f462b 468 w++;
1918june 5:4af4865f462b 469 }
1918june 5:4af4865f462b 470
1918june 5:4af4865f462b 471 }
1918june 5:4af4865f462b 472
1918june 5:4af4865f462b 473 void dataprint(int16_t data[9])
1918june 5:4af4865f462b 474 {
1918june 5:4af4865f462b 475 for(int c=0; c<9; c++) {
1918june 5:4af4865f462b 476 printf("%d ",data[c]);
1918june 12:7fb5636ca854 477 // if((c+1)%3==0) {
1918june 12:7fb5636ca854 478 // printf("||");
1918june 12:7fb5636ca854 479 //}
1918june 5:4af4865f462b 480 }
1918june 5:4af4865f462b 481 printf("\n");
1918june 5:4af4865f462b 482 }
1918june 3:987e54d48f77 483 void ItrSw1()
1918june 3:987e54d48f77 484 {
1918june 3:987e54d48f77 485 mode=1;
1918june 3:987e54d48f77 486 }
1918june 3:987e54d48f77 487
1918june 3:987e54d48f77 488 void ItrSw2()
1918june 3:987e54d48f77 489 {
1918june 3:987e54d48f77 490 mode=2;
1918june 3:987e54d48f77 491 }
1918june 3:987e54d48f77 492
1918june 3:987e54d48f77 493 void ItrSw3()
1918june 3:987e54d48f77 494 {
1918june 3:987e54d48f77 495 mode=0;
1918june 3:987e54d48f77 496 }
1918june 3:987e54d48f77 497 void printstring(int x,int y,string s,int n,int m)
1918june 3:987e54d48f77 498 {
1918june 2:38376c7c1685 499 int k=0;
1918june 2:38376c7c1685 500 lcd.LCD_set_XY(x,y);
1918june 3:987e54d48f77 501 while(k<n) {
1918june 3:987e54d48f77 502 lcd.LCD_write_char(s[k],m);
1918june 3:987e54d48f77 503 k++;
1918june 2:38376c7c1685 504 }
1918june 2:38376c7c1685 505 }
1918june 9:b83b9198988d 506 int checkpoint()
1918june 9:b83b9198988d 507 {
1918june 9:b83b9198988d 508 int p;
1918june 12:7fb5636ca854 509 //p= //สมการคำนวน excel
1918june 9:b83b9198988d 510 return p;
1918june 9:b83b9198988d 511 }
1918june 0:930d2bc16788 512 int main()
1918june 0:930d2bc16788 513 {
1918june 0:930d2bc16788 514 pc.baud(9600);
1918june 3:987e54d48f77 515 Sw1.rise(&ItrSw1);
1918june 3:987e54d48f77 516 Sw2.rise(&ItrSw2);
1918june 5:4af4865f462b 517 compass1.init();
1918june 5:4af4865f462b 518 compass2.init();
1918june 5:4af4865f462b 519 compass3.init();
1918june 3:987e54d48f77 520 string NameJune="Natthanicha",recieveNameJune,SurJune="Jamroonpan",recieveSurJune,IdJune="57340500023",recieveIdJune; //June start @ 1-36
1918june 3:987e54d48f77 521 string NameO="Sirawat",recieveNameO,SurO="Sok",recieveSurO,IdO="57340500071",recieveIdO;
1918june 3:987e54d48f77 522 uint8_t UID[8],recieveUID[8];
1918june 2:38376c7c1685 523 UID[0]=106;
1918june 2:38376c7c1685 524 UID[1]=224;
1918june 2:38376c7c1685 525 UID[2]=37;
1918june 2:38376c7c1685 526 UID[3]=231;
1918june 3:987e54d48f77 527 UID[4]=106;
1918june 3:987e54d48f77 528 UID[5]=76;
1918june 3:987e54d48f77 529 UID[6]=39;
1918june 3:987e54d48f77 530 UID[7]=231;
1918june 3:987e54d48f77 531
1918june 13:9ad2742de34f 532 eeprom.write(1,&NameJune,11);
1918june 3:987e54d48f77 533 wait_us(1);
1918june 3:987e54d48f77 534 eeprom.write(12,&SurJune,10);
1918june 3:987e54d48f77 535 wait_us(1);
1918june 3:987e54d48f77 536 eeprom.write(22,&IdJune,11);
1918june 3:987e54d48f77 537 wait_us(1);
1918june 3:987e54d48f77 538 eeprom.write(41,&NameO,7);
1918june 3:987e54d48f77 539 wait_us(1);
1918june 3:987e54d48f77 540 eeprom.write(48,&SurO,3);
1918june 3:987e54d48f77 541 wait_us(1);
1918june 3:987e54d48f77 542 eeprom.write(51,&IdO,11);
1918june 3:987e54d48f77 543 wait_us(1);
1918june 3:987e54d48f77 544 eeprom.write(81,UID,8);
1918june 13:9ad2742de34f 545 wait_us(1);
1918june 3:987e54d48f77 546 eeprom.read(1,&recieveNameJune,11);
1918june 3:987e54d48f77 547 eeprom.read(12,&recieveSurJune,10);
1918june 3:987e54d48f77 548 eeprom.read(22,&recieveIdJune,11);
1918june 3:987e54d48f77 549 eeprom.read(41,&recieveNameO,7);
1918june 3:987e54d48f77 550 eeprom.read(48,&recieveSurO,3);
1918june 3:987e54d48f77 551 eeprom.read(51,&recieveIdO,11);
1918june 3:987e54d48f77 552 eeprom.read(81,recieveUID,8);
1918june 3:987e54d48f77 553 //pc.printf("%s\n",recieveNameJune);
1918june 3:987e54d48f77 554 //pc.printf("%s\n",recieveSurJune);
1918june 3:987e54d48f77 555 //pc.printf("%s\n",recieveIdJune);
1918june 3:987e54d48f77 556 //pc.printf("%s\n",recieveNameO);
1918june 3:987e54d48f77 557 //pc.printf("%s\n",recieveSurO);
1918june 3:987e54d48f77 558 //pc.printf("%s\n",recieveIdO);
1918june 3:987e54d48f77 559 //pc.printf("%d %d %d %d %d %d %d %d\n",recieveUID[0],recieveUID[1],recieveUID[2],recieveUID[3],recieveUID[4],recieveUID[5],recieveUID[6],recieveUID[7]);
1918june 2:38376c7c1685 560 RFID.PCD_Init();
1918june 0:930d2bc16788 561 lcd.LCD_init();
1918june 0:930d2bc16788 562 lcd.backlight(ON);
1918june 2:38376c7c1685 563 wait(2);
1918june 0:930d2bc16788 564 lcd.LCD_clear();
1918june 13:9ad2742de34f 565 servoup();
1918june 3:987e54d48f77 566 while(1) {
1918june 4:b56a8a051fdd 567 lcd.LCD_write_string(0,0,"SCAN ID CARD",0);
1918june 12:7fb5636ca854 568 if ( ! RFID.PICC_IsNewCardPresent()) // Look for new cards read all time
1918june 0:930d2bc16788 569 continue;
1918june 12:7fb5636ca854 570 if ( ! RFID.PICC_ReadCardSerial() ) // Select one of the cards check card
1918june 0:930d2bc16788 571 continue;
1918june 0:930d2bc16788 572 pc.printf("Card UID: ");
1918june 0:930d2bc16788 573 for (uint8_t i = 0; i < RFID.uid.size; i++)
1918june 0:930d2bc16788 574 pc.printf(" %d", RFID.uid.uidByte[i]);
1918june 0:930d2bc16788 575 printf("\n\r");
1918june 3:987e54d48f77 576 if( RFID.uid.uidByte[0]==recieveUID[0] && RFID.uid.uidByte[1]==recieveUID[1] && RFID.uid.uidByte[2]==recieveUID[2] && RFID.uid.uidByte[3]==recieveUID[3] ) {
1918june 3:987e54d48f77 577 //จำ
1918june 3:987e54d48f77 578 pc.printf("%s\t%s\t%s\n",recieveNameJune,recieveSurJune,recieveIdJune);
1918june 4:b56a8a051fdd 579 lcd.LCD_clear();
1918june 12:7fb5636ca854 580 string name,sur,id;
1918june 3:987e54d48f77 581 eeprom.read(1,&name,11);
1918june 3:987e54d48f77 582 eeprom.read(12,&sur,10);
1918june 12:7fb5636ca854 583 eeprom.read(22,&id,11);
1918june 3:987e54d48f77 584 printstring(0,0,name,11,0);
1918june 3:987e54d48f77 585 printstring(0,1,sur,10,0);
1918june 12:7fb5636ca854 586 printstring(0,2,id,11,0);
1918june 12:7fb5636ca854 587 lcd.LCD_write_string(0,4,"1 select item",0);
1918june 12:7fb5636ca854 588 lcd.LCD_write_string(0,5,"2 select place",0);
1918june 9:b83b9198988d 589 user=1;
1918june 3:987e54d48f77 590 //lcd.LCD_clear();
1918june 3:987e54d48f77 591 }
1918june 12:7fb5636ca854 592 else if( RFID.uid.uidByte[0]==recieveUID[4] && RFID.uid.uidByte[1]==recieveUID[5] && RFID.uid.uidByte[2]==recieveUID[6] && RFID.uid.uidByte[3]==recieveUID[7] ) {
1918june 3:987e54d48f77 593 pc.printf("%s\t%s\t%s\n",recieveNameO,recieveSurO,recieveIdO);
1918june 12:7fb5636ca854 594 string name,sur,id;
1918june 4:b56a8a051fdd 595 lcd.LCD_clear();
1918june 3:987e54d48f77 596 eeprom.read(41,&name,7);
1918june 3:987e54d48f77 597 eeprom.read(48,&sur,3);
1918june 12:7fb5636ca854 598 eeprom.read(51,&id,11);
1918june 3:987e54d48f77 599 printstring(0,0,name,7,0);
1918june 3:987e54d48f77 600 printstring(0,1,sur,3,0);
1918june 12:7fb5636ca854 601 printstring(0,2,id,11,0);
1918june 12:7fb5636ca854 602 lcd.LCD_write_string(0,4,"1 select item",0);
1918june 12:7fb5636ca854 603 lcd.LCD_write_string(0,5,"2 select place",0);
1918june 9:b83b9198988d 604 user=2;
1918june 0:930d2bc16788 605 }
1918june 12:7fb5636ca854 606 else main();
1918june 12:7fb5636ca854 607
1918june 12:7fb5636ca854 608 while(1) {
1918june 12:7fb5636ca854 609 if(mode==1) {
1918june 12:7fb5636ca854 610 printf("1");
1918june 12:7fb5636ca854 611 lcd.LCD_clear();
1918june 12:7fb5636ca854 612 lcd.LCD_write_string(0,0,"mode 1",0);
1918june 12:7fb5636ca854 613 lcd.LCD_write_string(0,1,"select item",0);
1918june 12:7fb5636ca854 614 while(1) {
1918june 12:7fb5636ca854 615 if(Switch1.read()==1) {
1918june 12:7fb5636ca854 616 char str[10];
1918june 12:7fb5636ca854 617 selectitem++;
1918june 12:7fb5636ca854 618 if(selectitem > 9)
1918june 12:7fb5636ca854 619 selectitem=1;
1918june 12:7fb5636ca854 620 wait(0.2);
1918june 12:7fb5636ca854 621 sprintf(str,"item : %d",selectitem);
1918june 12:7fb5636ca854 622 printstring(3,2,str,8,0);
1918june 3:987e54d48f77 623 }
1918june 12:7fb5636ca854 624 if(Switch2.read()==1 && selectitem !=0) {
1918june 12:7fb5636ca854 625 mode=1;
1918june 12:7fb5636ca854 626 char str[10];
1918june 12:7fb5636ca854 627 place=((int) (Rnd.read()*1000))%4 +1;
1918june 12:7fb5636ca854 628 wait(0.2);
1918june 12:7fb5636ca854 629 sprintf(str,"place :%d",place);
1918june 12:7fb5636ca854 630 printstring(3,3,str,8,0);
1918june 12:7fb5636ca854 631 break;
1918june 12:7fb5636ca854 632 }
1918june 12:7fb5636ca854 633 }
1918june 12:7fb5636ca854 634 wait(1);
1918june 9:b83b9198988d 635
1918june 12:7fb5636ca854 636 while(1) { //
1918june 12:7fb5636ca854 637 control();
1918june 12:7fb5636ca854 638 if(Switch2.read()==1) {
1918june 12:7fb5636ca854 639 char str[12];
1918june 12:7fb5636ca854 640 mode=1;
1918june 12:7fb5636ca854 641 servodown();//servo down
1918june 12:7fb5636ca854 642 read(data);
1918june 12:7fb5636ca854 643 dataprint(data);
1918june 12:7fb5636ca854 644 servoup();//servo up
1918june 12:7fb5636ca854 645 point = checkpoint();
1918june 12:7fb5636ca854 646 sprintf(str,"point = %d",point);
1918june 12:7fb5636ca854 647 lcd.LCD_clear();
1918june 12:7fb5636ca854 648 lcd.LCD_write_string(0,1,str,0);
1918june 12:7fb5636ca854 649 if(user==1)
1918june 12:7fb5636ca854 650 eeprom.write(POINT_June,&point,1);
1918june 12:7fb5636ca854 651 if(user==2)
1918june 12:7fb5636ca854 652 eeprom.write(POINT_O,&point,1);
1918june 12:7fb5636ca854 653 break;
1918june 11:d55f0cc16089 654 }
1918june 4:b56a8a051fdd 655 }
1918june 13:9ad2742de34f 656 mode=0;
1918june 13:9ad2742de34f 657 main();
1918june 12:7fb5636ca854 658 }
1918june 12:7fb5636ca854 659 if(mode==2) {//selectplace
1918june 12:7fb5636ca854 660 printf("2");
1918june 12:7fb5636ca854 661 lcd.LCD_clear();
1918june 12:7fb5636ca854 662 lcd.LCD_write_string(0,0,"mode 2",0);
1918june 12:7fb5636ca854 663 lcd.LCD_write_string(0,1,"select place",0);
1918june 12:7fb5636ca854 664 while(1) {
1918june 12:7fb5636ca854 665 if(Switch1.read()==1) {
1918june 12:7fb5636ca854 666 mode=2;
1918june 12:7fb5636ca854 667 char str[10];
1918june 12:7fb5636ca854 668 place++;
1918june 12:7fb5636ca854 669 if(place > 4)
1918june 12:7fb5636ca854 670 place=1;
1918june 12:7fb5636ca854 671 wait(0.2);
1918june 12:7fb5636ca854 672 sprintf(str,"place : %d",place);
1918june 12:7fb5636ca854 673 printstring(3,2,str,9,0);
1918june 12:7fb5636ca854 674 }
1918june 12:7fb5636ca854 675 if(Switch2.read()==1 && place!=0) {
1918june 12:7fb5636ca854 676 char str[10];
1918june 12:7fb5636ca854 677 selectitem=((int) (Rnd.read()*1000))%9 +1;
1918june 12:7fb5636ca854 678 wait(0.2);
1918june 12:7fb5636ca854 679 sprintf(str,"item : %d",selectitem);
1918june 12:7fb5636ca854 680 printstring(3,3,str,8,0);
1918june 12:7fb5636ca854 681 break;
1918june 12:7fb5636ca854 682 }
1918june 12:7fb5636ca854 683 }
1918june 12:7fb5636ca854 684 wait(1);
1918june 12:7fb5636ca854 685 while(1) { //
1918june 12:7fb5636ca854 686 control();
1918june 12:7fb5636ca854 687 if(Switch2.read()==1) {
1918june 12:7fb5636ca854 688 char str[12];
1918june 12:7fb5636ca854 689 mode=2;
1918june 12:7fb5636ca854 690 servodown();//servo down
1918june 12:7fb5636ca854 691 read(data);
1918june 12:7fb5636ca854 692 dataprint(data);
1918june 12:7fb5636ca854 693 servoup();//servo up
1918june 12:7fb5636ca854 694 point = checkpoint();
1918june 12:7fb5636ca854 695 sprintf(str,"point = %d",point);
1918june 12:7fb5636ca854 696 lcd.LCD_clear();
1918june 12:7fb5636ca854 697 lcd.LCD_write_string(0,1,str,0);
1918june 12:7fb5636ca854 698 if(user==1)
1918june 12:7fb5636ca854 699 eeprom.write(POINT_June,&point,1);
1918june 12:7fb5636ca854 700 if(user==2)
1918june 12:7fb5636ca854 701 eeprom.write(POINT_O,&point,1);
1918june 13:9ad2742de34f 702 break; }
1918june 12:7fb5636ca854 703 }
1918june 13:9ad2742de34f 704 mode=0;
1918june 13:9ad2742de34f 705 main();
1918june 12:7fb5636ca854 706 }
1918june 12:7fb5636ca854 707 // printf("mode=%d\n",mode);
1918june 12:7fb5636ca854 708 } //end while mode loop
1918june 12:7fb5636ca854 709 }
1918june 3:987e54d48f77 710
1918june 3:987e54d48f77 711 }
1918june 4:b56a8a051fdd 712