Lab 9 การใช้งานเข็มทิศดิจิตอล  (ใช้ร่วมกับโมดูล HMC5883L)

#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);

  */

}

#include <LotusNanoBot.h>

void setup() {

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

}

void loop() {

//Serial.println(cp());

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

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

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

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

}

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

}}