Costruzione pilota automatico per barca a vela

[pepilene]

P/S Marco ho cambiato gli ingressi dei bottoni, dal digitale all'analogico.

// Sketch per pilota automatico che casino

#include <LiquidCrystal.h>

#include <Wire.h> //i2c library for the Digital Compass



LiquidCrystal lcd( 12, 11, 5, 4, 3, 2 );

const int hmc5883Address = 0x1E; //0011110b, I2C 7bit address for compass

const byte hmc5883ModeRegister = 0x02;

const byte hmcContinuousMode = 0x00;

const byte hmcDataOutputXMSBAddress = 0x03;

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

Wire.beginTransmission(hmc5883Address); //Begin communication with compass

Wire.write(hmc5883ModeRegister); //select the mode register

Wire.write(hmcContinuousMode); //continuous measurement mode

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()

{

int x,y,z;

Wire.beginTransmission(hmc5883Address);

Wire.write(hmcDataOutputXMSBAddress); //Select register 3, X MSB register

Wire.endTransmission();

Wire.requestFrom(hmc5883Address,6);

if(6<=Wire.available())

{

x = Wire.read()<<8; //X msb

x |= Wire.read(); //X lsb

z = Wire.read()<<8; //Z msb

z |= Wire.read(); //Z lsb

y = Wire.read()<<8; //Y msb

y |= Wire.read(); //Y lsb

}

int angle = atan2(-y,x)/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 * 100);digitalWrite(rele6, HIGH); delay(100);}

if (diff > 3){digitalWrite(rele7, LOW); delay(scostamento * 100);digitalWrite(rele7, HIGH); delay(100);}


}

else

{digitalWrite(rele6, HIGH); digitalWrite(rele7, HIGH);}


}