/* autopilot www.blue-felix.de v1.0 beta (version without rudder feedback), 18.04.14 Dr. Sven Seren
hardware: arduino uno rev3; display 1602 LCD 2x16; compass sensor HMC5883L; relay 4x SRD; 2x poti 50 kOhm; 2x LED; buzzer; switcher;
wiring:
LCD RS to pin D12
LCD Enable to pin D11
LCD R/W to ground 
//LCD VO to pin D10 (LCD contrast voltage via PWM)              //LCD contrast voltage via resistors
LCD D4 to pin D9
LCD D5 to pin D8
LCD D6 to pin D7
LCD D7 to pin D6
compass module HMC5883L: SDA to pin A4, SCL to pin A5
relay: INT1 to pin A0, INT2 to pin A1
LEDs to pin D4 (red) and D5 (green)
buzzer to pin D13
switch (with pull up resistor) to pin A3
poti 1 for adjust heading target: middle to pin A6
poti 2 for steering sensitivity: middle to pin A7
*/

#include <LiquidCrystal.h>                    //LCD library
LiquidCrystal lcd(12, 11, 9, 8, 7, 6);        //define display pins: LiquidCrystal(rs, enable, d4, d5, d6, d7) 
//const int lcd_contrast = 10;                //LCD contrast voltage via PWM at pin 10

#include <Wire.h>                             //I2C library
#include <HMC5883L.h>                         //HMC5883L compass library
HMC5883L compass;                             //store compass as variable

//adjust software with following parameters:

const int heading_reliability = 1;            //factor to detect unreliable heading measurements (1 = 100%)
int heading_offset = 40;                      //magnetic compass correction (in grad)
int sensitivity_unit = 10;                    //trigger unit value (in degrees) for course correction, can be alterd by switcher
float sensitivity_factor = 1.5;               //factor for course correction (if switcher is enabled)
int steeringduration = 1000;                  //time unit (in ms) how long rudder is set (i.e. relays are enabled) 
const float steeringfactor = 1.0;             //factor for steeringduration for rudder backward movement (should normally be <= 1)
const int rudderpause = 1000;                 //time (in ms) rudder stays in steering position

//end of adjustable parameters

float sensitivity = 0;                        //trigger value (in degrees) for course correction (calculated)
float heading = 0;                            //heading
float heading_target = 0;                     //heading target
float heading_corrected = 0;                  //magnetic compass correction (in grad) corrected by potentiometer value
int steeringduration_corrected = 0;           //time unit how long relays are enabled adjusted by potentiometer value
int delta = 0;                                //difference between heading target and heading
int trigger = 0;                              //trigger value for course correction

int poti1_ini = 0;                            //poti values for heading correction
int poti1_delta = 0;
int poti2_ini = 0;                            //poti values for steering sensitivity
int poti2_delta = 0;
int switcherstate = 0;                        //switcher state (pull up rsistor)

const int relay_1 = A0;                       //relay INT1 at pin A0
const int relay_2 = A1;                       //relay INT2 at pin A1
const int LEDred = 4;                         //LED red, port
const int LEDgreen = 5;                       //LED green, starbord                      
const int buzzer = 13;                        //buzzer
const int switcher = A3;                      //switch

void setup() {
 //Serial.begin(9600);                        //initialize serial port (for debugging only)
 pinMode(lcd_contrast, OUTPUT);               //define LCD contrast voltage pin
 analogWrite(lcd_contrast, 100);              //generate contrast voltage: analogWrite(pin, value), value = 0 (always off) ... 255 (always on)
 lcd.begin(16, 2);                            //LCD has 2 lines a 16 chars
 Wire.begin();                                //start I2C interface
 compass = HMC5883L();                        //new instance of HMC5883L library (construct a new HMC5883 compass)
 setupHMC5883L();                             //setup the HMC5883L
   
 pinMode(LEDgreen, OUTPUT); digitalWrite(LEDgreen, LOW);   //set LED pins as output and switch LEDs off
 pinMode(LEDred, OUTPUT); digitalWrite(LEDred, LOW);
 pinMode(relay_1,OUTPUT); pinMode(relay_2,OUTPUT);         //set relay pins as output
 digitalWrite(relay_1,HIGH); digitalWrite(relay_2,HIGH);   //relay closes when input is pulled to ground
 pinMode(buzzer, OUTPUT); digitalWrite(buzzer, LOW);  //define buzzer pin 
 pinMode(switcher, INPUT);
 
 heading_target = getHeading();               //get heading value and store in heading_target (after reset)
 
 poti1_ini = analogRead(6);                   //poti value from pin A6
 //Serial.println("poti1_ini"); Serial.println(poti1_ini);   //value should go from 0 to 1023
 poti2_ini = analogRead(7);                   //poti value from pin A7
 //Serial.println("poti2_ini"); Serial.println(poti2_ini);   //value should go from 0 to 1023
 
 display_info();                               //show software info on LCD
} 

void loop() {                                 
 heading = getHeading();                                        //calculate heading in function getHeading and store in variable heading
 //Serial.println("heading"); Serial.println(heading); 
 lcd.setCursor(0, 1); lcd.print("                "); lcd.setCursor(0, 1); lcd.print("heading"); lcd.setCursor(13, 1); lcd.print(heading,0); 
 
 poti1_delta = (poti1_ini - analogRead(6))/30;                        //calculate poti1 (heading) delta and division for smaller value
 //Serial.println("poti1 delta"); Serial.println(poti1_delta); 
 poti2_delta = (analogRead(7) - poti2_ini)*2;                         //calculate poti2 (rudder) delta and multiplication for larger value
 //Serial.println("poti1 delta"); Serial.println(poti2_delta); 

 steeringduration_corrected = steeringduration - poti2_delta;         //duration how long rudder is set
 if(steeringduration_corrected <= 0) steeringduration_corrected = 0;  //check for values < 0
 //Serial.println("steeringduration_corrected"); Serial.println(steeringduration_corrected); 
 
 heading_corrected = heading_target - poti1_delta;
 if(heading_corrected < 0) heading_corrected += 360;                  //correct for when signs are reversed
 if(heading_corrected > 360) heading_corrected -= 360;
 if(heading_corrected == 0) heading_corrected = 360;                  //set value for north heading to zero
 //Serial.println("head.targt"); Serial.println(heading_corrected);
 lcd.setCursor(0, 0); lcd.print("                "); lcd.setCursor(0, 0); lcd.print("head.target"); lcd.setCursor(13, 0); lcd.print(heading_corrected,0); 
  
 digitalWrite(LEDgreen, LOW); digitalWrite(LEDred, LOW);              //swich off LEDs (defined state)
 digitalWrite(relay_1,HIGH); digitalWrite(relay_2,HIGH);              //switch relays off (defined state); relay opens when input is pulled to ground
 
 switcherstate = digitalRead (switcher);                              //check if switcher is enabled (i.e. state was pulled to LOW)
 if (switcherstate == LOW){sensitivity = sensitivity_unit * sensitivity_factor;}  //extend sensitivity value if switcher is enabled
 else {sensitivity = sensitivity_unit;}
 
 delta = heading_corrected - heading;
 if(delta > 180) {trigger = delta - 360;}
 if(delta < -180) {trigger = delta + 360;}
 if(delta >= -180 && delta <= 180) {trigger = delta;}
 
 if(trigger < 0 && abs(trigger) >= sensitivity) {          //heading has to be corrected to portside
  digitalWrite(LEDred, HIGH);
  digitalWrite(relay_1,LOW); digitalWrite(relay_2,HIGH);   //current flow (= set rudder) by enabling relay
  delay(steeringduration_corrected);                       //wait
  digitalWrite(relay_1,HIGH); digitalWrite(relay_2,HIGH);  //stop current flow
  delay(rudderpause);                                      //leave rudder in steering position for defined time
  digitalWrite(relay_1,HIGH); digitalWrite(relay_2,LOW);   //reverse current (= set rudder back)
  delay(steeringduration_corrected*steeringfactor);        //wait (time adjustable, if necessary)
  digitalWrite(relay_1,HIGH); digitalWrite(relay_2,HIGH);  //stop current flow
 }
 
 if(trigger > 0 && abs(trigger) >= sensitivity) {          //heading has to be corrected to starbord
  digitalWrite(LEDgreen, HIGH);
  digitalWrite(relay_1,HIGH); digitalWrite(relay_2,LOW);   //current flow (= set rudder) by enabling relay
  delay(steeringduration_corrected);                       //wait
  digitalWrite(relay_1,HIGH); digitalWrite(relay_2,HIGH);  //stop current flow
  delay(rudderpause);                                      //leave rudder in steering position for defined time
  digitalWrite(relay_1,LOW); digitalWrite(relay_2,HIGH);   //reverse current (= set rudder back)
  delay(steeringduration_corrected*steeringfactor);        //wait (time adjustable, if necessary)
  digitalWrite(relay_1,HIGH); digitalWrite(relay_2,HIGH);  //stop current flow
 }
 
 if(abs(trigger) > 180) {                                        //error!
  digitalWrite(LEDred, HIGH); digitalWrite(LEDgreen, HIGH);      //switch on both LEDs to indicate error
  digitalWrite(relay_1,HIGH); digitalWrite(relay_2,HIGH);        //stop current flow (defined state)
  delay(5000);                                                   //wait few seconds (for compass sensor to get better value?)
 }        
delay(500);                                                      //wait shortly
} 

void setupHMC5883L() {                                            //setup the HMC5883L, and check for errors
 int error;  
 error = compass.SetScale(1.3);                                   //set scale of compass
 //if(error != 0) {Serial.println(compass.GetErrorText(error));}  //check for errors
 error = compass.SetMeasurementMode(Measurement_Continuous);      //set measurement mode to Continuous
 //if(error != 0) {Serial.println(compass.GetErrorText(error));}  //check for errors
}

float getHeading() {                                              //get reading from HMC5883L and calculate heading
 start:                                                           //label for goto jump
 MagnetometerScaled scaled = compass.ReadScaledAxis();            //request scaled values from compass
 float heading1 = atan2(scaled.YAxis, scaled.XAxis); delay(500);  //calculate angle of rotation for Z axis from length of X, Y vectors
 scaled = compass.ReadScaledAxis();
 float heading2 = atan2(scaled.YAxis, scaled.XAxis); delay(500);
 scaled = compass.ReadScaledAxis();
 float heading3 = atan2(scaled.YAxis, scaled.XAxis);
 float heading_mean  = (heading1 + heading2 + heading3)/3;        //calculate arithmetic mean value of single measurements
 //Serial.println("head. 123"); Serial.print(heading1);Serial.print(" ");Serial.print(heading2);Serial.print(" ");Serial.println(heading3);
 //Serial.println("head_mean"); Serial.println(heading_mean);
 if (abs((heading_mean - heading1)/heading1) > heading_reliability) {digitalWrite(buzzer, HIGH); delay(100); digitalWrite(buzzer, LOW); goto start;}     //restart measurement if unreliable
 if (abs((heading_mean - heading2)/heading2) > heading_reliability) {digitalWrite(buzzer, HIGH); delay(100); digitalWrite(buzzer, LOW); goto start;}     //restart measurement if unreliable
 if (abs((heading_mean - heading3)/heading3) > heading_reliability) {digitalWrite(buzzer, HIGH); delay(100); digitalWrite(buzzer, LOW); goto start;}     //restart measurement if unreliable
 heading = heading_mean + (heading_offset * 0.0175);              //correct with magnetic offset (converted to radiants)
 if(heading < 0) heading += 2*PI;                                 //correct for when signs are reversed
 if(heading > 2*PI) heading -= 2*PI;
 heading = heading * RAD_TO_DEG;                                  //calculation to degrees
 heading = int (heading + 0.5);                                   //round to integer value for easier calculation
 if(heading == 0) heading = 360;                                  //set value for north heading to 360
 return heading;                                                  //return value to main program
}

void display_info () {
 lcd.setCursor(0, 0); lcd.print("                "); lcd.setCursor(0, 1); lcd.print("                ");   //clear display
 lcd.setCursor(0, 0); lcd.print("   blue felix   ");                    
 lcd.setCursor(0, 1); lcd.print(" autopilot v1.0 ");
}