Hi,
In firmware for Arduino Mega for my trolling motor autopilot for small boat
I’m using multiple IF statement/conditions and multiple millis "blink without delay".
in the main while (true) loop for go home procedure I set few conditions how my trolling motor should operate to keep me close to my home position (GPS based positioning).
The first condition is that boat (GPS position) must be between 1.5 and 2500 m from home (stored GPS position).
When that is true the program set other conditions based on angle difference of trolling motor (compass) heading and GPS bearing
(calculated from home GPS coordinates and current GPS coordinates) named "error" absolute value.
I set two maximum angle "error" differences T and Tx
T is maximum "error" when trolling motor propeller is ON "if (abs(error) <= 30 motor is ON; else is OFF"
Tx is maximum "error" difference when stepper motor that rotate trolling motor shaft CW or CCW is OFF "if (error < 0 && abs(error) > Tx); if (error > 0 && abs(error) > Tx)
Now my questions are:
-
are this IF statements/conditions properly written
-
are all this millis "blink without delay" (mostly used for acceleration and deceleration of propeller and stepper motor timing) properly written (don’t conflict between each other)
Basically everything seams to work but some time I think that rotation of shaft and activation of prop is little bit off from given conditions.
if (Distance_To_Home > 1.50 && Distance_To_Home <= 2500){ // If the boat is between 1.5 and 2500 m from the home spot, run
//*******************************************************
//turning ON/OFF trolling motor proppeler
if (abs(error) <= T){ // T = 30°
unsigned long currentMillis = millis();
if (((unsigned long)(currentMillis - previousMillis) >= interval_a) && (pwm < run_Speed)) {
pwm = pwm + b;
analogWrite(runPin, pwm); // turn on trolling motor and accelerate to pwm walue
previousMillis = currentMillis;
//Serial.println(pwm);
}
else if (((unsigned long)(currentMillis - previousMillis) >= interval_d) && (run_Speed < pwm)){
pwm = pwm - b;
analogWrite(runPin, pwm); // turn on trolling motor and decelerate to pwm walue
previousMillis = currentMillis;
//Serial.println(pwm);
}
}
else {
digitalWrite(runPin, LOW);
pwm = 0;
}
//***************************************************
//rotating trolling motor CCW
if (error < 0 && abs(error) > Tx){ // Tx = maximum allowed angle between compass and gps heading (set to 10 degree)
digitalWrite(enablePin, LOW); //enable A4988 stepper motor driver who is rotating compass and trolling motor bar
delay(5);
digitalWrite(dirPin, HIGH);// Stepper motor rotate CCW; LOW/HIGH (check on hardware!!!)
unsigned long currentMillis = millis();
if (((unsigned long)(currentMillis - previousMillis) >= M) && (stepAcc < Puls)) {
stepAcc = stepAcc + 300;
previousMillis = currentMillis;
}
tone(stepPin,stepAcc);
}
//***************************************
//rotating trolling motor CW
if (error > 0 && abs(error) > Tx){ // Tx = maximum allowed angle diference between compass and gps heading (set to 10 degree)
digitalWrite(enablePin, LOW); //enable A4988 stepper motor driver who is rotating compassand trolling motor bar
delay(5);
digitalWrite(dirPin, LOW);// Stepper motor rotate CW; LOW/HIGH (check on hardware!!!)
unsigned long currentMillis = millis();
if (((unsigned long)(currentMillis - previousMillis) >= M) && (stepAcc < Puls)) {
stepAcc = stepAcc + 300;
previousMillis = currentMillis;
}
tone(stepPin,stepAcc);
}
//**********************************************************
//stop rotating trolling motor shaft
if (abs(error) <= Tx){
//digitalWrite(stepPin, LOW);
digitalWrite(enablePin, HIGH); //deactivation of stepper motor driver A4988
noTone(stepPin);
stepAcc = 0;
}
Serial.print("kut=");
Serial.print(error);
Serial.print(" D=");
Serial.print(Distance_To_Home);
Serial.print("m heading=");
Serial.print(headingct);
Serial.print(" GPS_Course=");
Serial.print(GPS_Course);
Serial.println("");
unsigned long cMillis = millis();
if ((unsigned long)(cMillis - pMillis) >= 500){
Serial1.print("Er:");
Serial1.print(error);
Serial1.print("° D=");
Serial1.print(Distance_To_Home);
Serial1.println("m ");
pMillis = cMillis;
}
} //End of Distance_To_Home >1.5m Loop
so first condition to start keeping position is that Arduino have all data and that boat is between 1,5 and 2500 m from waypoint.