File content as of revision 5:4af4865f462b:

#include "mbed.h"
#include "LCD4884.h"
#include "MFRC522.h"
#include "eeprom.h"
#include "string.h"
#include "HMC5883L.h"


// KL25Z Pin for MFRC522 reset
#define MF_RESET    D9


EEPROM  eeprom(I2C_SDA,I2C_SCL,0);
Serial  pc(SERIAL_TX,SERIAL_RX);
LCD4884 lcd;
MFRC522 RFID(D11, D12, D13, D10, MF_RESET);
InterruptIn Sw1(PC_5);
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;

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;
}

void ItrSw2()
{
    mode=2;
}

void ItrSw3()
{
    mode=0;
}
void printstring(int x,int y,string s,int n,int m)
{
    int k=0;
    lcd.LCD_set_XY(x,y);
    while(k<n) {
        lcd.LCD_write_char(s[k],m);
        k++;
    }
}

int main()
{
    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];
    UID[0]=106;
    UID[1]=224;
    UID[2]=37;
    UID[3]=231;
    UID[4]=106;
    UID[5]=76;
    UID[6]=39;
    UID[7]=231;

    eeprom.write(1,&NameJune,11);
    wait_us(1);
    eeprom.write(12,&SurJune,10);
    wait_us(1);
    eeprom.write(22,&IdJune,11);
    wait_us(1);
    eeprom.write(41,&NameO,7);
    wait_us(1);
    eeprom.write(48,&SurO,3);
    wait_us(1);
    eeprom.write(51,&IdO,11);
    wait_us(1);
    eeprom.write(81,UID,8);
    wait_us(1);
    eeprom.read(1,&recieveNameJune,11);
    eeprom.read(12,&recieveSurJune,10);
    eeprom.read(22,&recieveIdJune,11);
    eeprom.read(41,&recieveNameO,7);
    eeprom.read(48,&recieveSurO,3);
    eeprom.read(51,&recieveIdO,11);
    eeprom.read(81,recieveUID,8);
    //pc.printf("%s\n",recieveNameJune);
    //pc.printf("%s\n",recieveSurJune);
    //pc.printf("%s\n",recieveIdJune);
    //pc.printf("%s\n",recieveNameO);
    //pc.printf("%s\n",recieveSurO);
    //pc.printf("%s\n",recieveIdO);
    //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]);
    RFID.PCD_Init();
    lcd.LCD_init();
    lcd.backlight(ON);
    wait(2);
    lcd.LCD_clear();
    while(1) {
        lcd.LCD_write_string(0,0,"SCAN ID CARD",0);
        if ( ! RFID.PICC_IsNewCardPresent()) // Look for new cards
            continue;
        if ( ! RFID.PICC_ReadCardSerial())   // Select one of the cards

            continue;
        pc.printf("Card UID: ");
        for (uint8_t i = 0; i < RFID.uid.size; i++)
            pc.printf(" %d", RFID.uid.uidByte[i]);
        printf("\n\r");
        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] ) {
            //จำ
            pc.printf("%s\t%s\t%s\n",recieveNameJune,recieveSurJune,recieveIdJune);
            lcd.LCD_clear();
            string name,sur;
            eeprom.read(1,&name,11);
            eeprom.read(12,&sur,10);
            printstring(0,0,name,11,0);
            printstring(0,1,sur,10,0);
            //lcd.LCD_clear();
        }
        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);
            eeprom.read(48,&sur,3);
            printstring(0,0,name,7,0);
            printstring(0,1,sur,3,0);
        }
        //wait(2);
        //lcd.LCD_clear();
        lcd.LCD_write_string(0,2,"1 select item",0);
        lcd.LCD_write_string(0,3,"2 select place",0);
        //lcd.LCD_write_string(40,5,"cancel",0);
        while(1) {
            if(mode==1) {
                printf("1");
                lcd.LCD_clear();
                lcd.LCD_write_string(0,0,"mode 1",0);
                lcd.LCD_write_string(0,1,"select item",0);
                while(1)    {
                    if(Switch1.read()==1) {
                        char str[10];
                        selectitem++;
                        if(selectitem > 9)
                            selectitem=1;
                        wait(0.2);
                        sprintf(str,"item : %d",selectitem);
                        printstring(3,2,str,8,0);
                    }
                    if(Switch2.read()==1 && selectitem !=0) {
                        mode=1;
                        char str[10];
                        place=((int) (Rnd.read()*1000))%4 +1;
                        wait(0.2);
                        sprintf(str,"place :%d",place);
                        printstring(3,3,str,8,0);
                        break;
                    }
                }
                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
                printf("2");
                lcd.LCD_clear();
                lcd.LCD_write_string(0,0,"mode 2",0);
                lcd.LCD_write_string(0,1,"select place",0);
                while(1)    {
                    if(Switch1.read()==1) {
                        mode=2;
                        char str[10];
                        place++;
                        if(place > 4)
                            place=1;
                        wait(0.2);
                        sprintf(str,"place : %d",place);
                        printstring(3,2,str,9,0);
                    }
                    if(Switch2.read()==1 && place!=0) {
                        char str[10];
                        selectitem=((int) (Rnd.read()*1000))%9 +1;
                        wait(0.2);
                        sprintf(str,"item : %d",selectitem);
                        printstring(3,3,str,8,0);
                        break;
                    }
                }
                wait(1);
 
            }

            printf("mode=%d\n",mode);

        }  //end while mode loop
    }

}