Need to make a function complete its content no matter what without delay

Okej so this is my first "real project" an i´m pretty much a beginner to both programming and arduino so bare with me :slight_smile: ive compiled this of many different codes and tried to fill in the gaps with my own code :slight_smile:

what im trying to do is a kind of course holding autpilot, im shure the code is filled with errors and many people would have made things differently but this is my approach and it works!

The only thing im having trouble with is that im trying to replace the "delay" in the left/right turn functions (turnright is corrected, turnleft is untouched) at the bottom of the code with millis, mainly because i want to be able to adjust the "lockedHeading" while the arduino is executing a turn, but since this is going in a boat i need the motor to "turn, wait, turn back" when the boat is off course, but if i do this with delay i cant adjust my heading until im back in "kurs OK" (course OK) but if i use millis() the code stops the turn function as soon as i get to the "kurs OK" area, meaning it can stop while leaving relay 1 active or really at any time in the function!

Is there a way to tell the code to execute the whole function beforing returning to loop, even if the course is back to its specified range?

appreciate the help, thanks!'

EDIT: had to remove parts of the code due to to many characters.. important parts still at the bottom

void loop() {
  //----------------------------------------FishMode---------------------------------
  // read the state of the switch into a local variable:
  int reading = digitalRead(SW_pin);

  // check to see if you just pressed the button
  // (i.e. the input went from LOW to HIGH), and you've waited long enough
  // since the last press to ignore any noise:

  // If the switch changed, due to noise or pressing:
  if (reading != lastButtonState) {
    // reset the debouncing timer
    lastDebounceTime = millis();
  }

  if ((millis() - lastDebounceTime) > debounceDelay) {
    // whatever the reading is at, it's been there for longer than the debounce
    // delay, so take it as the actual current state:

    // if the button state has changed:
    if (reading != buttonState2) {
      buttonState2 = reading;

      // only toggle the LED if the new button state is HIGH
      if (buttonState2 == HIGH) {
        ledState = !ledState;
      }
    }
  }

  // set the LED:
  fishModeState, ledState;
  

 
  lastButtonState = reading;
  if (ledState == HIGH){
    Serial.println("FISK!");
    
    
if (millis() - lastUpdateTime3 > updateDelayFish) {
 lcd.setCursor (5,0);
  lcd.print("FISK!!");
lastUpdateTime3 = millis();
 if ((millis() - lastUpdateTime4) > updateDelayFish2) {
    lcd.clear();
    lastUpdateTime4 = millis();
    }
    }

 
    fishMode;
  }
  else {
//---------------------------------------kompass--------------------------------------------
    
  qmc.read(&x, &y, &z,&azimuth);
  //azimuth = qmc.azimuth(&y,&x);//you can get custom azimuth
   buttonState = digitalRead (buttonPin);
   Heading=(azimuth);
  //---------------------------------------------------Switch joystick---------------------------------------- 
  if ((millis() - lastUpdateTime5) > updateDelay) {
   if (analogRead(Y_pin)<=100){storedHeading = (storedHeading - 5);} 
   lastUpdateTime5 = millis();
   }
   if ((millis() - lastUpdateTime6) > updateDelay) {
    if (analogRead(Y_pin)>900){storedHeading = (storedHeading + 5);}
    lastUpdateTime6 = millis();
    }
if ((millis() - lastUpdateTime7) > updateDelay) {
    if ((analogRead(X_pin)<=100) && (Deviation<MaxDeviation)) {Deviation = (Deviation + 2);}
    lastUpdateTime7 = millis();
    }
   if ((millis() - lastUpdateTime8) > updateDelay){
    if ((analogRead(X_pin)>900) && (Deviation>MinDeviation)){Deviation = (Deviation - 2);}
    lastUpdateTime8 = millis();
    }
    
    
void CorrectHeading(){ // Svänga höger eller vä?
  
 if (Heading < storedHeading){
  Serial.println("Vanster");
   lcd.setCursor (0,1);
   lcd.print ("                ");
   lcd.setCursor (0,1);
    lcd.print("Correcting LEFT");
  TurnLeft();
  delay(20);
 }
 else{
   Serial.println("Hoger");
   lcd.setCursor (0,1);
   lcd.print ("                ");
   lcd.setCursor (0,1);
    lcd.print("Correcting RIGHT");
 TurnRight();
delay(20);

 }
}


void TurnRight(){ //Sväng höger
digitalWrite(relayPin1, HIGH);
if ((millis() - lastUpdateTime9) > turnTime) {
  digitalWrite(relayPin1, LOW);
    
} 



        if ((millis() - lastUpdateTime9) > waitTime + turnTime) {
            digitalWrite(relayPin2, HIGH);
        
        

                    if ((millis() - lastUpdateTime9) > turnBackTime + waitTime + turnTime) {
                          digitalWrite(relayPin2, LOW);
                          
                          
                    
 if ((millis() - lastUpdateTime9) > turnBackTime + (waitTime * 2) + turnTime) {
                          digitalWrite(relayPin2, LOW);
                          
                      lastUpdateTime9 = millis();     
                    }
               }
        }                     

}


void TurnLeft(){ //Sväng vänster
digitalWrite(relayPin2, HIGH);
delay(turnTime);
digitalWrite(relayPin2, LOW);
delay(waitTime);
digitalWrite(relayPin1, HIGH);
delay(turnBackTime);

digitalWrite(relayPin1, LOW);
delay(waitTime);
}

void fishMode(){

Removed the comment lines to fit entire code:

#include <Wire.h>
#include <MechaQMC5883.h>
#include <LCD.h> //LCD display
#include <LiquidCrystal_I2C.h> //LCD display

MechaQMC5883 qmc; //Kompass

const int buttonPin = 2; //Knapp för lagring av kurs
const int ledPin = 13;
const int relayPin1 = 12; //Relä högersväng
const int relayPin2 = 11; //Relä vänstersväng
const long turnTime = 1000; //Hur länge ratten ska svänga
const long waitTime = 1000; //Hur länge den väntar innan den svänger tillbaka
const long turnBackTime = 1000; //Hur länge den svänger tillbaka
const int MinDeviation = 10; //lägsta gränsen för avvikelse
const int MaxDeviation = 60; //högsta gränsen för avvikelse
 

int Heading; //Kurs
int storedHeading; //Lagrad kurs
int Deviation = 10; //avvikelse från Kurs innan rättning.
int buttonState; // knapp för lagring av kurs.
//-------------------------------------------Joystick integers-------------------------
int ledState = HIGH;         
int buttonState2;             
int lastButtonState = LOW;   
int fishModeState;
unsigned long lastDebounceTime = 0;  
unsigned long debounceDelay = 20;    
unsigned long updateDelay = 1000;
unsigned long updateDelayFish = 1000;
unsigned long updateDelayFish2 = 2000;
unsigned long lastUpdateTime = 0;
unsigned long lastUpdateTime2 = 0;
unsigned long lastUpdateTime3 = 0;
unsigned long lastUpdateTime4 = 0;
unsigned long lastUpdateTime5 = 0;
unsigned long lastUpdateTime6 = 0;
unsigned long lastUpdateTime7 = 0;
unsigned long lastUpdateTime8 = 0;
unsigned long lastUpdateTime9 = 0;
//---------------------------------------------joystick---------------------------------
// Arduino pin numbers
const int SW_pin = 3; 
const int X_pin = 0; 

const int Y_pin = 1; //---------------------------------------Kompass---------------------------------------------
int x, y, z;
  int azimuth;//float azimuth; 
//----------------------------------LCD------------------------------------------------------

#define I2C_ADDR    0x27 // 
#define BACKLIGHT_PIN     3
#define En_pin  2
#define Rw_pin  1
#define Rs_pin  0
#define D4_pin  4
#define D5_pin  5
#define D6_pin  6
#define D7_pin  7

LiquidCrystal_I2C  lcd(I2C_ADDR,En_pin,Rw_pin,Rs_pin,D4_pin,D5_pin,D6_pin,D7_pin);


//------------------------------------------------------setup-------------------------------------------------
void setup() {
   Wire.begin();
  Serial.begin(9600);
  qmc.init();
  //qmc.setMode(Mode_Continuous,ODR_200Hz,RNG_2G,OSR_256);
  //--------------------------------------joystick-----------------------
  pinMode(SW_pin, INPUT);
 digitalWrite(SW_pin, HIGH);
  digitalWrite(fishModeState, ledState);
//----------------------------------LCD------------------------
lcd.begin (16,2); 

 
// Switch on the backlight
lcd.setBacklightPin(BACKLIGHT_PIN,POSITIVE);
lcd.setBacklight(HIGH);
lcd.home (); // go home

//----------------------------------------------------------------

pinMode(ledPin, OUTPUT);
pinMode(buttonPin, INPUT);
pinMode(relayPin1, OUTPUT);
pinMode(relayPin2, OUTPUT);

 qmc.read(&x, &y, &z,&azimuth);

storedHeading = (azimuth);

}

void loop() {
  //----------------------------------------FishMode---------------------------------
 
  if (reading != lastButtonState) {
    // reset the debouncing timer
    lastDebounceTime = millis();
  }

  if ((millis() - lastDebounceTime) > debounceDelay) {
 

    
    if (reading != buttonState2) {
      buttonState2 = reading;

      
      if (buttonState2 == HIGH) {
        ledState = !ledState;
      }
    }
  }


  fishModeState, ledState;
  

 
  lastButtonState = reading;
  if (ledState == HIGH){
    Serial.println("FISK!");
    
    
if (millis() - lastUpdateTime3 > updateDelayFish) {
 lcd.setCursor (5,0);
  lcd.print("FISK!!");
lastUpdateTime3 = millis();
 if ((millis() - lastUpdateTime4) > updateDelayFish2) {
    lcd.clear();
    lastUpdateTime4 = millis();
    }
    }

 
    fishMode;
  }
  else {
//---------------------------------------kompass--------------------------------------------
    
  qmc.read(&x, &y, &z,&azimuth);
  //azimuth = qmc.azimuth(&y,&x);//you can get custom azimuth
   buttonState = digitalRead (buttonPin);
   Heading=(azimuth);
  //---------------------------------------------------Switch joystick---------------------------------------- 
  if ((millis() - lastUpdateTime5) > updateDelay) {
   if (analogRead(Y_pin)<=100){storedHeading = (storedHeading - 5);} 
   lastUpdateTime5 = millis();
   }
   if ((millis() - lastUpdateTime6) > updateDelay) {
    if (analogRead(Y_pin)>900){storedHeading = (storedHeading + 5);}
    lastUpdateTime6 = millis();
    }
if ((millis() - lastUpdateTime7) > updateDelay) {
    if ((analogRead(X_pin)<=100) && (Deviation<MaxDeviation)) {Deviation = (Deviation + 2);}
    lastUpdateTime7 = millis();
    }
   if ((millis() - lastUpdateTime8) > updateDelay){
    if ((analogRead(X_pin)>900) && (Deviation>MinDeviation)){Deviation = (Deviation - 2);}
    lastUpdateTime8 = millis();
    }
    
    

    //--------------------------------------------------Serial & lCD print------------------------------

if ((millis() - lastUpdateTime) > updateDelay) {
  
     Serial.print(" StoredHeading ");      
      Serial.println(storedHeading);      
      Serial.print(" Kurs ");      
      Serial.println(Heading);     
      Serial.print(" knapp ");
      Serial.println(digitalRead(buttonPin)); 
      Serial.print(" Deviation ");      
      Serial.println(Deviation);    
     lcd.setCursor (0,0);        // go to start of 2nd line
 lcd.print("                ");
 lcd.setCursor (0,0);
 lcd.print("Kurs");
 lcd.print(Heading);
lcd.setCursor (9,0);
 lcd.print("Lock");
 lcd.print(storedHeading);
 lcd.setCursor (0,8);
 lcd.print("            ");
 lcd.print("Dv");
 lcd.print(Deviation);
 lastUpdateTime = millis();
}

    if (buttonState == LOW) {
       
      storedHeading = Heading;
    
    } else{
      if (Heading > storedHeading - Deviation && Heading < storedHeading + Deviation){   
  digitalWrite(ledPin, HIGH);
 if ((millis() - lastUpdateTime2) > updateDelay) {

    Serial.println(" KURS OK ");
    lcd.setCursor (0,1);
    lcd.print("        ");
    lcd.setCursor (0,1);
    lcd.print("KURS OK");
    lastUpdateTime2 = millis();
    }
    delay(50);
   
    }
    else{
   digitalWrite(ledPin, LOW);
      CorrectHeading();
    
      
      
      
    

}
}

}


}

  



void CorrectHeading(){ 
  
 if (Heading < storedHeading){
  Serial.println("Vanster");
   lcd.setCursor (0,1);
   lcd.print ("                ");
   lcd.setCursor (0,1);
    lcd.print("Correcting LEFT");
  TurnLeft();
  delay(20);
 }
 else{
   Serial.println("Hoger");
   lcd.setCursor (0,1);
   lcd.print ("                ");
   lcd.setCursor (0,1);
    lcd.print("Correcting RIGHT");
 TurnRight();
delay(20);

 }
}


void TurnRight(){ 
digitalWrite(relayPin1, HIGH);
if ((millis() - lastUpdateTime9) > turnTime) {
  digitalWrite(relayPin1, LOW);
    
} 



        if ((millis() - lastUpdateTime9) > waitTime + turnTime) {
            digitalWrite(relayPin2, HIGH);
        
        

                    if ((millis() - lastUpdateTime9) > turnBackTime + waitTime + turnTime) {
                          digitalWrite(relayPin2, LOW);
                          
                          
                    
 if ((millis() - lastUpdateTime9) > turnBackTime + (waitTime * 2) + turnTime) {
                          digitalWrite(relayPin2, LOW);
                          
                      lastUpdateTime9 = millis();     
                    }
               }
        }                     

}


void TurnLeft(){ //Sväng vänster
digitalWrite(relayPin2, HIGH);
delay(turnTime);
digitalWrite(relayPin2, LOW);
delay(waitTime);
digitalWrite(relayPin1, HIGH);
delay(turnBackTime);

digitalWrite(relayPin1, LOW);
delay(waitTime);
}

void fishMode(){ 
  
  
  /*

if (digitalRead(SW_pin) == LOW){ 
delay(50); 
running = !running; 
fishModeState = running;
*/
}

[/code]

I removed some of the weird indentation (actually, I didn't - the IDE's auto format tool did, but I got rid of some of the unnecessary initialisations)

#include <Wire.h>
#include <MechaQMC5883.h>
#include <LCD.h> //LCD display
#include <LiquidCrystal_I2C.h> //LCD display

MechaQMC5883 qmc; //Kompass

const int buttonPin = 2; //Knapp för lagring av kurs
const int ledPin = 13;
const int relayPin1 = 12; //Relä högersväng
const int relayPin2 = 11; //Relä vänstersväng
const long turnTime = 1000; //Hur länge ratten ska svänga
const long waitTime = 1000; //Hur länge den väntar innan den svänger tillbaka
const long turnBackTime = 1000; //Hur länge den svänger tillbaka
const int MinDeviation = 10; //lägsta gränsen för avvikelse
const int MaxDeviation = 60; //högsta gränsen för avvikelse


int Heading; //Kurs
int storedHeading; //Lagrad kurs
int Deviation = 10; //avvikelse från Kurs innan rättning.
int buttonState; // knapp för lagring av kurs.
//-------------------------------------------Joystick integers-------------------------
int ledState = HIGH;
int buttonState2;
int lastButtonState = LOW;
int fishModeState;
unsigned long lastDebounceTime;
unsigned long debounceDelay = 20;
unsigned long updateDelay = 1000;
unsigned long updateDelayFish = 1000;
unsigned long updateDelayFish2 = 2000;
unsigned long lastUpdateTime;
unsigned long lastUpdateTime2;
unsigned long lastUpdateTime3;
unsigned long lastUpdateTime4;
unsigned long lastUpdateTime5;
unsigned long lastUpdateTime6;
unsigned long lastUpdateTime7;
unsigned long lastUpdateTime8;
unsigned long lastUpdateTime9;
//---------------------------------------------joystick---------------------------------
// Arduino pin numbers
const int SW_pin = 3;
const int X_pin = 0;

const int Y_pin = 1; //---------------------------------------Kompass---------------------------------------------
int x, y, z;
int azimuth;//float azimuth;
//----------------------------------LCD------------------------------------------------------

#define I2C_ADDR    0x27 //
#define BACKLIGHT_PIN     3
#define En_pin  2
#define Rw_pin  1
#define Rs_pin  0
#define D4_pin  4
#define D5_pin  5
#define D6_pin  6
#define D7_pin  7

LiquidCrystal_I2C  lcd(I2C_ADDR, En_pin, Rw_pin, Rs_pin, D4_pin, D5_pin, D6_pin, D7_pin);

//------------------------------------------------------setup-------------------------------------------------
void setup() {
  Wire.begin();
  Serial.begin(9600);
  qmc.init();
  //qmc.setMode(Mode_Continuous,ODR_200Hz,RNG_2G,OSR_256);
  //--------------------------------------joystick-----------------------
  pinMode(SW_pin, INPUT);
  digitalWrite(SW_pin, HIGH);
  digitalWrite(fishModeState, ledState);
  //----------------------------------LCD------------------------
  lcd.begin (16, 2);

  // Switch on the backlight
  lcd.setBacklightPin(BACKLIGHT_PIN, POSITIVE);
  lcd.setBacklight(HIGH);
  lcd.home (); // go home

  //----------------------------------------------------------------

  pinMode(ledPin, OUTPUT);
  pinMode(buttonPin, INPUT);
  pinMode(relayPin1, OUTPUT);
  pinMode(relayPin2, OUTPUT);

  qmc.read(&x, &y, &z, &azimuth);

  storedHeading = azimuth;
}

void loop() {
  //----------------------------------------FishMode---------------------------------

  if (reading != lastButtonState) {
    // reset the debouncing timer
    lastDebounceTime = millis();
  }

  if ((millis() - lastDebounceTime) > debounceDelay) {
    if (reading != buttonState2) {
      buttonState2 = reading;

      if (buttonState2 == HIGH) {
        ledState = !ledState;
      }
    }
  }

  fishModeState, ledState;

  lastButtonState = reading;
  if (ledState == HIGH) {
    Serial.println("FISK!");

    if (millis() - lastUpdateTime3 > updateDelayFish) {
      lcd.setCursor (5, 0);
      lcd.print("FISK!!");
      lastUpdateTime3 = millis();
      if ((millis() - lastUpdateTime4) > updateDelayFish2) {
        lcd.clear();
        lastUpdateTime4 = millis();
      }
    }

    fishMode;
  }
  else {
    //---------------------------------------kompass--------------------------------------------

    qmc.read(&x, &y, &z, &azimuth);
    //azimuth = qmc.azimuth(&y,&x);//you can get custom azimuth
    buttonState = digitalRead (buttonPin);
    Heading = (azimuth);
    //---------------------------------------------------Switch joystick----------------------------------------
    if ((millis() - lastUpdateTime5) > updateDelay) {
      if (analogRead(Y_pin) <= 100) {
        storedHeading = (storedHeading - 5);
      }
      lastUpdateTime5 = millis();
    }
    if ((millis() - lastUpdateTime6) > updateDelay) {
      if (analogRead(Y_pin) > 900) {
        storedHeading = (storedHeading + 5);
      }
      lastUpdateTime6 = millis();
    }
    if ((millis() - lastUpdateTime7) > updateDelay) {
      if ((analogRead(X_pin) <= 100) && (Deviation < MaxDeviation)) {
        Deviation = (Deviation + 2);
      }
      lastUpdateTime7 = millis();
    }
    if ((millis() - lastUpdateTime8) > updateDelay) {
      if ((analogRead(X_pin) > 900) && (Deviation > MinDeviation)) {
        Deviation = (Deviation - 2);
      }
      lastUpdateTime8 = millis();
    }

    //--------------------------------------------------Serial & lCD print------------------------------

    if ((millis() - lastUpdateTime) > updateDelay) {

      Serial.print(" StoredHeading ");
      Serial.println(storedHeading);
      Serial.print(" Kurs ");
      Serial.println(Heading);
      Serial.print(" knapp ");
      Serial.println(digitalRead(buttonPin));
      Serial.print(" Deviation ");
      Serial.println(Deviation);
      lcd.setCursor (0, 0);       // go to start of 2nd line
      lcd.print("                ");
      lcd.setCursor (0, 0);
      lcd.print("Kurs");
      lcd.print(Heading);
      lcd.setCursor (9, 0);
      lcd.print("Lock");
      lcd.print(storedHeading);
      lcd.setCursor (0, 8);
      lcd.print("            ");
      lcd.print("Dv");
      lcd.print(Deviation);
      lastUpdateTime = millis();
    }

    if (buttonState == LOW) {

      storedHeading = Heading;

    } else {
      if (Heading > storedHeading - Deviation && Heading < storedHeading + Deviation) {
        digitalWrite(ledPin, HIGH);
        if ((millis() - lastUpdateTime2) > updateDelay) {

          Serial.println(" KURS OK ");
          lcd.setCursor (0, 1);
          lcd.print("        ");
          lcd.setCursor (0, 1);
          lcd.print("KURS OK");
          lastUpdateTime2 = millis();
        }
        delay(50);

      }
      else {
        digitalWrite(ledPin, LOW);
        CorrectHeading();
      }
    }
  }
}

void CorrectHeading() {
  if (Heading < storedHeading) {
    Serial.println("Vanster");
    lcd.setCursor (0, 1);
    lcd.print ("                ");
    lcd.setCursor (0, 1);
    lcd.print("Correcting LEFT");
    TurnLeft();
    delay(20);
  }
  else {
    Serial.println("Hoger");
    lcd.setCursor (0, 1);
    lcd.print ("                ");
    lcd.setCursor (0, 1);
    lcd.print("Correcting RIGHT");
    TurnRight();
    delay(20);
  }
}

void TurnRight() {
  digitalWrite(relayPin1, HIGH);
  if ((millis() - lastUpdateTime9) > turnTime) {
    digitalWrite(relayPin1, LOW);

  }

  if ((millis() - lastUpdateTime9) > waitTime + turnTime) {
    digitalWrite(relayPin2, HIGH);

    if ((millis() - lastUpdateTime9) > turnBackTime + waitTime + turnTime) {
      digitalWrite(relayPin2, LOW);
      
      if ((millis() - lastUpdateTime9) > turnBackTime + (waitTime * 2) + turnTime) {
        digitalWrite(relayPin2, LOW);

        lastUpdateTime9 = millis();
      }
    }
  }
}

void TurnLeft() { //Sväng vänster
  digitalWrite(relayPin2, HIGH);
  delay(turnTime);
  digitalWrite(relayPin2, LOW);
  delay(waitTime);
  digitalWrite(relayPin1, HIGH);
  delay(turnBackTime);

  digitalWrite(relayPin1, LOW);
  delay(waitTime);
}

void fishMode() {
  /*

    if (digitalRead(SW_pin) == LOW){
    delay(50);
    running = !running;
    fishModeState = running;
  */
}

look, e few moments in and im already learning things i had no idea existed :wink: great tool!

  fishMode;

Oops

 // set the LED:
  fishModeState, ledState;

Oops2

To save hair-loss later, change all instances like

    Serial.println("FISK!");

to    Serial.println(F("FISK!"));

AWOL:

  fishMode;
 // set the LED:

fishModeState, ledState;



Oops<sup>2</sup>

To save hair-loss later, change all instances like


Serial.println("FISK!");



to`    Serial.println(F("FISK!"));`

sorry but i dont get what the oops mean? fishmodestate is to store the value making the momentary switch in the joystick to a toggle switch, pausing the course correction and in time putting the gear in neutral! "fishmodestate, ledState" is just me being to lazy to rewrite the code from the example i copied?

also, what does the extra F in serial println do?

Oops means either you've done something that compiles but doesn't do what you think, or you've done something very silly, or both.

In this case, it means you've done something that compiles but doens't do what you think.

The F keeps the string literal in read-only memory, where it belongs.

okey finally have a code that works! Thanks AWOL for helpful tips!
i added an integer to keep track of what part of the void "TurnLeft" & "TurnRight" have been executed!
i´m adding the revised code in case it can help anyone else havong the same problems! (had to remove comments and some serial codes to not exeed maximum lenght)

*/#include <Wire.h>
#include <MechaQMC5883.h>
#include <LCD.h> //LCD display
#include <LiquidCrystal_I2C.h> //LCD display

MechaQMC5883 qmc; //Kompass

const int buttonPin = 2; //Knapp för lagring av kurs
const int ledPin = 13;
const int relayPin1 = 12; //Relä högersväng
const int relayPin2 = 11; //Relä vänstersväng
const long turnTime = 1000; //Hur länge ratten ska svänga
const long waitTime = 1000; //Hur länge den väntar innan den svänger tillbaka
const long turnBackTime = 1000; //Hur länge den svänger tillbaka
const int MinDeviation = 10; //lägsta gränsen för avvikelse
const int MaxDeviation = 60; //högsta gränsen för avvikelse


int Heading; //Kurs
int storedHeading; //Lagrad kurs
int Deviation = 10; //avvikelse från Kurs innan rättning.
int buttonState; // knapp för lagring av kurs.


int TurnRightActive;
int TurnLeftActive;


int ledState = HIGH;         // the current state of the output pin
int buttonState2;             // the current reading from the input pin
int lastButtonState = LOW;   // the previous reading from the input pin

unsigned long lastDebounceTime = 0;  // the last time the output pin was toggled
unsigned long debounceDelay = 20;    // the debounce time; increase if the output flickers
unsigned long updateDelay = 500;
unsigned long updateDelayFish = 1000;
unsigned long updateDelayFish2 = 2000;
unsigned long lastUpdateTime = 0;
unsigned long lastUpdateTime2 = 0;
unsigned long lastUpdateTime3 = 0;
unsigned long lastUpdateTime4 = 0;
unsigned long lastUpdateTime5 = 0;
unsigned long lastUpdateTime6 = 0;
unsigned long lastUpdateTime7 = 0;
unsigned long lastUpdateTime8 = 0;
unsigned long lastUpdateTime9 = 0;
unsigned long lastUpdateTime10 = 0;
unsigned long lastUpdateTime11 = 0;
unsigned long lastUpdateTime12 = 0;
unsigned long lastUpdateTime13 = 0;
unsigned long lastUpdateTime14 = 0;


// Arduino pin numbers
const int SW_pin = 3; // digital pin connected to switch output
const int X_pin = 0; // analog pin connected to X output
const int Y_pin = 1; // analog pin connected to Y output





int x, y, z;
int azimuth;//float azimuth; //is supporting float too



#define I2C_ADDR    0x27 // <<----- Add your address here.  Find it from I2C Scanner
#define BACKLIGHT_PIN     3
#define En_pin  2
#define Rw_pin  1
#define Rs_pin  0
#define D4_pin  4
#define D5_pin  5
#define D6_pin  6
#define D7_pin  7

LiquidCrystal_I2C  lcd(I2C_ADDR, En_pin, Rw_pin, Rs_pin, D4_pin, D5_pin, D6_pin, D7_pin);


void setup() {
  Wire.begin();
  Serial.begin(9600);
  qmc.init();
  
 

  pinMode(SW_pin, INPUT);
  digitalWrite(SW_pin, HIGH);


  TurnRightActive = 0;
  TurnLeftActive = 0;

  
  lcd.begin (16, 2);



  lcd.setBacklightPin(BACKLIGHT_PIN, POSITIVE);
  lcd.setBacklight(HIGH);
  lcd.home (); // go home


  pinMode(ledPin, OUTPUT);
  pinMode(buttonPin, INPUT);
  pinMode(relayPin1, OUTPUT);
  pinMode(relayPin2, OUTPUT);

  qmc.read(&x, &y, &z, &azimuth);

  storedHeading = (azimuth);

}

void loop() {
  if (TurnRightActive >= 1) {
    TurnRight();
  }
  if (TurnLeftActive >= 1) {
    TurnLeft();
  }
  int reading = digitalRead(SW_pin);

 
  if (reading != lastButtonState) {
  
    lastDebounceTime = millis();
  }

  if ((millis() - lastDebounceTime) > debounceDelay) {

    
   
    if (reading != buttonState2) {
      buttonState2 = reading;

    
      if (buttonState2 == HIGH) {
        ledState = !ledState;
      }
    }
  }





  lastButtonState = reading;
  if (ledState == HIGH) {
    Serial.println("FISK!");


    if (millis() - lastUpdateTime3 > updateDelayFish) {
      lcd.setCursor (5, 0);
      lcd.print("FISK!!");
      lastUpdateTime3 = millis();
      if ((millis() - lastUpdateTime4) > updateDelayFish2) {
        lcd.clear();
        lastUpdateTime4 = millis();
      }
    }


    fishMode;
  }
  else {
 

    qmc.read(&x, &y, &z, &azimuth);
    //azimuth = qmc.azimuth(&y,&x);//you can get custom azimuth
    buttonState = digitalRead (buttonPin);
    Heading = (azimuth);


    if (analogRead(Y_pin) <= 100) {
      if ((millis() - lastUpdateTime5) > updateDelay) {
        storedHeading = (storedHeading - 5);
      }
      lastUpdateTime5 = millis();
    }

    if (analogRead(Y_pin) > 900) {
      if ((millis() - lastUpdateTime6) > updateDelay) {
        storedHeading = (storedHeading + 5);
      }
      lastUpdateTime6 = millis();
    }

    if ((analogRead(X_pin) <= 100) && (Deviation < MaxDeviation)) {
      if ((millis() - lastUpdateTime7) > updateDelay) {
        Deviation = (Deviation + 2);
      }
      lastUpdateTime7 = millis();
    }

    if ((analogRead(X_pin) > 900) && (Deviation > MinDeviation)) {
      if ((millis() - lastUpdateTime8) > updateDelay) {
        Deviation = (Deviation - 2);
      }
      lastUpdateTime8 = millis();
    }





  

    if (buttonState == LOW) {

      storedHeading = Heading;

    } else {
      if (Heading > storedHeading - Deviation && Heading < storedHeading + Deviation) {
        digitalWrite(ledPin, HIGH);
        if ((millis() - lastUpdateTime2) > updateDelay) {

          Serial.println(" KURS OK ");
          lcd.setCursor (0, 1);
          lcd.print("        ");
          lcd.setCursor (0, 1);
          lcd.print("KURS OK");
          lastUpdateTime2 = millis();
        }
        delay(50);

      }
      else {
        digitalWrite(ledPin, LOW);
        CorrectHeading();






      }
    }

  }


}





void CorrectHeading() { // Svänga höger eller vä?

  if (Heading < storedHeading) {
    if ((millis() - lastUpdateTime13) > updateDelay) {
      Serial.println("Vanster");
      lcd.setCursor (0, 1);
      lcd.print ("           ");
      lcd.setCursor (0, 1);
      lcd.print("Corr LEFT");
      lastUpdateTime13 = millis();
    }
    if ((TurnRightActive == 0) && (TurnLeftActive == 0)) {
      TurnLeftActive = 1;
    }
    TurnLeft();
    delay(10);
  }
  else {
    if ((millis() - lastUpdateTime14) > updateDelay) {
      Serial.println("Hoger");
      lcd.setCursor (0, 1);
      lcd.print ("           ");
      lcd.setCursor (0, 1);
      lcd.print("Corr RIGHT");
      lastUpdateTime14 = millis();
    }
    if ((TurnLeftActive == 0) && (TurnRightActive == 0)) {
      TurnRightActive = 1;
    }
    TurnRight();

    delay(10);

  }
}


void TurnRight() { 

  if (TurnRightActive == 1) {
    digitalWrite(relayPin1, HIGH);
    TurnRightActive = 2;
    delay(10);
    lastUpdateTime9 = millis();
  }
  if ((millis() - lastUpdateTime9) > turnTime && (TurnRightActive == 2)) {
    digitalWrite(relayPin1, LOW);
    TurnRightActive = 3;
    delay(10);
    lastUpdateTime10 = millis();
  }

  if (TurnRightActive == 3) {
    if ((millis() - lastUpdateTime10) > waitTime) {
      TurnRightActive = 4;
      delay(10);
      lastUpdateTime11 = millis();
    }
  }
  if (TurnRightActive == 4) {
    digitalWrite(relayPin2, HIGH);
    if ((millis() - lastUpdateTime11) > turnBackTime) {
      digitalWrite(relayPin2, LOW);
      TurnRightActive = 5;
      delay(10);
      lastUpdateTime12 = millis();
    }
  }
  if (TurnRightActive == 5) {
    if ((millis() - lastUpdateTime12) > waitTime) {
      TurnRightActive = 0;
      delay(10);

    }

  }
}


void TurnLeft() { 
  if (TurnLeftActive == 1) {
    digitalWrite(relayPin2, HIGH);
    TurnLeftActive = 2;
    delay(10);
    lastUpdateTime9 = millis();
  }
  if ((millis() - lastUpdateTime9) > turnTime && (TurnLeftActive == 2)) {
    digitalWrite(relayPin2, LOW);
    TurnLeftActive = 3;
    delay(10);
    lastUpdateTime10 = millis();
  }

  if (TurnLeftActive == 3) {
    if ((millis() - lastUpdateTime10) > waitTime) {
      TurnLeftActive = 4;
      delay(10);
      lastUpdateTime11 = millis();
    }
  }
  if (TurnLeftActive == 4) {
    digitalWrite(relayPin1, HIGH);
    if ((millis() - lastUpdateTime11) > turnBackTime) {
      digitalWrite(relayPin1, LOW);
      TurnLeftActive = 5;
      delay(10);
      lastUpdateTime12 = millis();
    }
  }
  if (TurnLeftActive == 5) {
    if ((millis() - lastUpdateTime12) > waitTime) {
      TurnLeftActive = 0;
      delay(10);

    }

  }
}



void fishMode() {


}

    fishMode; Still oops