Costruzione pilota automatico per barca a vela

[pepilene]

Andomast sei sempre il benvenuto.
E per la pace di tutti ecco i nuovi segmenti con il giroscopio.


#include <Wire.h> //i2c library for the Digital Compass
const int MPU=0x68; // I2C address of the MPU-6050
int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ;
LiquidCrystal lcd( 12, 11, 5, 4, 3, 2 );
int rotta=0;
int diff=0;
int scostamento=0;
const int rele6 = 6; //Porte uscita relè
const int rele7 = 7;
void setup()
{
// set up the LCD's number of columns and rows:
lcd.begin(16, 2);
lcd.clear(); //make sure screen is clear.
lcd.print("pepilene Go");
delay(4000);
Wire.begin(); // Start the i2c communication
//Begin communication with compass
Wire.beginTransmission(MPU);
Wire.write(0x6B); // PWR_MGMT_1 register
Wire.write(0); // set to zero (wakes up the MPU-6050)
Wire.endTransmission();
pinMode(rele6, OUTPUT);//LED FUTURO RELAIS MOTORE A SINISTRA
pinMode(rele7, OUTPUT);//LED FUTURO RELAIS MOTORE A DRITTA
pinMode(13, OUTPUT);//LED PILOTA ON /OFF
pinMode(A0, INPUT);//INTERRUTTORE ON OFF PILOTA
pinMode(A1, INPUT);//PULSANTE MEMORIZZA ROTTA NUOVA DALL'ANGOLO ATTUALE
pinMode(A2, INPUT);//PULSANTE + DIECI GRADI A DRITTA
pinMode(A3, INPUT);//PULSANTE - DIECI GRADI A SINISTRA
digitalWrite (rele6, HIGH); //Spegne tutti i relè
digitalWrite (rele7, HIGH); // Attenzione HIGH il rele sta a riposo
// LOW il rele viene eccitato, praticamente al contrario che cassata
}

void loop()

{
Wire.beginTransmission(MPU);
Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H)
Wire.endTransmission();
Wire.requestFrom(MPU,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)
AcZ=Wire.read()<<8|Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
Tmp=Wire.read()<<8|Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
GyX=Wire.read()<<8|Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
GyY=Wire.read()<<8|Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
GyZ=Wire.read()<<8|Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)

int angle = atan2(AcY,AcX)/M_PI*180;

// serie di if che portano l'angolo compreso tra 0 e 360 gradi
if (angle < 0) {angle = angle +360;}
if (angle > 360) {angle = angle - 360;}

//IL DISPLAY VISUALIZZA l'angolo e la rotta
lcd.clear();
lcd.setCursor(0,0);
if (digitalRead(A0) == HIGH) {lcd.print("ON:");} else {lcd.print("OFF:");}
lcd.setCursor(4,0);
lcd.print("rotta:");
lcd.setCursor(0,1);
lcd.print("gradi:");
lcd.setCursor(11,0);
lcd.print(rotta);
lcd.setCursor(11,1);
lcd.print(angle);
delay(500);

//PRIMA VA IMPOSTATA LA ROTA PREMENDO IL BOTTONE 5
if (digitalRead(A1) == HIGH) {rotta = angle;}

//POI VA AVVIATO IL PILOTA CON L'INTERRUTTORE 1
if (digitalRead(A0) == HIGH)
{
digitalWrite(13, HIGH);
//PILOTA ADESSO AVVIATO FUNZIONANO I TASTI PER CAMBIARE ROTTA
if (digitalRead(A2) == HIGH) {rotta = rotta + 10;}
if (digitalRead(A3) == HIGH) {rotta = rotta - 10;}
// trova la differenza tra angolo e rotta, se minore di -180 aggiunge 360, se maggiore di 180 sottrae 360 in modo che la differenza sia sempre compresa tra 180 e -180
//in questo modo sono gestiti i casi limite; esempio 1: rotta = 1, angolo = 356 -> diff = -355 -> diff = 5; esempio 2: rotta = 357, angolo = 3 -> diff = 354 -> diff = -6
diff = rotta - angle;
if (diff < -180) {diff = diff + 360;}
if (diff > 180) {diff = diff - 360;}
scostamento = abs(diff);
//routine pilota lavora su diff, +3 e - 3 (mi sembrano troppi veramente 6 gradi di errore tollerabile....) servono ad attenuare
//la responsività del pilota a piccoli angoli di spostamento dalla rotta, mentre scostamento rende la durata dell'azionamento del
// servomotore del pilota proporzionale al grado di scostamento tra rotta e angolo operativi
if (diff < -3){digitalWrite(rele6, LOW); delay(scostamento * 50);digitalWrite(rele6, HIGH); delay(10);}
if (diff > 3){digitalWrite(rele7, LOW); delay(scostamento * 50);digitalWrite(rele7, HIGH); delay(10);}
}
else
{digitalWrite(rele6, HIGH); digitalWrite(rele7, HIGH);}


}