Diff: project.cpp

Revision:

5:4af4865f462b

Parent:

4:b56a8a051fdd

Child:

6:4923616c7321

Child:

8:b12138d249ba

--- a/project.cpp	Tue Dec 08 16:11:25 2015 +0000
+++ b/project.cpp	Tue Dec 08 17:42:29 2015 +0000
@@ -3,6 +3,7 @@
 #include "MFRC522.h"
 #include "eeprom.h"
 #include "string.h"
+#include "HMC5883L.h"
 
 
 // KL25Z Pin for MFRC522 reset
@@ -14,14 +15,256 @@
 LCD4884 lcd;
 MFRC522 RFID(D11, D12, D13, D10, MF_RESET);
 InterruptIn Sw1(PC_5);
-AnalogIn Rnd(A0);
+AnalogIn Rnd(A2);
 DigitalIn Switch1(PC_5);
 DigitalIn Switch2(PC_6);
 InterruptIn Sw2(PC_6);
-
+HMC5883L compass1(D14,D15);
+HMC5883L compass2(D3,D6);
+HMC5883L compass3(D5,D7);
+DigitalOut servoPin(D4); //servo
+DigitalOut m1inA1(PB_13); //Motor 1
+DigitalOut m1inA2(PB_14);
+DigitalOut m1inB1(PB_15);
+DigitalOut m1inB2(PB_1);
+DigitalOut m2inA1(PB_5); //Motor 2
+DigitalOut m2inA2(PB_4);
+DigitalOut m2inB1(PB_10);
+DigitalOut m2inB2(PA_8);
+AnalogIn X(A0); // Joystick
+AnalogIn Y(A1);
+int16_t data[12] = {0};
 uint8_t mode=0;
-uint8_t selectitem=0,place=0;
+
+void m1step1()
+{
+    m1inA1 = 0;
+    m1inA2 = 1;
+    m1inB1 = 1;
+    m1inB2 = 0;
+    wait_ms(25);
+}
+void m1step2()
+{
+    m1inA1 = 0;
+    m1inA2 = 1;
+    m1inB1 = 0;
+    m1inB2 = 1;
+    wait_ms(25);
+}
+void m1step3()
+{
+    m1inA1 = 1;
+    m1inA2 = 0;
+    m1inB1 = 0;
+    m1inB2 = 1;
+    wait_ms(25);
+}
+void m1step4()
+{
+    m1inA1 = 1;
+    m1inA2 = 0;
+    m1inB1 = 1;
+    m1inB2 = 0;
+    wait_ms(25);
+}
+void m1stopMotor()
+{
+    m1inA1 = 0;
+    m1inA2 = 0;
+    m1inB1 = 0;
+    m1inB2 = 0;
+}
+
+void m2step1()
+{
+    m2inA1 = 0;
+    m2inA2 = 1;
+    m2inB1 = 1;
+    m2inB2 = 0;
+    wait_ms(50);
 
+}
+void m2step2()
+{
+    m2inA1 = 0;
+    m2inA2 = 1;
+    m2inB1 = 0;
+    m2inB2 = 1;
+    wait_ms(50);
+}
+void m2step3()
+{
+    m2inA1 = 1;
+    m2inA2 = 0;
+    m2inB1 = 0;
+    m2inB2 = 1;
+    wait_ms(50);
+}
+void m2step4()
+{
+    m2inA1 = 1;
+    m2inA2 = 0;
+    m2inB1 = 1;
+    m2inB2 = 0;
+    wait_ms(50);
+}
+void m2stopMotor()
+{
+    m2inA1 = 0;
+    m2inA2 = 0;
+    m2inB1 = 0;
+    m2inB2 = 0;
+}
+//PUT
+void Down()
+{
+    m1step1();
+    m1step2();
+    m1step3();
+    m1step4();
+}
+void Up()
+{
+    m1step3();
+    m1step2();
+    m1step1();
+    m1step4();
+}
+void Left()
+{
+    m2step1();
+    m2step2();
+    m2step3();
+    m2step4();
+}
+void Right()
+{
+    m2step3();
+    m2step2();
+    m2step1();
+    m2step4();
+}
+void DownRight()
+{
+    m1step1();
+    m1step2();
+    m1step3();
+    m1step4();
+    m2step3();
+    m2step2();
+    m2step1();
+    m2step4();
+}
+void DownLeft()
+{
+    m1step1();
+    m1step2();
+    m1step3();
+    m1step4();
+    m2step1();
+    m2step2();
+    m2step3();
+    m2step4();
+}
+void UpRight()
+{
+    m1step3();
+    m1step2();
+    m1step1();
+    m1step4();
+    m2step3();
+    m2step2();
+    m2step1();
+    m2step4();
+}
+void UpLeft()
+{
+    m1step3();
+    m1step2();
+    m1step1();
+    m1step4();
+    m2step1();
+    m2step2();
+    m2step3();
+    m2step4();
+}
+void calibrate()
+{
+
+}
+float x,y;
+void control()
+{
+
+    x = X.read()*360;
+    y = Y.read()*360;
+    printf("X = %f.2\tY = %f.2\n",x,y);
+    if(x > 200) {
+        Up();
+    }
+    if(x < 160) {
+        Down();
+    }
+    if(y > 200) {
+        Right();
+    }
+    if(y < 160) {
+        Left();
+    }
+
+}
+void servodown()
+{
+    for(int i=0; i<=6; i++) {
+        servoPin = 1;
+        wait_us(500);
+        servoPin = 0;
+        wait(1);
+    }
+}
+void servoup()
+{
+    for(int i=0; i<=6; i++) {
+        servoPin = 1;
+        wait_us(2500);
+        servoPin = 0;
+        wait(1);
+    }
+}
+void read(int16_t data[9])
+{
+    int16_t a[3],b[3],c[3];
+
+    int w=0;
+
+    compass1.getXYZ(a);
+    compass2.getXYZ(b);
+    compass3.getXYZ(c);
+    for(int i=0; i<3; i++) {
+        data[w]=a[i]/10;
+        w++;
+    }
+
+    for(int i=0; i<3; i++) {
+        data[w]=b[i]/10;
+        w++;
+    }
+    for(int i=0; i<3; i++) {
+        data[w]=c[i]/10;
+        w++;
+    }
+
+}
+
+void dataprint(int16_t data[9])
+{
+    for(int c=0; c<9; c++) {
+        printf("%d ",data[c]);
+        // if((c+1)%3==0){printf("||");}
+    }
+    printf("\n");
+}
 void ItrSw1()
 {
     mode=1;
@@ -51,7 +294,10 @@
     pc.baud(9600);
     Sw1.rise(&ItrSw1);
     Sw2.rise(&ItrSw2);
-
+    Sw3.rise(&ItrSw3);
+    compass1.init();
+    compass2.init();
+    compass3.init();
     string NameJune="Natthanicha",recieveNameJune,SurJune="Jamroonpan",recieveSurJune,IdJune="57340500023",recieveIdJune; //June start @ 1-36
     string NameO="Sirawat",recieveNameO,SurO="Sok",recieveSurO,IdO="57340500071",recieveIdO;
     uint8_t UID[8],recieveUID[8];
@@ -122,7 +368,7 @@
         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] ) {
             pc.printf("%s\t%s\t%s\n",recieveNameO,recieveSurO,recieveIdO);
 
-            //จำ
+
             string name,sur;
             lcd.LCD_clear();
             eeprom.read(41,&name,7);
@@ -161,8 +407,17 @@
                         break;
                     }
                 }
-                while(1) { //อ่านค่าจากแผ่นนั่นว่ามันจิ้มตรงมั้ย
-
+                wait(1);
+                while(1) { //
+                    control();
+                    if(Switch2.read()==1) {
+                        mode=1;
+                        servodown();//servo down
+                        read(data);
+                        dataprint(data);
+                        wait_ms(500);
+                        servoup();//servo up
+                    }
                 }
             }
             if(mode==2) {//selectplace
@@ -190,13 +445,11 @@
                         break;
                     }
                 }
-                while(1) {}
+                wait(1);
+ 
             }
-            if(mode==3) {
-                printf("0");
 
-            }
-            printf("mode=%d",mode);
+            printf("mode=%d\n",mode);
 
         }  //end while mode loop
     }