// AUTOPILOT version 2
// by Marco Zonca, 2019-2022
/*
This sketch works as Autopilot for small sailing boats
Arduino Nano as CPU, Arduino Nano as watchdog, GPS BT-220 nmea, stepper motor + controller, rf315Mhz RC, 6 buttons,
buzzer, i2c display, 3xLEDS, i2c 24c04 eeprom, Mux 4051 for trasducers (sensors), lipo 2s 7.4v 2600mA,
7805 voltage regulator, Thermistor NTC, ADC MCP3424, etc.;
*/
#include <LiquidCrystal_I2C.h>
#include <NewTone.h>
#include <Stepper.h>
#include <Wire.h>
#include <MCP342x.h>
#include <PID_v2.h>
String inputString = "";
String tzone = "00";
String nm_time = "00:00:00";
String nm_validity = "V";
String nm_latitude = "ddmm.mmmm'N";
String nm_longitude = "dddmm.mmmm'E";
String nm_knots = "0.0kn";
float nmf_knots = 0.0;
String nm_truecourse = "360";
float nmf_truecourse = 360;
String nm_date = "dd/mm/yyyy";
String nm_routetofollow = "000";
float nmf_routetofollow = 0;
unsigned long previousStearingMillis = 0;
unsigned long currentStearingMillis = 0;
unsigned long prevCheckSensorsMillis = 0;
unsigned long currCheckSensorsMillis = 0;
int CheckSensorsInterval = 10000;
bool stringComplete = false;
bool isfirstfix = true;
bool ispause = true;
bool isStearing = false;
bool isSetup = false;
int s=0;
int y=0;
int z=0;
int d=0;
int rfRemoteControlValue = 0;
int HWButtonValue = 0;
int SetupParameter = 0;
float calcmove = 0;
float cm = 0;
float Stearing = 0;
float prevStearing = 0;
float t = 0;
int EEdisk = 0x50;
int EEid1 = 0x29;
int EEid2 = 0x00;
uint8_t ADCaddr = 0x68;
unsigned int EEaddress = 0;
unsigned int EEbytes = 24; // eeprom nr of bytes to r/w, see also EEdata[ ]
byte EEdata[24]; // put the same as EEbytes
byte EEbytedata;
int EEerr = 0;
float SensorVBatt=0;
float SensorVRes=0;
float SensorTemp=0;
float SensormAmp=0;
// following parameters are the defaults but are read/write in eeprom
// eeprom is initialized if at addresses 0 and 1 the content is different addres len type notes
// 0-255 bytes at 0x50 EEdisk, 256-512 bytes at 0x51 (not used) ---------------------------------------------------------------
// 0 1B byte 01001001 (0x29 as autopilot project id1)
// 1 1B byte 00000000 (0x00 " " id2)
int StearingInterval = 800; // millis between try and back 2 2B int StearingInterval 100-5000 step 100
int StearingMinToMove = 4; // compass_degrees 4 2B int StearingMinToMove 1-20 step 1
int StearingMaxMove = 90; // compass_degrees 6 2B int StearingMaxMove 10-360 step 10
// 8 2B int StearingSpeedOut 1-100 step 1
// 10 2B int StearingSpeedBack 1-100 step 1
// 12 2B int StearingKP 0-400 step 10
// 14 2B int StearingKI 0-400 step 10
// 16 2B int StearingKD 0-400 step 10
// 18 2B int StearingReverse 0-1 step 1
// 20 2B int StearingPWM 100-255 step 1
// 22 2B int StearingTZone -12-+12 step 1
// 24 free
int StearingSpeedOut = 40; // speed move out ("fast") steps/sec
int StearingSpeedBack = 20; // speed move back ("slow") steps/sec
int StearingKP = 150; // PID "P" 1.00 (cents)
int StearingKI = 10; // PID "I" 0.20 (cents)
int StearingKD = 10; // PID "D" 0.20 (cents)
int StearingReverse = 0; // 0=false 1=true (clockwise or not)
int StearingPWM = 245; // PWM on enable motor pins
int StearingTZone = 0; // GMT=0 Italy is +1 or +2
byte bStearingInterval[sizeof(int)];
byte bStearingMinToMove[sizeof(int)];
byte bStearingMaxMove[sizeof(int)];
byte bStearingSpeedOut[sizeof(int)];
byte bStearingSpeedBack[sizeof(int)];
byte bStearingKP[sizeof(int)];
byte bStearingKI[sizeof(int)];
byte bStearingKD[sizeof(int)];
byte bStearingReverse[sizeof(int)];
byte bStearingPWM[sizeof(int)];
byte bStearingTZone[sizeof(int)];
int prev_StearingInterval=0;
int prev_StearingMinToMove=0;
int prev_StearingMaxMove=0;
int prev_StearingSpeedOut=0;
int prev_StearingSpeedBack=0;
int prev_StearingKP=0;
int prev_StearingKI=0;
int prev_StearingKD=0;
int prev_StearingReverse=0;
int prev_StearingPWM=0;
int prev_StearingTZone=0;
const int ledpausePin = 2;
const int watchDogPin = 3;
const int MuxSelBit0Pin = 8; // 000=Vin 001=Vbatt 010=Temp
const int MuxSelBit1Pin = 7; //
const int MuxSelBit2Pin = 6; //
const int motorsABenablePin = 5; // PWM
const int MuxIOPin = 14;
const int ButtonsPin = 15;
const int rfRemoteControlPin = 16;
const int speakerPin = 17;
const int RCleftbutton = 201;
const int RCrightbutton = 202;
const int RCleft10button = 203;
const int RCright10button = 204;
const int HWleftbutton = 101;
const int HWrightbutton = 102;
const int HWpausebutton = 103;
const int HWsetupbutton = 104;
const int HWleft10button = 105;
const int HWright10button = 106;
const int motorStepsPerRevolution = 216; // 216 steps for model 23LM, 1.8 per step, 54 steps = 1/4 of revolution
LiquidCrystal_I2C lcd (0x27, 2, 1, 0, 4, 5, 6, 7, 3, POSITIVE);
Stepper motor(motorStepsPerRevolution, 9, 10, 11, 12);
MCP342x adc = MCP342x(ADCaddr);
PID_v2 myPID((float)(StearingKP/100.0),(float)(StearingKI/100.0),(float)(StearingKD/100.0), PID::Direct); // P, I, D, d/r
void setup() {
Serial.begin(4800);
lcd.begin(16,2);
Wire.begin();
inputString.reserve(200);
pinMode(motorsABenablePin, OUTPUT);
pinMode(MuxSelBit0Pin, OUTPUT);
pinMode(MuxSelBit1Pin, OUTPUT);
pinMode(MuxSelBit2Pin, OUTPUT);
digitalWrite(motorsABenablePin, LOW);
digitalWrite(MuxSelBit0Pin, LOW);
digitalWrite(MuxSelBit1Pin, LOW);
digitalWrite(MuxSelBit2Pin, LOW);
pinMode(ledpausePin, OUTPUT);
pinMode(watchDogPin, OUTPUT);
digitalWrite(ledpausePin, LOW);
digitalWrite(watchDogPin, LOW);
MCP342x::generalCallReset();
delay(1); // MC342x needs 300us to settle, wait 1ms
// read+check EEPROM (formatting (initializing) if new or not identified)
lcd.clear();
lcd.setCursor(0,0);
lcd.print("Memory check...");
lcd.setCursor(0,1);
for (s = 0; s < EEbytes; s ++) {
EEaddress = s;
EEbytedata = readEEPROM (EEdisk, EEaddress);
EEdata[s] = EEbytedata;
}
if (EEerr) {
lcd.print("E=");
lcd.print(EEerr);
delay(5000);
}
if ((EEdata[0] != EEid1) || (EEdata[1] != EEid2)) {
lcd.print(" init! ");
goupdateEEPROM();
if (EEerr) {
lcd.print("E=");
lcd.print(EEerr);
delay(5000);
}
}
memcpy(bStearingInterval, EEdata+2, sizeof(int));
memcpy(bStearingMinToMove, EEdata+4, sizeof(int));
memcpy(bStearingMaxMove, EEdata+6, sizeof(int));
memcpy(bStearingSpeedOut, EEdata+8, sizeof(int));
memcpy(bStearingSpeedBack, EEdata+10, sizeof(int));
memcpy(bStearingKP, EEdata+12, sizeof(int));
memcpy(bStearingKI, EEdata+14, sizeof(int));
memcpy(bStearingKD, EEdata+16, sizeof(int));
memcpy(bStearingReverse, EEdata+18, sizeof(int));
memcpy(bStearingPWM, EEdata+20, sizeof(int));
memcpy(bStearingTZone, EEdata+22, sizeof(int));
StearingInterval = *((int*) bStearingInterval);
StearingMinToMove = *((int*) bStearingMinToMove);
StearingMaxMove = *((int*) bStearingMaxMove);
StearingSpeedOut = *((int*) bStearingSpeedOut);
StearingSpeedBack = *((int*) bStearingSpeedBack);
StearingKP = *((int*) bStearingKP);
StearingKI = *((int*) bStearingKI);
StearingKD = *((int*) bStearingKD);
StearingReverse = *((int*) bStearingReverse);
StearingPWM = *((int*) bStearingPWM);
StearingTZone = *((int*) bStearingTZone);
prev_StearingInterval = StearingInterval;
prev_StearingMinToMove = StearingMinToMove;
prev_StearingMaxMove = StearingMaxMove;
prev_StearingSpeedOut = StearingSpeedOut;
prev_StearingSpeedBack = StearingSpeedBack;
prev_StearingKP = StearingKP;
prev_StearingKI = StearingKI;
prev_StearingKD = StearingKD;
prev_StearingReverse = StearingReverse;
prev_StearingPWM = StearingPWM;
prev_StearingTZone = StearingTZone;
myPID.SetOutputLimits((StearingMaxMove*(-1)), StearingMaxMove);
myPID.Start(0, 0, 0); // input, output, setpoint (direction, correction, target)
myPID.SetSampleTime(StearingInterval);
lcd.print(" OK");
delay(1000);
lcd.clear();
lcd.print("GPS reading...");
delay(1000);
}
void loop() {
// CHECK SENSORS -----------------
currCheckSensorsMillis = millis();
if (currCheckSensorsMillis - prevCheckSensorsMillis >= CheckSensorsInterval) {
readMuxSensors();
if ((SensorVBatt <= 6.8) || (SensorTemp >= 60)) {
lcd.clear();
lcd.setCursor(0,0);
lcd.print("Alarm sensors! ");
lcd.setCursor(1,1);
lcd.print("V=");
lcd.print(SensorVBatt);
lcd.print(" ");
lcd.print(int(SensorTemp));
lcd.write(0xDF);
lcd.print("C");
NewTone (speakerPin,10);
delay(1000);
noNewTone();
}
prevCheckSensorsMillis = currCheckSensorsMillis;
}
// STEARING CONTROL ----------------
currentStearingMillis = millis();
if (currentStearingMillis - previousStearingMillis >= StearingInterval) {
if (isStearing == false && ispause == false) {
myPID.SetOutputLimits((StearingMaxMove*(-1)), StearingMaxMove);
myPID.SetSampleTime(StearingInterval);
calcmove = nmf_routetofollow - nmf_truecourse;
if (calcmove < (-180)) {
calcmove = calcmove + 360;
} else {
if (calcmove > (+180)) {
calcmove = calcmove - 360;
}
}
if (StearingReverse==1) {
calcmove = (calcmove * -1);
}
if (abs(calcmove) >= StearingMinToMove) { // go try (move stearing)
myPID.SetTunings((float)(StearingKP/100.0),(float)(StearingKI/100.0),(float)(StearingKD/100.0));
Stearing = myPID.Run(calcmove); // calc PID correction
motor.setSpeed(StearingSpeedOut);
gomotor(int((Stearing * 216) / 360)); // 54 steps = 1/4 of revolution (90), 216 = 1 revolution (360)
prevStearing = Stearing;
isStearing = true;
}
} else { // go back (move stearing to "zero" position)
if (isStearing == true) {
Stearing = (prevStearing * -1);
motor.setSpeed(StearingSpeedBack);
gomotor(int((Stearing * 216) / 360)); // 54 steps = 1/4 of revolution (90), 216 = 1 revolution (360)
Stearing = 0;
prevStearing = 0;
isStearing = false;
}
}
previousStearingMillis = currentStearingMillis;
}
// RC RF BUTTONS ------------------
rfRemoteControlValue = checkRfRC();
if (rfRemoteControlValue) {
switch (rfRemoteControlValue) {
case RCleftbutton: // Left -1 RC button
goleft();
break;
case RCrightbutton: // Right +1 RC button
goright();
break;
case RCleft10button: // Left-10 RC button
goleft10();
break;
case RCright10button: // Right+10 RC button
goright10();
break;
}
}
// BUTTONS ------------------------
HWButtonValue = checkHWButtons();
if (HWButtonValue) {
switch (HWButtonValue) {
case HWleftbutton: // Left(-1) HW button
if (isSetup == false) {
goleft();
} else {
setupMinus();
}
break;
case HWrightbutton: // Right(+1) HW button
if (isSetup == false) {
goright();
} else {
setupPlus();
}
break;
case HWpausebutton: // Pause HW button
gopause();
break;
case HWsetupbutton: // Setup HW button
gosetup();
break;
case HWleft10button: // Left(-10) HW button
goleft10();
break;
case HWright10button: // Right(+10) HW button
goright10();
break;
}
}
// GPS NMEA ------------------
if (stringComplete == true) { // received nmea sentence by serial port RX
bool ret;
ret = nmeaExtractData();
inputString = "";
stringComplete = false;
if (ret == true) {
RefreshDisplay();
}
}
// WATCHDOG FEEDING ----------------
if (digitalRead(watchDogPin) == LOW) {
digitalWrite(watchDogPin, HIGH);
} else {
digitalWrite(watchDogPin, LOW);
}
}
// read sensors, trasducers on multiplexer, ADC
void readMuxSensors() {
uint8_t err = 0;
int x = 0;
long ADCval = 0;
float Fmem = 0;
float Vo = 0;
float n = 0;
float n1 = 0;
float n2 = 0;
float corr = 0;
float logR2 = 0;
float R2 = 0;
float T = 0;
float R1 = 100000; // 100k resistor in NTC voltage divider
float c1 = 6.66082410500E-004; // Steinhart-Hart coeff. 1 for NTC
float c2 = 2.23928204100E-004; // Steinhart-Hart coeff. 2 for NTC
float c3 = 7.19951882000E-008; // Steinhart-Hart coeff. 3 for NTC
digitalWrite(MuxSelBit0Pin, LOW); // 000=Vbatt
digitalWrite(MuxSelBit1Pin, LOW);
digitalWrite(MuxSelBit2Pin, LOW);
Fmem=0;
for (x=1;x<=33;x++) {
n = analogRead(MuxIOPin);
n1=(((10.00 * n) / 1023.00));
Fmem=Fmem+n1;
}
n1=(Fmem/(x-1));
n2=(n1 + ((n1 * 1.15) /100)); // arbitrary correction (not active = 0.0%)
SensorVBatt=roundTwoDec(n2);
digitalWrite(MuxSelBit0Pin, HIGH); // 001=Vres
digitalWrite(MuxSelBit1Pin, LOW);
digitalWrite(MuxSelBit2Pin, LOW);
Fmem=0;
for (x=1;x<=33;x++) {
n = analogRead(MuxIOPin);
n1=(((10.00 * n) / 1023.00));
Fmem=Fmem+n1;
}
n1=(Fmem/(x-1));
n2=(n1 + ((n1 * 1.15) /100)); // arbitrary correction (not active = 0.0%)
SensorVRes=roundTwoDec(n2);
digitalWrite(MuxSelBit0Pin, LOW); // 010=NTC Temp
digitalWrite(MuxSelBit1Pin, HIGH);
digitalWrite(MuxSelBit2Pin, LOW);
Fmem=0;
for (x=1;x<=33;x++) {
Vo = analogRead(MuxIOPin);
R2 = R1 * (1023.0 / Vo - 1.0); // Steinhart-Hart Temperature Calc.
logR2 = log(R2);
T = (1.0 / (c1 + c2 * logR2 + c3 * logR2 * logR2 * logR2));
n1 = T - 273.15; // Celsius
Fmem=Fmem+n1;
}
n2=(Fmem/(x-1));
SensorTemp=roundZeroDec(n2);
MCP342x::Config status; // differential ADC
err = adc.convertAndRead(MCP342x::channel1,MCP342x::oneShot,MCP342x::resolution12,MCP342x::gain2,1000000,ADCval,status);
n=(ADCval / 1.0); // (ADCgain=2 / Vdivider=2) = 1.0
n1=(n + ((n * 0.0) /100)); // arbitrary correction (not active = 0.0%)
n2=(n1/0.22); // n1=VADC, 0.22ohm = shunt resistor, mA
SensormAmp=roundZeroDec(n2);
}
// extract data from nmea inputString
bool nmeaExtractData() {
bool ret = false; //true if nmea sentence = $GNRMC and valid CHKSUM
if ((inputString.substring(0,6) == "$GNRMC") && (inputString.substring(inputString.length()-4,inputString.length()-2) == nmea0183_checksum(inputString))) {
y=0;
for (s = 1; s < 11; s ++) {
y=inputString.indexOf(",",y);
switch (s) {
case 1: //time
z=inputString.indexOf(",",y+1);
if (z>(y+1)) {
t=inputString.substring(y+1,y+2+1).toFloat();
t=t+StearingTZone;
if (t>23) {
t=t-24;
}
if (t<0) {
t=t+24;
}
tzone="0"+String(t,0);
nm_time=tzone.substring(tzone.length()-2)+":"+inputString.substring(y+1+2,y+4+1)+":"+inputString.substring(y+1+4,y+6+1);
}
y=z;
break;
case 2: //validity
z=inputString.indexOf(",",y+1);
if (z>(y+1)) {
nm_validity=inputString.substring(y+1,y+1+1);
}
y=z;
break;
case 3: //latitude
z=inputString.indexOf(",",y+1);
if (z>(y+1)) {
nm_latitude=inputString.substring(y+1,y+2+1)+""+inputString.substring(y+1+2,y+10+1)+"'";
}
y=z;
break;
case 4: //north/south
z=inputString.indexOf(",",y+1);
if (z>(y+1)) {
nm_latitude=nm_latitude + inputString.substring(y+1,y+1+1);
}
y=z;
break;
case 5: //longitude
z=inputString.indexOf(",",y+1);
if (z>(y+1)) {
nm_longitude=inputString.substring(y+1,y+3+1)+""+inputString.substring(y+1+3,y+11+1)+"'";
}
y=z;
break;
case 6: //east/west
z=inputString.indexOf(",",y+1);
if (z>(y+1)) {
nm_longitude=nm_longitude + inputString.substring(y+1,y+1+1);
}
y=z;
break;
case 7: //speed knots
z=inputString.indexOf(",",y+1);
if (z>(y+1)) {
nmf_knots=inputString.substring(y+1,z).toFloat();
t=roundOneDec(nmf_knots);
nm_knots=String(t,1)+"kn";
}
y=z;
break;
case 8: //true course
z=inputString.indexOf(",",y+1);
if (z>(y+1)) {
nmf_truecourse=inputString.substring(y+1,z).toFloat();
d=nmf_truecourse;
nm_truecourse=d;
}
y=z;
break;
case 9: //date
z=inputString.indexOf(",",y+1);
if (z>(y+1)) {
nm_date=inputString.substring(y+1,y+2+1)+"/"+inputString.substring(y+1+2,y+4+1)+"/20"+inputString.substring(y+1+4,y+6+1);
}
y=z;
break;
case 10:
// statements
break;
default:
// statements
break;
}
}
if ((isfirstfix == true) || (ispause == true)) {
nm_routetofollow=nm_truecourse;
nmf_routetofollow=nmf_truecourse;
isfirstfix=false;
}
ret=true;
}
return ret;
}
// increase(+) parameter value during setup
void setupPlus() {
switch (SetupParameter) {
case 2: //interval
StearingInterval = (StearingInterval + 100);
if (StearingInterval > 5000) {
StearingInterval = 5000;
}
break;
case 3: //min. to move
StearingMinToMove = (StearingMinToMove + 1);
if (StearingMinToMove > 20) {
StearingMinToMove = 20;
}
break;
case 4: //max. move
StearingMaxMove = (StearingMaxMove + 10);
if (StearingMaxMove > 360) {
StearingMaxMove = 360;
}
break;
case 5: //speed out
StearingSpeedOut = (StearingSpeedOut + 1);
if (StearingSpeedOut > 100) {
StearingSpeedOut = 100;
}
break;
case 6: //speed back
StearingSpeedBack = (StearingSpeedBack + 1);
if (StearingSpeedBack > 100) {
StearingSpeedBack = 100;
}
break;
case 7: //KP
StearingKP = (StearingKP + 10);
if (StearingKP > 400) {
StearingKP = 400;
}
break;
case 8: //KI
StearingKI = (StearingKI + 10);
if (StearingKI > 400) {
StearingKI = 400;
}
break;
case 9: //KD
StearingKD = (StearingKD + 10);
if (StearingKD > 400) {
StearingKD = 400;
}
break;
case 10: //reverse
StearingReverse = (StearingReverse + 1);
if (StearingReverse > 1) {
StearingReverse = 1;
}
break;
case 11: //PWM
StearingPWM = (StearingPWM + 1);
if (StearingPWM > 255) {
StearingPWM = 255;
}
break;
case 12: //TZone
StearingTZone = (StearingTZone + 1);
if (StearingTZone > 12) {
StearingTZone = 12;
}
break;
}
delay(400);
RefreshDisplay();
}
// decrease(-) parameter value during setup
void setupMinus() {
switch (SetupParameter) {
case 2: //interval
StearingInterval = (StearingInterval - 100);
if (StearingInterval < 100) {
StearingInterval = 100;
}
break;
case 3: //min. to move
StearingMinToMove = (StearingMinToMove - 1);
if (StearingMinToMove < 0) {
StearingMinToMove = 0;
}
break;
case 4: //max. move
StearingMaxMove = (StearingMaxMove - 10);
if (StearingMaxMove < 10) {
StearingMaxMove = 10;
}
break;
case 5: //speed out
StearingSpeedOut = (StearingSpeedOut - 1);
if (StearingSpeedOut < 1) {
StearingSpeedOut = 1;
}
break;
case 6: //speed back
StearingSpeedBack = (StearingSpeedBack - 1);
if (StearingSpeedBack < 1) {
StearingSpeedBack = 1;
}
break;
case 7: //KP
StearingKP = (StearingKP - 10);
if (StearingKP < 0) {
StearingKP = 0;
}
break;
case 8: //KI
StearingKI = (StearingKI - 10);
if (StearingKI < 0) {
StearingKI = 0;
}
break;
case 9: //KD
StearingKD = (StearingKD - 10);
if (StearingKD < 0) {
StearingKD = 0;
}
break;
case 10: //reverse
StearingReverse = (StearingReverse - 1);
if (StearingReverse < 0) {
StearingReverse = 0;
}
break;
case 11: //PWM
StearingPWM = (StearingPWM - 1);
if (StearingPWM < 100) {
StearingPWM = 100;
}
break;
case 12: //TZone
StearingTZone = (StearingTZone - 1);
if (StearingTZone < -12) {
StearingTZone = -12;
}
break;
}
delay(400);
RefreshDisplay();
}
// motor control (+)=forward (-)=backwards
void gomotor(int stepsToMove) {
analogWrite(motorsABenablePin, StearingPWM); // on (PWM)
motor.step(stepsToMove);
analogWrite(motorsABenablePin, 0); // off
}
// refresh data on display
void RefreshDisplay() {
if (isSetup == false) { //---------normal
lcd.clear();
lcd.setCursor(0,0);
lcd.print("R"+nm_routetofollow);
lcd.write(0xDF);
lcd.print(" H"+nm_truecourse);
lcd.write(0xDF);
if (ispause == true) {
if (nm_validity=="V") { //not valid data, no sat fix
lcd.print(" sat?");
} else {
lcd.print(" STOP");
}
} else {
if (Stearing > 0) {
lcd.print(" +");
}
if (Stearing == 0) {
lcd.print(" ");
}
if (Stearing < 0) {
lcd.print(" ");
}
lcd.print(int(Stearing));
}
lcd.setCursor(0,1);
lcd.print(nm_time+" "+nm_knots);
}
if (isSetup == true) { //-----------setup
lcd.clear();
lcd.setCursor(0,0);
lcd.print("setup: ");
switch (SetupParameter) {
case 1: //display sensors
readMuxSensors();
lcd.print("V=");
lcd.print(SensorVBatt);
lcd.setCursor(1,1);
lcd.print("mA=");
lcd.print(int(SensormAmp));
lcd.print(" ");
lcd.print(int(SensorTemp));
lcd.write(0xDF);
lcd.print("C");
break;
case 2: //interval
lcd.print("interval");
lcd.setCursor(7,1);
lcd.print(StearingInterval);
lcd.print(" mSec");
break;
case 3: //min. to move
lcd.print("minimum");
lcd.setCursor(7,1);
lcd.print(StearingMinToMove);
lcd.write(0xDF);
break;
case 4: //max. move
lcd.print("max");
lcd.setCursor(7,1);
lcd.print(StearingMaxMove);
lcd.write(0xDF);
break;
case 5: //speed out
lcd.print("speed Out");
lcd.setCursor(7,1);
lcd.print(StearingSpeedOut);
break;
case 6: //speed back
lcd.print("speed Bk");
lcd.setCursor(7,1);
lcd.print(StearingSpeedBack);
break;
case 7: //KP
lcd.print("coeff.P");
lcd.setCursor(7,1);
lcd.print((float)StearingKP/100.0);
break;
case 8: //KI
lcd.print("coeff.I");
lcd.setCursor(7,1);
lcd.print((float)StearingKI/100.0);
break;
case 9: //KD
lcd.print("coeff.D");
lcd.setCursor(7,1);
lcd.print((float)StearingKD/100.0);
break;
case 10: //reverse
lcd.print("clockwise");
lcd.setCursor(7,1);
if (StearingReverse==0) {
lcd.print("Direct");
} else {
lcd.print("Reverse");
}
break;
case 11: //PWM
lcd.print("PWM");
lcd.setCursor(7,1);
lcd.print(StearingPWM);
break;
case 12: //TZone
lcd.print("TimeZone");
lcd.setCursor(7,1);
if (StearingTZone > 0) lcd.print("+");
lcd.print(StearingTZone);
break;
}
}
}
void serialEvent() { // it runs between each time loop(), multiple data may be available
while (Serial.available()) {
char inChar = (char)Serial.read();
inputString += inChar;
if (inChar == '\n') { // if NL then an NMEA sentence is complete
stringComplete = true;
}
}
}
String nmea0183_checksum(String nmea_data) { //calculate checksum of NMEA sentence
int crc = 0;
String chSumString = "";
int i;
// ignore the first $ sign, checksum in sentence
for (i = 1; i < (nmea_data.length()-5); i ++) { // remove the - 5 if no "*" + cksum + cr + lf are present
crc ^= nmea_data[i];
}
chSumString = String(crc,HEX);
if (chSumString.length()==1) {
chSumString="0"+chSumString.substring(0,1);
}
chSumString.toUpperCase();
return chSumString;
}
//check RC which button is pressed
int checkRfRC() {
int n = 0;
int res = 0;
n = analogRead(rfRemoteControlPin);
//Serial.println(n);
if ((n>350) and (n<460)) { // button A
res = RCleftbutton;
}
if ((n> 90) and (n<190)) { // button B
res = RCrightbutton;
}
if ((n>540) and (n<640)) { // button C
res = RCleft10button;
}
if ((n>225) and (n<325)) { // button D
res = RCright10button;
}
return res;
}
//check HW which button is pressed
int checkHWButtons() {
int n = 0;
int res = 0;
n = analogRead(ButtonsPin);
//Serial.println(n);
if ((n>90) and (n<190)) { // button pause 143
res = HWpausebutton;
}
if ((n>220) and (n<320)) { // button right 265
res = HWrightbutton;
}
if ((n>340) and (n<440)) { // button left 388
res = HWleftbutton;
}
if ((n>480) and (n<580)) { // button right+10 532
res = HWright10button;
}
if ((n>670) and (n<770)) { // button setup 724
res = HWsetupbutton;
}
if ((n>960) and (n<1060)) { // button left-10 1023
res = HWleft10button;
}
return res;
}
void gosetup() { // setup button
if (isSetup == false) {
SetupParameter = 1;
isSetup = true;
} else {
if (SetupParameter < 12) {
SetupParameter ++;
} else {
if (prev_StearingInterval != StearingInterval || prev_StearingMinToMove != StearingMinToMove || prev_StearingMaxMove != StearingMaxMove
|| prev_StearingSpeedOut != StearingSpeedOut || prev_StearingSpeedBack != StearingSpeedBack || prev_StearingKP != StearingKP
|| prev_StearingKI != StearingKI || prev_StearingKD != StearingKD || prev_StearingReverse != StearingReverse
|| prev_StearingPWM != StearingPWM || prev_StearingTZone != StearingTZone) {
lcd.clear();
lcd.setCursor(0,0);
lcd.print("updating... ");
delay(1000);
goupdateEEPROM();
if (EEerr) {
lcd.print("E=");
lcd.print(EEerr);
delay(1000);
}
prev_StearingInterval = StearingInterval;
prev_StearingMinToMove = StearingMinToMove;
prev_StearingMaxMove = StearingMaxMove;
prev_StearingSpeedOut = StearingSpeedOut;
prev_StearingSpeedBack = StearingSpeedBack;
prev_StearingKP = StearingKP;
prev_StearingKI = StearingKI;
prev_StearingKD = StearingKD;
prev_StearingReverse = StearingReverse;
prev_StearingPWM = StearingPWM;
prev_StearingTZone = StearingTZone;
}
isSetup = false;
}
}
NewTone (speakerPin,2000);
delay(400);
noNewTone();
RefreshDisplay();
}
void goupdateEEPROM() {
EEaddress = 0; //id1
EEdata[0] = EEid1;
EEbytedata = EEid1;
writeEEPROM (EEdisk, EEaddress, EEbytedata);
EEaddress = 1; //id2
EEdata[1] = EEid2;
EEbytedata = EEid2;
writeEEPROM (EEdisk, EEaddress, EEbytedata);
memcpy(bStearingInterval, &StearingInterval, sizeof(int));
memcpy(bStearingMinToMove, &StearingMinToMove, sizeof(int));
memcpy(bStearingMaxMove, &StearingMaxMove, sizeof(int));
memcpy(bStearingSpeedOut, &StearingSpeedOut, sizeof(int));
memcpy(bStearingSpeedBack, &StearingSpeedBack, sizeof(int));
memcpy(bStearingKP, &StearingKP, sizeof(int));
memcpy(bStearingKI, &StearingKI, sizeof(int));
memcpy(bStearingKD, &StearingKD, sizeof(int));
memcpy(bStearingReverse, &StearingReverse, sizeof(int));
memcpy(bStearingPWM, &StearingPWM, sizeof(int));
memcpy(bStearingTZone, &StearingTZone, sizeof(int));
memcpy(EEdata+2,bStearingInterval,sizeof(int));
memcpy(EEdata+4,bStearingMinToMove,sizeof(int));
memcpy(EEdata+6,bStearingMaxMove,sizeof(int));
memcpy(EEdata+8,bStearingSpeedOut,sizeof(int));
memcpy(EEdata+10,bStearingSpeedBack,sizeof(int));
memcpy(EEdata+12,bStearingKP,sizeof(int));
memcpy(EEdata+14,bStearingKI,sizeof(int));
memcpy(EEdata+16,bStearingKD,sizeof(int));
memcpy(EEdata+18,bStearingReverse,sizeof(int));
memcpy(EEdata+20,bStearingPWM,sizeof(int));
memcpy(EEdata+22,bStearingTZone,sizeof(int));
for (s = 2; s < EEbytes; s ++) {
EEaddress = s; //data
EEbytedata = EEdata[s];
writeEEPROM (EEdisk, EEaddress, EEbytedata);
}
}
void goleft() { // left button/RC
if (ispause == false) {
nmf_routetofollow --;
if (nmf_routetofollow < 1) {
nmf_routetofollow = 360;
}
d=nmf_routetofollow;
nmf_routetofollow=d;
nm_routetofollow=d;
NewTone (speakerPin,400);
delay(200);
noNewTone();
} else {
NewTone (speakerPin,1000);
delay(50);
noNewTone();
}
RefreshDisplay();
}
void goleft10() { // left 10x button/RC
if (ispause == false) {
for (s = 1; s < 11; s ++) {
nmf_routetofollow --;
if (nmf_routetofollow < 1) {
nmf_routetofollow = 360;
}
}
d=nmf_routetofollow;
nmf_routetofollow=d;
nm_routetofollow=d;
NewTone (speakerPin,400);
delay(200);
noNewTone();
} else {
NewTone (speakerPin,1000);
delay(50);
noNewTone();
}
RefreshDisplay();
}
void goright() { // right button/RC
if (ispause == false) {
nmf_routetofollow ++;
if (nmf_routetofollow > 360) {
nmf_routetofollow = 1;
}
d=nmf_routetofollow;
nmf_routetofollow=d;
nm_routetofollow=d;
NewTone (speakerPin,800);
delay(200);
noNewTone();
} else {
NewTone (speakerPin,1000);
delay(50);
noNewTone();
}
RefreshDisplay();
}
void goright10() { // right 10x button/RC
if (ispause == false) {
for (s = 1; s < 11; s ++) {
nmf_routetofollow ++;
if (nmf_routetofollow > 360) {
nmf_routetofollow = 1;
}
}
d=nmf_routetofollow;
nmf_routetofollow=d;
nm_routetofollow=d;
NewTone (speakerPin,800);
delay(200);
noNewTone();
} else {
NewTone (speakerPin,1000);
delay(50);
noNewTone();
}
RefreshDisplay();
}
void gopause() { // pause button/RC
if (ispause == true) {
ispause=false;
digitalWrite(ledpausePin, HIGH);
NewTone (speakerPin,50);
delay(200);
NewTone (speakerPin,200);
delay(800);
noNewTone();
} else {
ispause=true;
digitalWrite(ledpausePin, LOW);
NewTone (speakerPin,200);
delay(200);
NewTone (speakerPin,50);
delay(800);
noNewTone();
}
RefreshDisplay();
}
// reading eeprom
byte readEEPROM (int diskaddress, unsigned int memaddress) {
byte rdata = 0x00;
Wire.beginTransmission (diskaddress);
Wire.write (memaddress);
if (Wire.endTransmission () == 0) {
Wire.requestFrom (diskaddress,1);
if (Wire.available()) {
rdata = Wire.read();
} else {
EEerr = 1; //"READ no data available"
}
} else {
EEerr = 2; //"READ eTX error"
}
Wire.endTransmission (true);
return rdata;
}
// writing eeprom
void writeEEPROM (int diskaddress, unsigned int memaddress, byte bytedata) {
Wire.beginTransmission (diskaddress);
Wire.write (memaddress);
Wire.write (bytedata);
if (Wire.endTransmission () != 0) {
EEerr = 3; //"WRITING eTX error"
}
Wire.endTransmission (true);
delay(5);
}
// round zero decimal
float roundZeroDec(float f) {
float y, d;
y = f*1;
d = y - (int)y;
y = (float)(int)(f*1)/1;
if (d >= 0.5) {
y += 1;
} else {
if (d < -0.5) {
y -= 1;
}
}
return y;
}
// round one decimal
float roundOneDec(float f) {
float y, d;
y = f*10;
d = y - (int)y;
y = (float)(int)(f*10)/10;
if (d >= 0.5) {
y += 0.1;
} else {
if (d < -0.5) {
y -= 0.1;
}
}
return y;
}
// round two decimals
float roundTwoDec(float f) {
float y, d;
y = f*100;
d = y - (int)y;
y = (float)(int)(f*100)/100;
if (d >= 0.5) {
y += 0.01;
} else {
if (d < -0.5) {
y -= 0.01;
}
}
return y;
}
Comments
Please log in or sign up to comment.