Kendali Arah dan Kemiringan Sudut Motor Stepper Driver TB6600 Arduino
Pada kesempatan kali ini saya akan menjelaskan mengenai bagaimana cara membuat sebuah alat yang dapat mengendalikan arah dan sudut kemiringan motor stepper dengan driver TB6600 dan Arduino Mega sebagai kontrolernya. sensor arah yang dipakai yaitu HMC5883L dan sudut kemiringan MPU6050. untuk lebih jelasnya berikut adalah koding dan komponennya.
b. Program Arduino IDE
#include <Wire.h> // i2C Conection Library
#include <LiquidCrystal_I2C.h> //i2C LCD Library
#include <SPI.h>
#include <Keypad.h>
#include <SoftwareSerial.h>
#include <HMC5883L.h>
#include <TinyGPS.h>
#define dirPin 4
#define stepPin 5
#define stepsPerRevolution 800 // sesuaikan dengan settingan SW1-SW3 pada modul motor driver
#define dirPin2 2
#define stepPin2 3
#define stepsPerRevolution2 800 // sesuaikan dengan settingan SW1-SW3 pada modul motor driver
HMC5883L compass;
SoftwareSerial mySerial(10, 11);
TinyGPS gps;
LiquidCrystal_I2C lcd(0x27, 16, 2);
void gpsdump(TinyGPS &gps);
void printFloat(double f, int digits = 2);
int sudut;
float sudutku;
int nilaisudut = 0;
int nilaimiring = 0;
int nilaisudutfix;
int mark = 0;
int tanda;
int manual;
int motor;
char customKey;
const byte ROWS = 4;
const byte COLS = 4;
char keys[ROWS][COLS] = {
{'1', '2', '3', 'A'},
{'4', '5', '6', 'B'},
{'7', '8', '9', 'C'},
{'*', '0', '#', 'D'}
};
byte rowPins[ROWS] = {A0,A1,A2,A3}; //connect to the row pinouts of the keypad
byte colPins[COLS] = {A4,A5,A6,A7}; //connect to the column pinouts of the keypad
//initialize an instance of class NewKeypad
Keypad customKeypad = Keypad( makeKeymap(keys), rowPins, colPins, ROWS, COLS);
const int MPU_addr=0x68; // I2C address of the MPU-6050
int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ;
void setup()
{
Wire.begin();
Wire.beginTransmission(MPU_addr);
Wire.write(0x6B); // PWR_MGMT_1 register
Wire.write(0); // set to zero (wakes up the MPU-6050)
Wire.endTransmission(true);
delay(10);
lcd.begin();
lcd.clear();
lcd.noCursor();
pinMode(stepPin, OUTPUT);
pinMode(dirPin, OUTPUT);
pinMode(stepPin2, OUTPUT);
pinMode(dirPin2, OUTPUT);
// Oploen serial communications and wait for port to open:
Serial.begin(9600);
// set the data rate for the SoftwareSerial port
mySerial.begin(9600);
delay(1000);
Serial.println();
Serial.print("Sizeof(gpsobject) = ");
Serial.println(sizeof(TinyGPS));
Serial.println();
// 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);
}
void loop() // run over and over
{
customKey = customKeypad.getKey();
if(customKey >= '0' && customKey <= '9')
{
manual = manual * 10 + (customKey - '0');
lcd.setCursor(0,1);
lcd.print(manual);
}
lcd.setCursor(0,0);
lcd.print("PILIH MENU ");
if((customKey == '*')&&(manual == 1)){
for (int i = 0; i < 30; i++) {
digitalWrite(dirPin, LOW); // 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((customKey == '#')&&(manual == 1)){
for (int i = 0; i < 30; i++) {
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((customKey == '*')&&(manual == 2)){
for (int i = 0; i < 30; i++) {
digitalWrite(dirPin2, LOW); // putar CCW jarum jam
digitalWrite(stepPin2, HIGH);
delayMicroseconds(1000); // ganti delay untuk mempercepat motor
digitalWrite(stepPin2, LOW);
delayMicroseconds(1000); // ganti delay untuk mempercepat motor
}
}
if((customKey == '#')&&(manual == 2)){
for (int i = 0; i < 30; i++) {
digitalWrite(dirPin2, HIGH); // putar CCW jarum jam
digitalWrite(stepPin2, HIGH);
delayMicroseconds(1000); // ganti delay untuk mempercepat motor
digitalWrite(stepPin2, LOW);
delayMicroseconds(1000); // ganti delay untuk mempercepat motor
}
}
if(customKey == 'A'){
lcd.clear();
delay(1000);
tampilgps();
}
if(customKey == 'B'){
lcd.clear();
delay(1000);
carititiknol();
kompas();
}
if(customKey == 'C'){
lcd.clear();
delay(1000);
nilaimiring = 0;
accel();
}
if(customKey == 'D'){
lcd.clear();
delay(1000);
manual= 0;
motor = 0;
}
}
void accel(){
Wire.beginTransmission(MPU_addr);
Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H)
Wire.endTransmission(false);
Wire.requestFrom(MPU_addr,14,true); // request a total of 14 registers
AcX=Wire.read()<<8|Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
AcY=Wire.read()<<8|Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
Serial.print(AcX); Serial.println('a');
Serial.print(AcY); Serial.println('b');
sudutku = (AcX + 15013)/163.6;
lcd.setCursor(0,0);
lcd.print("SUDUT: ");
lcd.print(sudutku);
lcd.print(" ");
customKey = customKeypad.getKey();
if(customKey == 'D'){
lcd.clear();
delay(1000);
return;
}
if(customKey >= '0' && customKey <= '9')
{
nilaimiring = nilaimiring * 10 + (customKey - '0');
lcd.setCursor(0,1);
lcd.print("X: ");
lcd.print(nilaimiring);
lcd.print(" ");
}
if(customKey == '*'){
lcd.clear();
delay(200);
tanda = 1;
}
if((tanda == 1)&&(nilaimiring < 90)){
digitalWrite(dirPin2, LOW); // putar CCW jarum jam
digitalWrite(stepPin2, HIGH);
delayMicroseconds(1000); // ganti delay untuk mempercepat motor
digitalWrite(stepPin2, LOW);
delayMicroseconds(1000); // ganti delay untuk mempercepat motor
}
if((tanda == 1)&&(nilaimiring >= 90)){
digitalWrite(dirPin2, HIGH); // putar CW jarum jam
digitalWrite(stepPin2, HIGH);
delayMicroseconds(1000); // ganti delay untuk mempercepat motor
digitalWrite(stepPin2, LOW);
delayMicroseconds(1000); // ganti delay untuk mempercepat motor
}
if((tanda == 1)&&(sudutku >= nilaimiring - 5)&&(sudutku <= nilaimiring + 5)){
digitalWrite(dirPin2, LOW);
digitalWrite(stepPin2, LOW);
lcd.clear();
delay(1000);
tanda = 0;
return;
}
accel();
}
void tampilgps(){
customKey = customKeypad.getKey();
if(customKey == 'D'){
lcd.clear();
delay(1000);
return;
}
bool newdata = false;
unsigned long start = millis();
// Every 5 seconds we print an update
while (millis() - start < 5000)
{
if (mySerial.available())
{
char c = mySerial.read();
//Serial.print(c); // uncomment to see raw GPS data
if (gps.encode(c))
{
newdata = true;
break; // uncomment to print new data immediately!
}
}
}
if (newdata)
{
Serial.println("Acquired Data");
Serial.println("-------------");
gpsdump(gps);
Serial.println("-------------");
Serial.println();
}
tampilgps();
}
void kompas(){
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 == 'D'){
lcd.clear();
delay(1000);
return;
}
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, HIGH); // 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, LOW); // putar CCW jarum jam
digitalWrite(stepPin, HIGH);
delayMicroseconds(1000); // ganti delay untuk mempercepat motor
digitalWrite(stepPin, LOW);
delayMicroseconds(1000); // ganti delay untuk mempercepat motor
}
kompas();
}
void gpsdump(TinyGPS &gps)
{
long lat, lon;
float flat, flon;
unsigned long age, date, time, chars;
int year;
byte month, day, hour, minute, second, hundredths;
unsigned short sentences, failed;
gps.get_position(&lat, &lon, &age);
Serial.print("Lat/Long(10^-5 deg): "); Serial.print(lat); Serial.print(", "); Serial.print(lon);
Serial.print(" Fix age: "); Serial.print(age); Serial.println("ms.");
gps.f_get_position(&flat, &flon, &age);
Serial.print("Lat/Long(float): "); printFloat(flat, 5); Serial.print(", "); printFloat(flon, 5);
Serial.print(" Fix age: "); Serial.print(age); Serial.println("ms.");
lcd.setCursor(0,0);
lcd.print("LAT:");
lcd.print(flat, 5);
lcd.setCursor(0,1);
lcd.print("LONG:");
lcd.print(flon, 5);
}
void printFloat(double number, int digits)
{
// Handle negative numbers
if (number < 0.0)
{
Serial.print('-');
number = -number;
}
// Round correctly so that print(1.999, 2) prints as "2.00"
double rounding = 0.5;
for (uint8_t i=0; i<digits; ++i)
rounding /= 10.0;
number += rounding;
// Extract the integer part of the number and print it
unsigned long int_part = (unsigned long)number;
double remainder = number - (double)int_part;
Serial.print(int_part);
// Print the decimal point, but only if there are digits beyond
if (digits > 0)
Serial.print(".");
// Extract digits from the remainder one at a time
while (digits-- > 0)
{
remainder *= 10.0;
int toPrint = int(remainder);
Serial.print(toPrint);
remainder -= toPrint;
}
}
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, HIGH); // 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, LOW); // 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 < 3)){
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 - 10)&&(headingDegrees < nilaisudutfix + 10)){
mark = 0;
digitalWrite(dirPin, LOW); // putar CCW jarum jam
digitalWrite(stepPin, LOW);
lcd.clear();
delay(1000);
return;
}
lcd.setCursor(0,1);
lcd.print("Cari Titik: ");
lcd.print(nilaisudutfix);
lcd.print(" ");
carititik();
}
c. VIDEO HASILNYA
No comments:
Post a Comment