how to display compass reading result in nodemcu8266
Posted: Sun Sep 05, 2021 6:52 amhi, let me ask.
I have a project to simulate the heading of a compass and the movement of two dc motors. when I operate it on arduino nano/uno, no problem. but when I operate it on nodemcu, somehow the compass readings don't appear on the serial monitor. does anyone know about SDA or SCL on nodemcu?
here's the code I use
I have a project to simulate the heading of a compass and the movement of two dc motors. when I operate it on arduino nano/uno, no problem. but when I operate it on nodemcu, somehow the compass readings don't appear on the serial monitor. does anyone know about SDA or SCL on nodemcu?
here's the code I use
Code: Select all
#include <TinyGPS++.h>
#include <SoftwareSerial.h>
#include <Wire.h>
#include <QMC5883LCompass.h>
//#include <ESP8266WiFi.h>
#define RX_PIN 2
#define TX_PIN 3
#define LED_PIN 13
#define GPS_BAUD 9600
#define WAYPOINT_THRESHHOLD 3
int LEFT_MOTOR[3] = {8, 4, 5};
int RIGHT_MOTOR[3] = {9, 6, 7};
TinyGPSPlus gps;
//const char* ssid = "wifi kami"; //ssid of your wifi
//const char* password = "cipicung"; //password of your wifi
//float latitude , longitude;
//int hour , minute , second;
//String time_str , lat_str , lng_str;
//int pm;
//WiFiServer server(80);
int wp = 0;
bool selesai = false;
QMC5883LCompass compass;
SoftwareSerial ss(RX_PIN, TX_PIN);
int calcLeftMotor(double relBearing) {
if (relBearing > 0)
return 255;
else
return 1.425 * relBearing + 255;
}
int calcRightMotor(double relBearing) {
if (relBearing <= 0)
return 255;
else
return -1.425 * relBearing + 255;
}
//KALAU NGASIH RAW DATA MAGNETIK (if give raw magnetic data)
double calcHeading (int x, int y) {
return -atan2(x, y) * 180 / 3.141592654;
}
void setup() {
Serial.begin(9600);
ss.begin(GPS_BAUD);
pinMode(LED_PIN, OUTPUT); //LED
pinMode (LEFT_MOTOR, OUTPUT);
pinMode (RIGHT_MOTOR, OUTPUT);
// WiFi.begin("wifi kami", "cipicung"); //connecting to wifi
// while (WiFi.status() != WL_CONNECTED)// while wifi not connected
// Serial.println("");
// Serial.println("WiFi connected");
// server.begin();
// Serial.println("Server started");
// Serial.println(WiFi.localIP());
Serial.println("Menuju Waypoint");
//init compass
compass.init();
}
void loop() {
double targetLat[] = {-6.992833};
double targetLon[] = {107.642863};
int jmlWP = sizeof(targetLat)/4;
while (ss.available() > 0)
gps.encode(ss.read());
if(wp >= jmlWP)
selesai = true;
double dist = 0;
if (gps.location.isValid()) {
dist = distance(targetLat[wp], targetLon[wp], gps.location.lat(), gps.location.lng());
Serial.print("distance to target: ");
Serial.println(dist);
double bearing = 0; //target angle
compass.read(); // reads from compass
double heading = calcHeading(compass.getX(), compass.getY());
double relBearing = bearing - heading;
int leftSpeed = map(calcLeftMotor(relBearing),0,255,130,150);
int rightSpeed = map(calcRightMotor(relBearing),0,255,130,150);
if(selesai){
digitalWrite(LEFT_MOTOR[1], 0);
digitalWrite(LEFT_MOTOR[2], 0);
analogWrite(LEFT_MOTOR[0], 0);
// RIGHT_MOTOR
digitalWrite(RIGHT_MOTOR[1], 0);
digitalWrite(RIGHT_MOTOR[2], 0);
analogWrite(RIGHT_MOTOR[0], 0);
}else if (dist > WAYPOINT_THRESHHOLD) {
//LEFT_MOTOR
digitalWrite(LEFT_MOTOR[1], 1);
digitalWrite(LEFT_MOTOR[2], 0);
analogWrite(LEFT_MOTOR[0], leftSpeed);
// RIGHT_MOTOR
digitalWrite(RIGHT_MOTOR[1], 0);
digitalWrite(RIGHT_MOTOR[2], 1);
analogWrite(RIGHT_MOTOR[0], rightSpeed);
}else if(dist <= WAYPOINT_THRESHHOLD){
wp++;
}
// if (dist < WAYPOINT_THRESHHOLD)
// digitalWrite(LED_PIN, HIGH);
//
// if (dist > WAYPOINT_THRESHHOLD)
// digitalWrite(LED_PIN, LOW);
Serial.print(" Heading: "); Serial.println(heading);
Serial.print("Left: "); Serial.print(leftSpeed); Serial.print(" Right: "); Serial.println(rightSpeed);
Serial.println(" ");
delay (1000);
}
}
double distance(double lat1, double lon1, double lat2, double lon2)
{
// Conversion factor from degrees to radians (pi/180)
const double toRadian = 0.01745329251;
// First coordinate (Radians)
double lat1_r = lat1 * toRadian;
double lon1_r = lon1 * toRadian;
// Second coordinate (Radians)
double lat2_r = lat2 * toRadian;
double lon2_r = lon2 * toRadian;
// Delta coordinates
double deltaLat_r = (lat2 - lat1) * toRadian;
double deltaLon_r = (lon2 - lon1) * toRadian;
// Distance
double a = sin(deltaLat_r / 2) * sin(deltaLat_r / 2) + cos(lat1_r) * cos(lat2_r) * sin(deltaLon_r / 2) * sin(deltaLon_r / 2);
double c = 2 * atan2(sqrt(a), sqrt(1 - a));
double distance = 6371 * c * 1000;
return distance;
}