Ciao,
Ho sviluppato il seguente codice che mi dovrebbe permettere la possibilità di far muovere una macchinina che deve seguire un angolo della bussola. Solo che non legge nulla dal bluetooth, cosa potrebbe esserci che non va?
#include <L298N.h>
#include <Wire.h>
#include <SoftwareSerial.h>
#include <Adafruit_HMC5883_U.h>
#define echoPin 31
#define trigPin 30
#define HMC5883L_ADDR 0x1E
SoftwareSerial bluetooth(26, 27); // RX, TX
Adafruit_HMC5883_Unified magnetometer;
const int IN1 = 2;
const int IN2 = 3;
const int IN3 = 4;
const int IN4 = 5;
L298N MotorRight(IN3, IN4);
L298N MotorLeft(IN1, IN2);
bool haveHMC5883L = false;
int angle = 0;
bool detectHMC5883L() {
Wire.beginTransmission(HMC5883L_ADDR);
Wire.write(10);
Wire.endTransmission();
Wire.requestFrom(HMC5883L_ADDR, 3);
if (3 == Wire.available()) {
char a = Wire.read();
char b = Wire.read();
char c = Wire.read();
if (a == 'H' && b == '4' && c == '3') {
return true;
}
}
return false;
}
void setup() {
pinMode(echoPin, INPUT);
pinMode(trigPin, OUTPUT);
magnetometer.begin();
if (!magnetometer.begin()) {
haveHMC5883L = false;
} else {
haveHMC5883L = true;
}
bluetooth.begin(9600);
MotorLeft.stop();
MotorRight.stop();
Serial.begin(9600);
}
void loop() {
if (bluetooth.available() > 0) {
angle = bluetooth.parseInt();
Serial.println(angle);
}
if (haveHMC5883L) {
sensors_event_t event;
magnetometer.getEvent(&event);
float heading = atan2(event.magnetic.y, event.magnetic.x);
float declinationAngle = 0.045; // magnetic declination angle in Rome, Italy
heading += declinationAngle;
if (heading < 0) {
heading += 2 * PI;
}
if (heading > 2 * PI) {
heading -= 2 * PI;
}
int angle_degrees = int(heading * 180 / M_PI);
int delta_angle = angle_degrees - angle;
if (delta_angle > 10) {
// Turn right
MotorLeft.forward();
MotorRight.backward();
} else if (delta_angle < -10) {
// Turn left
MotorLeft.backward();
MotorRight.forward();
} else {
// Go straight
MotorLeft.forward();
MotorRight.forward();
}
} else {
MotorLeft.forward();
MotorRight.forward();
}
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
int distance = pulseIn(echoPin, HIGH);
distance = distance / 58;
if ((distance < 25) && (distance != 0)) {
MotorRight.stop();
MotorLeft.forward();
delay(500);
}
}