Bueno a ver que te parece esta rutina
Esto es lo que tenía en la mente y no podía plasmarlo.
void DireccionRueda (int compValue) {
int velocidad = 255;
int diff = compValue - destino; // diff es tu error
if (diff > 180) {
diff = -360 + diff;
}
else if (diff < -180 ) {
diff = 360 + diff;
}
diff = map(diff, -180, 180, -speedy, speedy);
if (diff > 0) {
speedLeft = velocidad - diff;
speedRight = velocidad;
}
else {
speedLeft = velocidad;
speedRight = velocidad + diff;
}
//Serial.println(speedLeft);
//Serial.println(speedRight);
}
Sigo pensando en como integrar mas cosas.
si usas la librería #include <Adafruit_Sensor.h>
El compas podemos leerlo de este modo (sacado de la web Adafruit)
float CompassSensor() {
sensors_event_t event;
mag.getEvent(&event);
// Se calcula la cabezera con el nivel del magnetómetro, luego se corrije el signo según los ejes.
float heading = atan2(event.magnetic.y, event.magnetic.x);
// Una vez que tienes la cabecera, se debe agregar el "Angulo de Declinación" que es el error del campo magnético terrestre según su locación.
// Encontrá el tuyo acá: http://www.magnetic-declination.com/
float declinationAngle = 0.2045;
heading += declinationAngle;
if (heading < 0) {
heading += 2 * PI;
}
if (heading > 2 * PI) {
heading -= 2 * PI;
}
float headingDegrees = (heading * 180) / M_PI;
//Serial.println(headingDegrees);
return headingDegrees;
}