Lab 9 การใช้งานเข็มทิศDigital I2C HMC5883L Lotus Nano Bot

1. การติดตั้งโมดูลเข็มทิศ hmc5883l เข้ากับบอร์ด

2.ทำการติดตั้ง Library ให้เรียบร้อย โดย Download Library for compass hmc5883 module : ที่นี่

พร้อมทั้ง copy code ข้างล่างไปใช้งานครับ

Code Arduino IDE

#include <Wire.h>

#include <MechaQMC5883.h>

#include <Wire.h>

#include <SPI.h>

#include <Adafruit_GFX.h>

#include <Adafruit_SSD1306.h>

Adafruit_SSD1306 OLED(-1);

MechaQMC5883 qmc;

void setup() {

OLED.begin(SSD1306_SWITCHCAPVCC,0x3C); // กำหนดแอดเดรสของพอร์ตจอเป็น 0x3C (for the 128x64)

Wire.begin();

Serial.begin(9600);

qmc.init(); //qmc.setMode(Mode_Continuous,ODR_200Hz,RNG_2G,OSR_256);

}

void loop() {

int x, y, z;

int azimuth;

//float azimuth; //is supporting float too

qmc.read(&x, &y, &z,&azimuth);

//azimuth = qmc.azimuth(&y,&x);//you can get custom azimuth

//////////////////////แสดงคำสั่งแต่ละแกนที่จอ OLED //////////////////

OLED.clearDisplay();

OLED.setTextColor(WHITE,BLACK); //สีอักษรเป็นสีขาว ,พื้นหลังดำ

OLED.setCursor(0, 0); // เซตตำแหน่ง 0,0

OLED.setTextSize(2); // เซตขนาดอักษรมีขนาดเป็น 2

OLED.print("x = ");

OLED.println(x);

OLED.print("y = ");

OLED.println(y);

OLED.print("z = ");

OLED.println(z);

OLED.print("a = ");

OLED.println(azimuth);

OLED.display();

/*//////////////////// โชว์คำสั่งแต่ละแกนที่ Serial monitor ///////////

Serial.print("x: ");

Serial.print(x);

Serial.print(" y: ");

Serial.print(y);

Serial.print(" z: ");

Serial.print(z);

Serial.print(" a: ");

Serial.print(azimuth);

Serial.println();

delay(100);

*/

}

วีดีโอประกอบการทำงาน

Code การอ่านค่าจากการใช้ Library LotusNanoBot.h

Code Library LotusNanoBot.h

#include <LotusNanoBot.h>

void setup() {

ok(); // รอจนกว่ากดปุ่มสวิตซ์ D2

}

void loop() {

//Serial.println(cp());

oleddel(); // ลบหน้าจอ oled

oledp(0,0,2," CP = ",cp()); // แสดงค่ามุมอาซิมุส

oledplay(); // คำสั่งแสดงผลออกจอ

delay(100); // แสดงผลทุกๆ 100 มิลลิวินาที

}

ตัวอย่างโค้ดการใช้งานการเขียนเมนู การเลี้ยวล็อคทิศ และการแทร็คทิศ

คำสั่งการใช้งานเบื้องต้น

1. cp_read(); // คำสั่งอ่านค่าเข็มทิศ

2 cpr(มุมอาซิมุสที่ต้องการหยุดหมุนขวา)

3.cpl(มุมอาซิมุสที่ต้องการหยุดหมุนซ้าย)

4.track_cp(มุมอาซิมุสที่ต้องการแทร็ค ,จำนวนนับที่ต้องการแทร็ค)

Code Library LotusNanoBot.h

#include <LotusNanoBot.h>

void setup() {

set();

}

void loop(){

int sw = in(2);

int menu = map(inA(7),0,1023,0,5); // เทียบอัตราส่วนของพอร์ต A7 จาก 0-1023 เพื่อทำเป็นเมนู 0-5

oleddel();

oledlnp(0,0,2,"menu = ",(menu));

oledplay();

if ((sw == 0)and(menu==0)) {beep();} // เลือกเมนู0 อ่านค่าเข็มทิศ

if ((sw == 0)and(menu==1)) {beep();} // เลือกเมนู1

if ((sw == 0)and(menu==2)) {beep();} // เลือกเมนู2

if ((sw == 0)and(menu==3)) {beep();} // เลือกเมนู3

if ((sw == 0)and(menu==4)) {beep();} // เลือกเมนู4

if ((sw == 0)and(menu==5)) {beep();} // เลือกเมนู5

delay(100);

}

///////////////// คำสั่งอ่านค่าเข็มทิศ /////////////

void cp_read() {

while(true){

oleddel(); // ลบหน้าจอ oled

oledp(0,0,2," CP = ",cp()); // แสดงค่ามุมอาซิมุส

oledplay(); // คำสั่งแสดงผลออกจอ

delay(100); // แสดงผลทุกๆ 100 มิลลิวินาที

if(in(2)==0){break;}

}

}

///////////////// คำสั่งเลี้ยวล็อคทิศ ////////////////////

void cpr(int x) {while(true){run(100,-100); //คำสั่งเลี้ยวขวาล็อคทิศ ตัวอย่างใช้งานเช่น cpr(มุมอาซิมุสที่ต้องการให้หยุดหมุนขวา);

if ((cp() > (x - 5)) and (cp() < (x + 5))) {run(0,0);tone(3, 900, 500);break;}}}

//////////////////////////////////////////////////

void cpl(int x) {while(true){run(-100,100); // คำสั่งเลี้ยวซ้ายล็อคทิศ ตัวอย่างใช้งานเช่น cpl(มุมอาซิมุสที่ต้องการให้หยุดหมุนซ้าย);

if ((cp() > (x - 5)) and (cp() < (x + 5))) {run(0,0);tone(3, 900, 500);break;}}}

///////////////// คำสั่งแทร็คทิศ ////////////////////

void track_cp(int x ,int y) // คำสั่งการแทร็กทิศการตัวอย่างใช้งานเช่น track_cp(มุมอาซิมุส ,จำนวนรอบการนับ);

{int j = 0 ;

while(true){

if (cp() > (x + 5)) {run(100,-80);delay(20);j++;}

if (cp() < (x - 5)) {run(-80,100);delay(20);j++;}

if (j > y){run(0,0);beep();break;}

else{run(120,120); j++;}

}}