hmc5883l
Dependencies: HMC5883L LCD4884 MFRC522 eeprom mbed
project.cpp
- Committer:
- 1918june
- Date:
- 2015-12-08
- Revision:
- 7:7daef23ab362
- Parent:
- 6:4923616c7321
File content as of revision 7:7daef23ab362:
#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 #define POINT_June 100 #define POINT_O 101 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; uint8_t point=0; uint8_t selectitem=0,place=0; uint8_t user=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() { for(int i=0; i<=4; i++) { servoPin = 1; wait_us(700); servoPin = 0; wait(1); } read(data); dataprint(data);//อ่านค่าSensor************************* for(int i=0; i<=6; i++) { servoPin = 1; wait_us(2500); servoPin = 0; wait(1); } for (int i=0; i<=250; i++) { m1step1(); m1step2(); m1step3(); m1step4(); } m1stopMotor(); wait(1); for(int i=0; i<=4; i++) { servoPin = 1; wait_us(700); servoPin = 0; wait(1); } read(data); dataprint(data);//อ่านค่าSensor************************* for(int i=0; i<=6; i++) { servoPin = 1; wait_us(2500); servoPin = 0; wait(1); } for (int i=0; i<=300; i++) { m2step1(); m2step2(); m2step3(); m2step4(); } m2stopMotor(); wait(1); for(int i=0; i<=4; i++) { servoPin = 1; wait_us(700); servoPin = 0; wait(1); } read(data); dataprint(data);//อ่านค่าSensor************************* for(int i=0; i<=6; i++) { servoPin = 1; wait_us(2500); servoPin = 0; wait(1); } for (int i=0; i<=250; i++) { m1step3(); m1step2(); m1step1(); m1step4(); } m1stopMotor(); wait(1); for(int i=0; i<=4; i++) { servoPin = 1; wait_us(700); servoPin = 0; wait(1); } read(data); dataprint(data);//อ่านค่าSensor************************* for(int i=0; i<=6; i++) { servoPin = 1; wait_us(2500); servoPin = 0; wait(1); } for (int i=0; i<=300; i++) { m2step3(); m2step2(); m2step1(); m2step4(); } m2stopMotor(); wait(1); }*/ 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 checkpoint() { int P; //P=(sd89sd9))) วิธีการคำนวณคะแนน return P; } int main() { pc.baud(9600); Sw1.rise(&ItrSw1); Sw2.rise(&ItrSw2); 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] ) { //จำ printf("%s\t%s\t%s\n",recieveNameJune,recieveSurJune,recieveIdJune); lcd.LCD_clear(); string name,sur,point;//June eeprom.read(1,&name,11); eeprom.read(12,&sur,10); eeprom.read(POINT_June,&point,1); printstring(0,0,name,11,0); printstring(0,1,sur,10,0); printstring(0,2,point,1,0); user=1; } 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] ) { // printf("%s\t%s\t%s\n",recieveNameO,recieveSurO,recieveIdO); string name,sur,point; lcd.LCD_clear(); eeprom.read(41,&name,7); eeprom.read(48,&sur,3); eeprom.read(POINT_O,&point,1); printstring(0,0,name,7,0); printstring(0,1,sur,3,0); printstring(0,2,point,1,0); user=2; } printf("check"); wait(2); //lcd.LCD_clear(); lcd.LCD_write_string(0,4,"1 select item",0); lcd.LCD_write_string(0,5,"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) { char str[12]; mode=1; servodown();//servo down read(data); dataprint(data); wait_ms(500); servoup();//servo up point = checkpoint(); sprintf(str,"point = %d",point); lcd.LCD_clear(); lcd.LCD_write_string(0,1,str,0); if(user==1) eeprom.write(POINT_June,&point,1); if(user==2) eeprom.write(POINT_O,&point,1); break; } } } 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); while(1) { // control(); if(Switch2.read()==1) { char str[12]; mode=1; servodown();//servo down read(data); dataprint(data); wait_ms(500); servoup();//servo up point = checkpoint(); sprintf(str,"point = %d",point); lcd.LCD_clear(); lcd.LCD_write_string(0,1,str,0); if(user==1) eeprom.write(POINT_June,&point,1); if(user==2) eeprom.write(POINT_O,&point,1); break; } } printf("mode=%d\n",mode); } //end while mode loop } } }