FRA221_2015 / Mbed 2 deprecated 10A_ClassProject_MegnaticGame

hmc5883l

Dependencies:   HMC5883L LCD4884 MFRC522 eeprom mbed

project.cpp@11:d55f0cc16089, 2015-12-09 (annotated)

Committer:
1918june
Date:
Wed Dec 09 04:01:58 2015 +0000
Revision:
11:d55f0cc16089
Parent:
10:7475e737bc0f
Child:
12:7fb5636ca854
magnetic

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 10:7475e737bc0f 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 10:7475e737bc0f 477 if((c+1)%3==0){printf("||");}
1918june 5:4af4865f462b 478 }
1918june 5:4af4865f462b 479 printf("\n");
1918june 5:4af4865f462b 480 }
1918june 3:987e54d48f77 481 void ItrSw1()
1918june 3:987e54d48f77 482 {
1918june 3:987e54d48f77 483 mode=1;
1918june 3:987e54d48f77 484 }
1918june 3:987e54d48f77 485
1918june 3:987e54d48f77 486 void ItrSw2()
1918june 3:987e54d48f77 487 {
1918june 3:987e54d48f77 488 mode=2;
1918june 3:987e54d48f77 489 }
1918june 3:987e54d48f77 490
1918june 3:987e54d48f77 491 void ItrSw3()
1918june 3:987e54d48f77 492 {
1918june 3:987e54d48f77 493 mode=0;
1918june 3:987e54d48f77 494 }
1918june 3:987e54d48f77 495 void printstring(int x,int y,string s,int n,int m)
1918june 3:987e54d48f77 496 {
1918june 2:38376c7c1685 497 int k=0;
1918june 2:38376c7c1685 498 lcd.LCD_set_XY(x,y);
1918june 3:987e54d48f77 499 while(k<n) {
1918june 3:987e54d48f77 500 lcd.LCD_write_char(s[k],m);
1918june 3:987e54d48f77 501 k++;
1918june 2:38376c7c1685 502 }
1918june 2:38376c7c1685 503 }
1918june 9:b83b9198988d 504 int checkpoint()
1918june 9:b83b9198988d 505 {
1918june 9:b83b9198988d 506 int p;
1918june 9:b83b9198988d 507 //p= //สมการคำนวน
1918june 9:b83b9198988d 508 return p;
1918june 9:b83b9198988d 509 }
1918june 0:930d2bc16788 510 int main()
1918june 0:930d2bc16788 511 {
1918june 0:930d2bc16788 512 pc.baud(9600);
1918june 3:987e54d48f77 513 Sw1.rise(&ItrSw1);
1918june 3:987e54d48f77 514 Sw2.rise(&ItrSw2);
1918june 5:4af4865f462b 515 compass1.init();
1918june 5:4af4865f462b 516 compass2.init();
1918june 5:4af4865f462b 517 compass3.init();
1918june 10:7475e737bc0f 518
1918june 3:987e54d48f77 519 string NameJune="Natthanicha",recieveNameJune,SurJune="Jamroonpan",recieveSurJune,IdJune="57340500023",recieveIdJune; //June start @ 1-36
1918june 3:987e54d48f77 520 string NameO="Sirawat",recieveNameO,SurO="Sok",recieveSurO,IdO="57340500071",recieveIdO;
1918june 3:987e54d48f77 521 uint8_t UID[8],recieveUID[8];
1918june 2:38376c7c1685 522 UID[0]=106;
1918june 2:38376c7c1685 523 UID[1]=224;
1918june 2:38376c7c1685 524 UID[2]=37;
1918june 2:38376c7c1685 525 UID[3]=231;
1918june 3:987e54d48f77 526 UID[4]=106;
1918june 3:987e54d48f77 527 UID[5]=76;
1918june 3:987e54d48f77 528 UID[6]=39;
1918june 3:987e54d48f77 529 UID[7]=231;
1918june 3:987e54d48f77 530
1918june 3:987e54d48f77 531 eeprom.write(1,&NameJune,11);
1918june 3:987e54d48f77 532 wait_us(1);
1918june 3:987e54d48f77 533 eeprom.write(12,&SurJune,10);
1918june 3:987e54d48f77 534 wait_us(1);
1918june 3:987e54d48f77 535 eeprom.write(22,&IdJune,11);
1918june 3:987e54d48f77 536 wait_us(1);
1918june 3:987e54d48f77 537 eeprom.write(41,&NameO,7);
1918june 3:987e54d48f77 538 wait_us(1);
1918june 3:987e54d48f77 539 eeprom.write(48,&SurO,3);
1918june 3:987e54d48f77 540 wait_us(1);
1918june 3:987e54d48f77 541 eeprom.write(51,&IdO,11);
1918june 3:987e54d48f77 542 wait_us(1);
1918june 3:987e54d48f77 543 eeprom.write(81,UID,8);
1918june 3:987e54d48f77 544 wait_us(1);
1918june 3:987e54d48f77 545 eeprom.read(1,&recieveNameJune,11);
1918june 3:987e54d48f77 546 eeprom.read(12,&recieveSurJune,10);
1918june 3:987e54d48f77 547 eeprom.read(22,&recieveIdJune,11);
1918june 3:987e54d48f77 548 eeprom.read(41,&recieveNameO,7);
1918june 3:987e54d48f77 549 eeprom.read(48,&recieveSurO,3);
1918june 3:987e54d48f77 550 eeprom.read(51,&recieveIdO,11);
1918june 3:987e54d48f77 551 eeprom.read(81,recieveUID,8);
1918june 3:987e54d48f77 552 //pc.printf("%s\n",recieveNameJune);
1918june 3:987e54d48f77 553 //pc.printf("%s\n",recieveSurJune);
1918june 3:987e54d48f77 554 //pc.printf("%s\n",recieveIdJune);
1918june 3:987e54d48f77 555 //pc.printf("%s\n",recieveNameO);
1918june 3:987e54d48f77 556 //pc.printf("%s\n",recieveSurO);
1918june 3:987e54d48f77 557 //pc.printf("%s\n",recieveIdO);
1918june 3:987e54d48f77 558 //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 559 RFID.PCD_Init();
1918june 0:930d2bc16788 560 lcd.LCD_init();
1918june 0:930d2bc16788 561 lcd.backlight(ON);
1918june 2:38376c7c1685 562 wait(2);
1918june 0:930d2bc16788 563 lcd.LCD_clear();
1918june 10:7475e737bc0f 564 // servodown();
1918june 3:987e54d48f77 565 while(1) {
1918june 4:b56a8a051fdd 566 lcd.LCD_write_string(0,0,"SCAN ID CARD",0);
1918june 0:930d2bc16788 567 if ( ! RFID.PICC_IsNewCardPresent()) // Look for new cards
1918june 0:930d2bc16788 568 continue;
1918june 0:930d2bc16788 569 if ( ! RFID.PICC_ReadCardSerial()) // Select one of the cards
1918june 4:b56a8a051fdd 570
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 3:987e54d48f77 580 string name,sur;
1918june 3:987e54d48f77 581 eeprom.read(1,&name,11);
1918june 3:987e54d48f77 582 eeprom.read(12,&sur,10);
1918june 3:987e54d48f77 583 printstring(0,0,name,11,0);
1918june 3:987e54d48f77 584 printstring(0,1,sur,10,0);
1918june 9:b83b9198988d 585 user=1;
1918june 3:987e54d48f77 586 //lcd.LCD_clear();
1918june 3:987e54d48f77 587 }
1918june 3:987e54d48f77 588 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 589 pc.printf("%s\t%s\t%s\n",recieveNameO,recieveSurO,recieveIdO);
1918june 2:38376c7c1685 590
1918june 5:4af4865f462b 591
1918june 3:987e54d48f77 592 string name,sur;
1918june 4:b56a8a051fdd 593 lcd.LCD_clear();
1918june 3:987e54d48f77 594 eeprom.read(41,&name,7);
1918june 3:987e54d48f77 595 eeprom.read(48,&sur,3);
1918june 3:987e54d48f77 596 printstring(0,0,name,7,0);
1918june 3:987e54d48f77 597 printstring(0,1,sur,3,0);
1918june 9:b83b9198988d 598 user=2;
1918june 0:930d2bc16788 599 }
1918june 3:987e54d48f77 600 //wait(2);
1918june 3:987e54d48f77 601 //lcd.LCD_clear();
1918june 3:987e54d48f77 602 lcd.LCD_write_string(0,2,"1 select item",0);
1918june 3:987e54d48f77 603 lcd.LCD_write_string(0,3,"2 select place",0);
1918june 4:b56a8a051fdd 604 //lcd.LCD_write_string(40,5,"cancel",0);
1918june 3:987e54d48f77 605 while(1) {
1918june 3:987e54d48f77 606 if(mode==1) {
1918june 3:987e54d48f77 607 printf("1");
1918june 3:987e54d48f77 608 lcd.LCD_clear();
1918june 3:987e54d48f77 609 lcd.LCD_write_string(0,0,"mode 1",0);
1918june 3:987e54d48f77 610 lcd.LCD_write_string(0,1,"select item",0);
1918june 3:987e54d48f77 611 while(1) {
1918june 3:987e54d48f77 612 if(Switch1.read()==1) {
1918june 3:987e54d48f77 613 char str[10];
1918june 3:987e54d48f77 614 selectitem++;
1918june 3:987e54d48f77 615 if(selectitem > 9)
1918june 3:987e54d48f77 616 selectitem=1;
1918june 3:987e54d48f77 617 wait(0.2);
1918june 3:987e54d48f77 618 sprintf(str,"item : %d",selectitem);
1918june 3:987e54d48f77 619 printstring(3,2,str,8,0);
1918june 3:987e54d48f77 620 }
1918june 4:b56a8a051fdd 621 if(Switch2.read()==1 && selectitem !=0) {
1918june 4:b56a8a051fdd 622 mode=1;
1918june 3:987e54d48f77 623 char str[10];
1918june 3:987e54d48f77 624 place=((int) (Rnd.read()*1000))%4 +1;
1918june 3:987e54d48f77 625 wait(0.2);
1918june 3:987e54d48f77 626 sprintf(str,"place :%d",place);
1918june 3:987e54d48f77 627 printstring(3,3,str,8,0);
1918june 3:987e54d48f77 628 break;
1918june 3:987e54d48f77 629 }
1918june 3:987e54d48f77 630 }
1918june 5:4af4865f462b 631 wait(1);
1918june 9:b83b9198988d 632
1918june 5:4af4865f462b 633 while(1) { //
1918june 5:4af4865f462b 634 control();
1918june 11:d55f0cc16089 635 //read(data);
1918june 11:d55f0cc16089 636 //wait(0.5);
1918june 11:d55f0cc16089 637 //dataprint(data);
1918june 5:4af4865f462b 638 if(Switch2.read()==1) {
1918june 9:b83b9198988d 639 char str[12];
1918june 5:4af4865f462b 640 mode=1;
1918june 5:4af4865f462b 641 servodown();//servo down
1918june 5:4af4865f462b 642 read(data);
1918june 5:4af4865f462b 643 dataprint(data);
1918june 10:7475e737bc0f 644 // wait_ms(500);
1918june 5:4af4865f462b 645 servoup();//servo up
1918june 9:b83b9198988d 646 point = checkpoint();
1918june 9:b83b9198988d 647 sprintf(str,"point = %d",point);
1918june 9:b83b9198988d 648 lcd.LCD_clear();
1918june 9:b83b9198988d 649 lcd.LCD_write_string(0,1,str,0);
1918june 9:b83b9198988d 650 if(user==1)
1918june 9:b83b9198988d 651 eeprom.write(POINT_June,&point,1);
1918june 9:b83b9198988d 652 if(user==2)
1918june 9:b83b9198988d 653 eeprom.write(POINT_O,&point,1);
1918june 9:b83b9198988d 654 break;
1918june 5:4af4865f462b 655 }
1918june 3:987e54d48f77 656 }
1918june 9:b83b9198988d 657
1918june 3:987e54d48f77 658 }
1918june 3:987e54d48f77 659 if(mode==2) {//selectplace
1918june 3:987e54d48f77 660 printf("2");
1918june 3:987e54d48f77 661 lcd.LCD_clear();
1918june 3:987e54d48f77 662 lcd.LCD_write_string(0,0,"mode 2",0);
1918june 3:987e54d48f77 663 lcd.LCD_write_string(0,1,"select place",0);
1918june 3:987e54d48f77 664 while(1) {
1918june 3:987e54d48f77 665 if(Switch1.read()==1) {
1918june 4:b56a8a051fdd 666 mode=2;
1918june 3:987e54d48f77 667 char str[10];
1918june 3:987e54d48f77 668 place++;
1918june 3:987e54d48f77 669 if(place > 4)
1918june 3:987e54d48f77 670 place=1;
1918june 3:987e54d48f77 671 wait(0.2);
1918june 3:987e54d48f77 672 sprintf(str,"place : %d",place);
1918june 3:987e54d48f77 673 printstring(3,2,str,9,0);
1918june 3:987e54d48f77 674 }
1918june 4:b56a8a051fdd 675 if(Switch2.read()==1 && place!=0) {
1918june 3:987e54d48f77 676 char str[10];
1918june 3:987e54d48f77 677 selectitem=((int) (Rnd.read()*1000))%9 +1;
1918june 3:987e54d48f77 678 wait(0.2);
1918june 3:987e54d48f77 679 sprintf(str,"item : %d",selectitem);
1918june 3:987e54d48f77 680 printstring(3,3,str,8,0);
1918june 3:987e54d48f77 681 break;
1918june 3:987e54d48f77 682 }
1918june 4:b56a8a051fdd 683 }
1918june 5:4af4865f462b 684 wait(1);
1918june 11:d55f0cc16089 685 while(1) { //
1918june 11:d55f0cc16089 686 control();
1918june 11:d55f0cc16089 687 //read(data);
1918june 11:d55f0cc16089 688 //wait(0.5);
1918june 11:d55f0cc16089 689 // dataprint(data);
1918june 11:d55f0cc16089 690 if(Switch2.read()==1) {
1918june 11:d55f0cc16089 691 char str[12];
1918june 11:d55f0cc16089 692 mode=2;
1918june 11:d55f0cc16089 693 servodown();//servo down
1918june 11:d55f0cc16089 694 read(data);
1918june 11:d55f0cc16089 695 dataprint(data);
1918june 11:d55f0cc16089 696 // wait_ms(500);
1918june 11:d55f0cc16089 697 servoup();//servo up
1918june 11:d55f0cc16089 698 point = checkpoint();
1918june 11:d55f0cc16089 699 sprintf(str,"point = %d",point);
1918june 11:d55f0cc16089 700 lcd.LCD_clear();
1918june 11:d55f0cc16089 701 lcd.LCD_write_string(0,1,str,0);
1918june 11:d55f0cc16089 702 if(user==1)
1918june 11:d55f0cc16089 703 eeprom.write(POINT_June,&point,1);
1918june 11:d55f0cc16089 704 if(user==2)
1918june 11:d55f0cc16089 705 eeprom.write(POINT_O,&point,1);
1918june 11:d55f0cc16089 706 break;
1918june 11:d55f0cc16089 707 }
1918june 11:d55f0cc16089 708 }
1918june 4:b56a8a051fdd 709 }
1918june 2:38376c7c1685 710
1918june 10:7475e737bc0f 711 // printf("mode=%d\n",mode);
1918june 3:987e54d48f77 712
1918june 4:b56a8a051fdd 713 } //end while mode loop
1918june 4:b56a8a051fdd 714 }
1918june 3:987e54d48f77 715
1918june 3:987e54d48f77 716 }
1918june 4:b56a8a051fdd 717