Alat Penunjuk Arah Compass dengan Fitur Stabilizer Arah

Alat Penunjuk Arah Compass dengan Fitur Stabilizer Arah

 

        Pada kesempatan kali saya akan menjelaskan mengenai bagaimana cara membuat sebuah alat yang bisa melakukan tracking titik menggunakan kompas sensor. jadi titik diinputkan menggunakan keypad lalu stepper akan mencari titik tersebut. setelah titik tersebut didapat maka sistem akan menjaga titik tersebut walaupun stepper digeser atau dirubah arahnya. untuk lebih jelasnya berikut adalah skema dan kodingnya.

 

 

 

a. Skema Alat

b. Program Arduino IDE

#include <Wire.h>
#include <Keypad.h>
#include <HMC5883L.h>
#include <LiquidCrystal.h>

#define dirPin 13
#define stepPin 12
#define stepsPerRevolution 800 // sesuaikan dengan settingan SW1-SW3 pada modul motor driver

LiquidCrystal lcd(2, 3, 4, 5, 6, 7);

HMC5883L compass;

int nilaisudut = 0;
int nilaisudutfix;
int mark = 0;
char customKey;
const byte ROWS = 4;
const byte COLS = 4;

char keys[ROWS][COLS] = {
{'/', 'C', '-', '+'},
{'=', '9', '6', '3'},
{'0', '8', '5', '2'},
{'*', '7', '4', '1'}
};

byte rowPins[ROWS] = {8,9,10,11}; //connect to the row pinouts of the keypad
byte colPins[COLS] = {A0,A1,A2,A3}; //connect to the column pinouts of the keypad

//initialize an instance of class NewKeypad
Keypad customKeypad = Keypad( makeKeymap(keys), rowPins, colPins, ROWS, COLS);


void setup(){
 
  Wire.begin();
  lcd.begin(16, 2);
  lcd.clear();
  lcd.noCursor();
  Serial.begin(9600);
  pinMode(stepPin, OUTPUT);
  pinMode(dirPin, OUTPUT);

  // Initialize Initialize HMC5883L
  Serial.println("Initialize HMC5883L");
  while (!compass.begin())
  {
    Serial.println("Could not find a valid HMC5883L sensor, check wiring!");
    delay(500);
  }
 

  // Set measurement range
  compass.setRange(HMC5883L_RANGE_1_3GA);

  // Set measurement mode
  compass.setMeasurementMode(HMC5883L_CONTINOUS);

  // Set data rate
  compass.setDataRate(HMC5883L_DATARATE_30HZ);

  // Set number of samples averaged
  compass.setSamples(HMC5883L_SAMPLES_8);

  // Set calibration offset. See HMC5883L_calibration.ino
  compass.setOffset(0, 0);

carititiknol();

}


void loop(){

customKey = customKeypad.getKey();

  if(customKey >= '0' && customKey <= '9')
  {
      nilaisudut = nilaisudut * 10 + (customKey - '0');
      lcd.setCursor(0,1);
      lcd.print("S:");
      lcd.print(nilaisudut);
      lcd.print("     ");
  }


  if(customKey == '*')
  {
   nilaisudutfix = nilaisudut;
   nilaisudut = 0;
   lcd.clear();
   mark = 1;
   delay(200);
  }

if(nilaisudutfix > 350){
nilaisudutfix = 350;
}

if(mark == 1){
delay(1000);
carititiknol();
carititik();
}

lcd.setCursor(0,1);
lcd.print("S:");
lcd.setCursor(8,1);
lcd.print("M:");
lcd.print(nilaisudutfix);
lcd.print("   ");

//============================================================================  
  Vector norm = compass.readNormalize();

  // Calculate heading
  float heading = atan2(norm.YAxis, norm.XAxis);

  float declinationAngle = (4.0 + (26.0 / 60.0)) / (180 / M_PI);
  heading += declinationAngle;

  // Correct for heading < 0deg and heading > 360deg
  if (heading < 0)
  {
    heading += 2 * PI;
  }

  if (heading > 2 * PI)
  {
    heading -= 2 * PI;
  }

  // Convert to degrees
  float headingDegrees = heading * 180/M_PI;

  lcd.setCursor(0,0);
  lcd.print("Sudut= ");
  lcd.print(headingDegrees);
  lcd.print("    ");


if((nilaisudutfix > 0)&&(headingDegrees < nilaisudutfix - 10)){
      digitalWrite(dirPin, LOW); // putar CW jarum jam
      digitalWrite(stepPin, HIGH);
      delayMicroseconds(1000); // ganti delay untuk mempercepat motor
      digitalWrite(stepPin, LOW);
      delayMicroseconds(1000); // ganti delay untuk mempercepat motor
}

if((nilaisudutfix > 0)&&(headingDegrees > nilaisudutfix + 10)){
      digitalWrite(dirPin, HIGH); // putar CCW jarum jam
      digitalWrite(stepPin, HIGH);
      delayMicroseconds(1000); // ganti delay untuk mempercepat motor
      digitalWrite(stepPin, LOW);
      delayMicroseconds(1000); // ganti delay untuk mempercepat motor
}

 
}


void carititiknol(){
 
//============================================================================  
  Vector norm = compass.readNormalize();

  // Calculate heading
  float heading = atan2(norm.YAxis, norm.XAxis);

  float declinationAngle = (4.0 + (26.0 / 60.0)) / (180 / M_PI);
  heading += declinationAngle;

  // Correct for heading < 0deg and heading > 360deg
  if (heading < 0)
  {
    heading += 2 * PI;
  }

  if (heading > 2 * PI)
  {
    heading -= 2 * PI;
  }

  // Convert to degrees
  float headingDegrees = heading * 180/M_PI;

  lcd.setCursor(0,0);
  lcd.print("Sudut= ");
  lcd.print(headingDegrees);
  lcd.print("    ");

if((headingDegrees >= 150)&&(headingDegrees <= 360)){  
  digitalWrite(dirPin, LOW); // putar CW jarum jam
  digitalWrite(stepPin, HIGH);
  delayMicroseconds(700); // ganti delay untuk mempercepat motor
  digitalWrite(stepPin, LOW);
  delayMicroseconds(700); // ganti delay untuk mempercepat motor
}

if((headingDegrees >= 1)&&(headingDegrees < 150)){
  digitalWrite(dirPin, HIGH); // putar CCW jarum jam
  digitalWrite(stepPin, HIGH);
  delayMicroseconds(700); // ganti delay untuk mempercepat motor
  digitalWrite(stepPin, LOW);
  delayMicroseconds(700); // ganti delay untuk mempercepat motor
}

if((headingDegrees >= 0)&&(headingDegrees < 1)){
mark = 0;
lcd.clear();
delay(1000);  
return;  
}  
 
carititiknol();
}



void carititik(){
 
//============================================================================  
  Vector norm = compass.readNormalize();

  // Calculate heading
  float heading = atan2(norm.YAxis, norm.XAxis);

  float declinationAngle = (4.0 + (26.0 / 60.0)) / (180 / M_PI);
  heading += declinationAngle;

  // Correct for heading < 0deg and heading > 360deg
  if (heading < 0)
  {
    heading += 2 * PI;
  }

  if (heading > 2 * PI)
  {
    heading -= 2 * PI;
  }

  // Convert to degrees
  float headingDegrees = heading * 180/M_PI;

  lcd.setCursor(0,0);
  lcd.print("Sudut= ");
  lcd.print(headingDegrees);
  lcd.print("    ");

if((nilaisudutfix >= 0)&&(nilaisudutfix <= 175)){  
  digitalWrite(dirPin, LOW); // putar CW jarum jam
  digitalWrite(stepPin, HIGH);
  delayMicroseconds(1000); // ganti delay untuk mempercepat motor
  digitalWrite(stepPin, LOW);
  delayMicroseconds(1000); // ganti delay untuk mempercepat motor
}

if((nilaisudutfix >= 175)&&(nilaisudutfix < 350)){
  digitalWrite(dirPin, HIGH); // putar CCW jarum jam
  digitalWrite(stepPin, HIGH);
  delayMicroseconds(1000); // ganti delay untuk mempercepat motor
  digitalWrite(stepPin, LOW);
  delayMicroseconds(1000); // ganti delay untuk mempercepat motor
}

if((headingDegrees >= nilaisudutfix - 5)&&(headingDegrees < nilaisudutfix + 5)){
mark = 0;   
lcd.clear();
delay(1000);  
return;  
}

lcd.setCursor(0,1);
lcd.print("Cari Titik: ");
lcd.print(nilaisudutfix);
lcd.print("   ");  
 
carititik();
}

c. VIDEO HASILNYA