FRA221_2015 / Mbed 2 deprecated 10A_ClassProject_MegnaticGame

hmc5883l

Dependencies:   HMC5883L LCD4884 MFRC522 eeprom mbed

project.cpp@10:7475e737bc0f, 2015-12-09 (annotated)

Committer:
1918june
Date:
Wed Dec 09 02:29:26 2015 +0000
Revision:
10:7475e737bc0f
Parent:
9:b83b9198988d
Child:
11:d55f0cc16089
pung leaw

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