Costruzione pilota automatico per barca a vela

[pepilene]

Quando proverò questo ho deciso di portare dietro il passaporto, non si sa mai.

#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;
const int buttonPin = A0;
const int ledPin = 13;
int val=0;
int old_val=0;
int buttonState= 0;
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(1000);

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(ledPin, OUTPUT);//LED PILOTA ON /OFF
pinMode(buttonPin, INPUT);
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(){
// Se spento è possibile azionare l'attuatore in manuale
if ( (buttonState == 0)){
if (digitalRead(A2) == HIGH) {digitalWrite(rele6, LOW);} // Azionamento in manuale
if (digitalRead(A3) == HIGH) {digitalWrite(rele7, LOW);} // Azionamento in manuale
}
// Conservare lo stato del bottone ON OFF

val = digitalRead(buttonPin);
if ((val == HIGH) && (old_val == LOW)) {
buttonState = 1 - buttonState;
delay(10);
}
old_val = val;

if (buttonState == 1) {
// turn LED on:
digitalWrite(ledPin, HIGH);
}
else {
// turn LED off:
digitalWrite(ledPin, LOW);
}
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 roll = atan2(-y,x)/M_PI*180;
int bec = atan2(-z,y)/M_PI*180;
int angle = atan2(-z,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;}
if (bec > 360) {bec = bec - 360;}
if (bec < 0) {bec = bec + 360;}
if (roll > 360) {roll = roll - 360;}
if (roll < 0) {roll = roll + 360;}

//IL DISPLAY VISUALIZZA l'angolo e la rotta

lcd.clear();
lcd.setCursor(0,0);
if ( (buttonState == 1)) {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 ( (buttonState == 1))
{
digitalWrite(13, HIGH);
//PILOTA ADESSO AVVIATO FUNZIONANO I TASTI PER CAMBIARE ROTTA
if (digitalRead(A2) == HIGH) {rotta = rotta + 5;}
if (digitalRead(A3) == HIGH) {rotta = rotta - 5;}

// 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);}
}