Components and supplies
Arduino Nano R3
Project description
Code
Watchdog code (update the bootloader first)
arduino
1/* 2 * WATCHDOG2 3 * by Marco Zonca 2019-2022 4 * 5 * This sketch is a Watchdog to keep CLIENT under control, on Arduino NANO 3.0 6 * CLIENT must feed Watchdog sooner then feedingInterval otherwise will be forced to restart 7 * The V.2 includes internal WatchDog to check itself - It is necessary to update the Bootloader 8 * 9 */ 10 11#include <avr/wdt.h> 12 13const int testWDOGPin = 12; 14const int feedingPin = 14; 15const int ledPin = 15; 16const int restartPin = 16; 17const int buzzerPin = 17; 18const long ledInterval = 1000; 19const long feedingInterval = 5000; 20const long timeForClientStart = 4000; 21const long timeToRestart = 10000; 22 23int ledState = LOW; 24int previousFeedingState = LOW; 25int feedingState = LOW; 26unsigned long previousLedMillis = 0; 27unsigned long previousFeedingMillis = 0; 28unsigned long currentMillis = 0; 29 30void setup() { 31 pinMode(testWDOGPin,INPUT_PULLUP); 32 pinMode(ledPin, OUTPUT); 33 pinMode(buzzerPin, OUTPUT); 34 pinMode(restartPin, OUTPUT); 35 pinMode(feedingPin, INPUT); 36 digitalWrite(restartPin, HIGH); // LOW will force CLIENT to restart 37 delay(timeForClientStart); // gives some time to CLIENT to start... 38 currentMillis = millis(); 39 previousFeedingMillis = currentMillis; 40 previousLedMillis = currentMillis; 41 wdt_enable(WDTO_2S); // enable 2" WDog 42} 43 44void loop() { 45 currentMillis = millis(); 46 // BLINK LED ------------------- 47 if (currentMillis - previousLedMillis >= ledInterval) { 48 previousLedMillis = currentMillis; 49 if (ledState == LOW) { 50 ledState = HIGH; 51 } else { 52 ledState = LOW; 53 } 54 digitalWrite(ledPin, ledState); 55 } 56 // CHECK THE FEEDING ------------------- 57 feedingState = digitalRead(feedingPin); // CLIENT must set pin HIGH -> LOW frequently to prove it's alive 58 if (feedingState == HIGH) { 59 if (previousFeedingState == LOW) { 60 previousFeedingMillis = currentMillis; 61 } 62 previousFeedingState = HIGH; 63 } else { 64 previousFeedingState = LOW; 65 } 66 if (currentMillis - previousFeedingMillis > feedingInterval) { // CLIENT is sleeping 67 wdt_disable(); // temporary stop WDog 68 ledState = HIGH; 69 digitalWrite(ledPin, ledState); 70 tone(buzzerPin,1500); 71 delay(500); 72 digitalWrite(restartPin, LOW); //restart CLIENT 73 tone(buzzerPin,1300); 74 delay(500); 75 digitalWrite(restartPin, HIGH); 76 tone(buzzerPin,1100); 77 delay(timeToRestart); // let CLIENT time to restart... 78 noTone(buzzerPin); 79 currentMillis = millis(); 80 previousFeedingState = LOW; 81 previousFeedingMillis = currentMillis; 82 previousLedMillis = currentMillis; 83 wdt_enable(WDTO_2S); // enable again 2" WDog 84 } 85 if (digitalRead(testWDOGPin)==LOW) delay(10000); // test internal WDOG 86 wdt_reset(); // reset internal WDog 87}//loop() 88
Autopilot2 Arduino code
arduino
1 2// AUTOPILOT version 2 3// by Marco Zonca, 2019-2022 4/* 5 This sketch works as Autopilot for small sailing boats 6 Arduino Nano as CPU, Arduino Nano as watchdog, GPS BT-220 nmea, stepper motor + controller, rf315Mhz RC, 6 buttons, 7 buzzer, i2c display, 3xLEDS, i2c 24c04 eeprom, Mux 4051 for trasducers (sensors), lipo 2s 7.4v 2600mA, 8 7805 voltage regulator, Thermistor NTC, ADC MCP3424, etc.; 9*/ 10 11#include <LiquidCrystal_I2C.h> 12#include <NewTone.h> 13#include <Stepper.h> 14#include <Wire.h> 15#include <MCP342x.h> 16#include <PID_v2.h> 17 18String inputString = ""; 19String tzone = "00"; 20String nm_time = "00:00:00"; 21String nm_validity = "V"; 22String nm_latitude = "ddmm.mmmm'N"; 23String nm_longitude = "dddmm.mmmm'E"; 24String nm_knots = "0.0kn"; 25float nmf_knots = 0.0; 26String nm_truecourse = "360"; 27float nmf_truecourse = 360; 28String nm_date = "dd/mm/yyyy"; 29String nm_routetofollow = "000"; 30float nmf_routetofollow = 0; 31unsigned long previousStearingMillis = 0; 32unsigned long currentStearingMillis = 0; 33unsigned long prevCheckSensorsMillis = 0; 34unsigned long currCheckSensorsMillis = 0; 35int CheckSensorsInterval = 10000; 36 37bool stringComplete = false; 38bool isfirstfix = true; 39bool ispause = true; 40bool isStearing = false; 41bool isSetup = false; 42 43int s=0; 44int y=0; 45int z=0; 46int d=0; 47int rfRemoteControlValue = 0; 48int HWButtonValue = 0; 49int SetupParameter = 0; 50float calcmove = 0; 51float cm = 0; 52float Stearing = 0; 53float prevStearing = 0; 54float t = 0; 55int EEdisk = 0x50; 56int EEid1 = 0x29; 57int EEid2 = 0x00; 58uint8_t ADCaddr = 0x68; 59unsigned int EEaddress = 0; 60unsigned int EEbytes = 24; // eeprom nr of bytes to r/w, see also EEdata[ ] 61byte EEdata[24]; // put the same as EEbytes 62byte EEbytedata; 63int EEerr = 0; 64float SensorVBatt=0; 65float SensorVRes=0; 66float SensorTemp=0; 67float SensormAmp=0; 68 69// following parameters are the defaults but are read/write in eeprom 70// eeprom is initialized if at addresses 0 and 1 the content is different addres len type notes 71// 0-255 bytes at 0x50 EEdisk, 256-512 bytes at 0x51 (not used) --------------------------------------------------------------- 72// 0 1B byte 01001001 (0x29 as autopilot project id1) 73// 1 1B byte 00000000 (0x00 " " id2) 74int StearingInterval = 800; // millis between try and back 2 2B int StearingInterval 100-5000 step 100 75int StearingMinToMove = 4; // compass_degrees 4 2B int StearingMinToMove 1-20 step 1 76int StearingMaxMove = 90; // compass_degrees 6 2B int StearingMaxMove 10-360 step 10 77// 8 2B int StearingSpeedOut 1-100 step 1 78// 10 2B int StearingSpeedBack 1-100 step 1 79// 12 2B int StearingKP 0-400 step 10 80// 14 2B int StearingKI 0-400 step 10 81// 16 2B int StearingKD 0-400 step 10 82// 18 2B int StearingReverse 0-1 step 1 83// 20 2B int StearingPWM 100-255 step 1 84// 22 2B int StearingTZone -12-+12 step 1 85// 24 free 86int StearingSpeedOut = 40; // speed move out ("fast") steps/sec 87int StearingSpeedBack = 20; // speed move back ("slow") steps/sec 88int StearingKP = 150; // PID "P" 1.00 (cents) 89int StearingKI = 10; // PID "I" 0.20 (cents) 90int StearingKD = 10; // PID "D" 0.20 (cents) 91int StearingReverse = 0; // 0=false 1=true (clockwise or not) 92int StearingPWM = 245; // PWM on enable motor pins 93int StearingTZone = 0; // GMT=0 Italy is +1 or +2 94 95byte bStearingInterval[sizeof(int)]; 96byte bStearingMinToMove[sizeof(int)]; 97byte bStearingMaxMove[sizeof(int)]; 98byte bStearingSpeedOut[sizeof(int)]; 99byte bStearingSpeedBack[sizeof(int)]; 100byte bStearingKP[sizeof(int)]; 101byte bStearingKI[sizeof(int)]; 102byte bStearingKD[sizeof(int)]; 103byte bStearingReverse[sizeof(int)]; 104byte bStearingPWM[sizeof(int)]; 105byte bStearingTZone[sizeof(int)]; 106 107int prev_StearingInterval=0; 108int prev_StearingMinToMove=0; 109int prev_StearingMaxMove=0; 110int prev_StearingSpeedOut=0; 111int prev_StearingSpeedBack=0; 112int prev_StearingKP=0; 113int prev_StearingKI=0; 114int prev_StearingKD=0; 115int prev_StearingReverse=0; 116int prev_StearingPWM=0; 117int prev_StearingTZone=0; 118 119const int ledpausePin = 2; 120const int watchDogPin = 3; 121const int MuxSelBit0Pin = 8; // 000=Vin 001=Vbatt 010=Temp 122const int MuxSelBit1Pin = 7; // 123const int MuxSelBit2Pin = 6; // 124const int motorsABenablePin = 5; // PWM 125const int MuxIOPin = 14; 126const int ButtonsPin = 15; 127const int rfRemoteControlPin = 16; 128const int speakerPin = 17; 129const int RCleftbutton = 201; 130const int RCrightbutton = 202; 131const int RCleft10button = 203; 132const int RCright10button = 204; 133const int HWleftbutton = 101; 134const int HWrightbutton = 102; 135const int HWpausebutton = 103; 136const int HWsetupbutton = 104; 137const int HWleft10button = 105; 138const int HWright10button = 106; 139const int motorStepsPerRevolution = 216; // 216 steps for model 23LM, 1.8 per step, 54 steps = 1/4 of revolution 140 141LiquidCrystal_I2C lcd (0x27, 2, 1, 0, 4, 5, 6, 7, 3, POSITIVE); 142Stepper motor(motorStepsPerRevolution, 9, 10, 11, 12); 143MCP342x adc = MCP342x(ADCaddr); 144PID_v2 myPID((float)(StearingKP/100.0),(float)(StearingKI/100.0),(float)(StearingKD/100.0), PID::Direct); // P, I, D, d/r 145 146void setup() { 147 Serial.begin(4800); 148 lcd.begin(16,2); 149 Wire.begin(); 150 inputString.reserve(200); 151 pinMode(motorsABenablePin, OUTPUT); 152 pinMode(MuxSelBit0Pin, OUTPUT); 153 pinMode(MuxSelBit1Pin, OUTPUT); 154 pinMode(MuxSelBit2Pin, OUTPUT); 155 digitalWrite(motorsABenablePin, LOW); 156 digitalWrite(MuxSelBit0Pin, LOW); 157 digitalWrite(MuxSelBit1Pin, LOW); 158 digitalWrite(MuxSelBit2Pin, LOW); 159 pinMode(ledpausePin, OUTPUT); 160 pinMode(watchDogPin, OUTPUT); 161 digitalWrite(ledpausePin, LOW); 162 digitalWrite(watchDogPin, LOW); 163 164 MCP342x::generalCallReset(); 165 delay(1); // MC342x needs 300us to settle, wait 1ms 166 167 // read+check EEPROM (formatting (initializing) if new or not identified) 168 lcd.clear(); 169 lcd.setCursor(0,0); 170 lcd.print("Memory check..."); 171 lcd.setCursor(0,1); 172 for (s = 0; s < EEbytes; s ++) { 173 EEaddress = s; 174 EEbytedata = readEEPROM (EEdisk, EEaddress); 175 EEdata[s] = EEbytedata; 176 } 177 if (EEerr) { 178 lcd.print("E="); 179 lcd.print(EEerr); 180 delay(5000); 181 } 182 if ((EEdata[0] != EEid1) || (EEdata[1] != EEid2)) { 183 lcd.print(" init! "); 184 goupdateEEPROM(); 185 if (EEerr) { 186 lcd.print("E="); 187 lcd.print(EEerr); 188 delay(5000); 189 } 190 } 191 memcpy(bStearingInterval, EEdata+2, sizeof(int)); 192 memcpy(bStearingMinToMove, EEdata+4, sizeof(int)); 193 memcpy(bStearingMaxMove, EEdata+6, sizeof(int)); 194 memcpy(bStearingSpeedOut, EEdata+8, sizeof(int)); 195 memcpy(bStearingSpeedBack, EEdata+10, sizeof(int)); 196 memcpy(bStearingKP, EEdata+12, sizeof(int)); 197 memcpy(bStearingKI, EEdata+14, sizeof(int)); 198 memcpy(bStearingKD, EEdata+16, sizeof(int)); 199 memcpy(bStearingReverse, EEdata+18, sizeof(int)); 200 memcpy(bStearingPWM, EEdata+20, sizeof(int)); 201 memcpy(bStearingTZone, EEdata+22, sizeof(int)); 202 203 StearingInterval = *((int*) bStearingInterval); 204 StearingMinToMove = *((int*) bStearingMinToMove); 205 StearingMaxMove = *((int*) bStearingMaxMove); 206 StearingSpeedOut = *((int*) bStearingSpeedOut); 207 StearingSpeedBack = *((int*) bStearingSpeedBack); 208 StearingKP = *((int*) bStearingKP); 209 StearingKI = *((int*) bStearingKI); 210 StearingKD = *((int*) bStearingKD); 211 StearingReverse = *((int*) bStearingReverse); 212 StearingPWM = *((int*) bStearingPWM); 213 StearingTZone = *((int*) bStearingTZone); 214 215 prev_StearingInterval = StearingInterval; 216 prev_StearingMinToMove = StearingMinToMove; 217 prev_StearingMaxMove = StearingMaxMove; 218 prev_StearingSpeedOut = StearingSpeedOut; 219 prev_StearingSpeedBack = StearingSpeedBack; 220 prev_StearingKP = StearingKP; 221 prev_StearingKI = StearingKI; 222 prev_StearingKD = StearingKD; 223 prev_StearingReverse = StearingReverse; 224 prev_StearingPWM = StearingPWM; 225 prev_StearingTZone = StearingTZone; 226 227 myPID.SetOutputLimits((StearingMaxMove*(-1)), StearingMaxMove); 228 myPID.Start(0, 0, 0); // input, output, setpoint (direction, correction, target) 229 myPID.SetSampleTime(StearingInterval); 230 231 lcd.print(" OK"); 232 delay(1000); 233 lcd.clear(); 234 lcd.print("GPS reading..."); 235 delay(1000); 236} 237 238void loop() { 239 // CHECK SENSORS ----------------- 240 currCheckSensorsMillis = millis(); 241 if (currCheckSensorsMillis - prevCheckSensorsMillis >= CheckSensorsInterval) { 242 readMuxSensors(); 243 if ((SensorVBatt <= 6.8) || (SensorTemp >= 60)) { 244 lcd.clear(); 245 lcd.setCursor(0,0); 246 lcd.print("Alarm sensors! "); 247 lcd.setCursor(1,1); 248 lcd.print("V="); 249 lcd.print(SensorVBatt); 250 lcd.print(" "); 251 lcd.print(int(SensorTemp)); 252 lcd.write(0xDF); 253 lcd.print("C"); 254 NewTone (speakerPin,10); 255 delay(1000); 256 noNewTone(); 257 } 258 prevCheckSensorsMillis = currCheckSensorsMillis; 259 } 260 // STEARING CONTROL ---------------- 261 currentStearingMillis = millis(); 262 if (currentStearingMillis - previousStearingMillis >= StearingInterval) { 263 if (isStearing == false && ispause == false) { 264 myPID.SetOutputLimits((StearingMaxMove*(-1)), StearingMaxMove); 265 myPID.SetSampleTime(StearingInterval); 266 calcmove = nmf_routetofollow - nmf_truecourse; 267 if (calcmove < (-180)) { 268 calcmove = calcmove + 360; 269 } else { 270 if (calcmove > (+180)) { 271 calcmove = calcmove - 360; 272 } 273 } 274 if (StearingReverse==1) { 275 calcmove = (calcmove * -1); 276 } 277 if (abs(calcmove) >= StearingMinToMove) { // go try (move stearing) 278 myPID.SetTunings((float)(StearingKP/100.0),(float)(StearingKI/100.0),(float)(StearingKD/100.0)); 279 Stearing = myPID.Run(calcmove); // calc PID correction 280 motor.setSpeed(StearingSpeedOut); 281 gomotor(int((Stearing * 216) / 360)); // 54 steps = 1/4 of revolution (90), 216 = 1 revolution (360) 282 prevStearing = Stearing; 283 isStearing = true; 284 } 285 } else { // go back (move stearing to "zero" position) 286 if (isStearing == true) { 287 Stearing = (prevStearing * -1); 288 motor.setSpeed(StearingSpeedBack); 289 gomotor(int((Stearing * 216) / 360)); // 54 steps = 1/4 of revolution (90), 216 = 1 revolution (360) 290 Stearing = 0; 291 prevStearing = 0; 292 isStearing = false; 293 } 294 } 295 previousStearingMillis = currentStearingMillis; 296 } 297 // RC RF BUTTONS ------------------ 298 rfRemoteControlValue = checkRfRC(); 299 if (rfRemoteControlValue) { 300 switch (rfRemoteControlValue) { 301 case RCleftbutton: // Left -1 RC button 302 goleft(); 303 break; 304 case RCrightbutton: // Right +1 RC button 305 goright(); 306 break; 307 case RCleft10button: // Left-10 RC button 308 goleft10(); 309 break; 310 case RCright10button: // Right+10 RC button 311 goright10(); 312 break; 313 } 314 } 315 // BUTTONS ------------------------ 316 HWButtonValue = checkHWButtons(); 317 if (HWButtonValue) { 318 switch (HWButtonValue) { 319 case HWleftbutton: // Left(-1) HW button 320 if (isSetup == false) { 321 goleft(); 322 } else { 323 setupMinus(); 324 } 325 break; 326 case HWrightbutton: // Right(+1) HW button 327 if (isSetup == false) { 328 goright(); 329 } else { 330 setupPlus(); 331 } 332 break; 333 case HWpausebutton: // Pause HW button 334 gopause(); 335 break; 336 case HWsetupbutton: // Setup HW button 337 gosetup(); 338 break; 339 case HWleft10button: // Left(-10) HW button 340 goleft10(); 341 break; 342 case HWright10button: // Right(+10) HW button 343 goright10(); 344 break; 345 } 346 } 347 // GPS NMEA ------------------ 348 if (stringComplete == true) { // received nmea sentence by serial port RX 349 bool ret; 350 ret = nmeaExtractData(); 351 inputString = ""; 352 stringComplete = false; 353 if (ret == true) { 354 RefreshDisplay(); 355 } 356 } 357 // WATCHDOG FEEDING ---------------- 358 if (digitalRead(watchDogPin) == LOW) { 359 digitalWrite(watchDogPin, HIGH); 360 } else { 361 digitalWrite(watchDogPin, LOW); 362 } 363} 364 365// read sensors, trasducers on multiplexer, ADC 366void readMuxSensors() { 367 uint8_t err = 0; 368 int x = 0; 369 long ADCval = 0; 370 float Fmem = 0; 371 float Vo = 0; 372 float n = 0; 373 float n1 = 0; 374 float n2 = 0; 375 float corr = 0; 376 float logR2 = 0; 377 float R2 = 0; 378 float T = 0; 379 float R1 = 100000; // 100k resistor in NTC voltage divider 380 float c1 = 6.66082410500E-004; // Steinhart-Hart coeff. 1 for NTC 381 float c2 = 2.23928204100E-004; // Steinhart-Hart coeff. 2 for NTC 382 float c3 = 7.19951882000E-008; // Steinhart-Hart coeff. 3 for NTC 383 384 digitalWrite(MuxSelBit0Pin, LOW); // 000=Vbatt 385 digitalWrite(MuxSelBit1Pin, LOW); 386 digitalWrite(MuxSelBit2Pin, LOW); 387 Fmem=0; 388 for (x=1;x<=33;x++) { 389 n = analogRead(MuxIOPin); 390 n1=(((10.00 * n) / 1023.00)); 391 Fmem=Fmem+n1; 392 } 393 n1=(Fmem/(x-1)); 394 n2=(n1 + ((n1 * 1.15) /100)); // arbitrary correction (not active = 0.0%) 395 SensorVBatt=roundTwoDec(n2); 396 397 digitalWrite(MuxSelBit0Pin, HIGH); // 001=Vres 398 digitalWrite(MuxSelBit1Pin, LOW); 399 digitalWrite(MuxSelBit2Pin, LOW); 400 Fmem=0; 401 for (x=1;x<=33;x++) { 402 n = analogRead(MuxIOPin); 403 n1=(((10.00 * n) / 1023.00)); 404 Fmem=Fmem+n1; 405 } 406 n1=(Fmem/(x-1)); 407 n2=(n1 + ((n1 * 1.15) /100)); // arbitrary correction (not active = 0.0%) 408 SensorVRes=roundTwoDec(n2); 409 410 digitalWrite(MuxSelBit0Pin, LOW); // 010=NTC Temp 411 digitalWrite(MuxSelBit1Pin, HIGH); 412 digitalWrite(MuxSelBit2Pin, LOW); 413 Fmem=0; 414 for (x=1;x<=33;x++) { 415 Vo = analogRead(MuxIOPin); 416 R2 = R1 * (1023.0 / Vo - 1.0); // Steinhart-Hart Temperature Calc. 417 logR2 = log(R2); 418 T = (1.0 / (c1 + c2 * logR2 + c3 * logR2 * logR2 * logR2)); 419 n1 = T - 273.15; // Celsius 420 Fmem=Fmem+n1; 421 } 422 n2=(Fmem/(x-1)); 423 SensorTemp=roundZeroDec(n2); 424 425 MCP342x::Config status; // differential ADC 426 err = adc.convertAndRead(MCP342x::channel1,MCP342x::oneShot,MCP342x::resolution12,MCP342x::gain2,1000000,ADCval,status); 427 n=(ADCval / 1.0); // (ADCgain=2 / Vdivider=2) = 1.0 428 n1=(n + ((n * 0.0) /100)); // arbitrary correction (not active = 0.0%) 429 n2=(n1/0.22); // n1=VADC, 0.22ohm = shunt resistor, mA 430 SensormAmp=roundZeroDec(n2); 431} 432 433// extract data from nmea inputString 434bool nmeaExtractData() { 435 bool ret = false; //true if nmea sentence = $GNRMC and valid CHKSUM 436 if ((inputString.substring(0,6) == "$GNRMC") && (inputString.substring(inputString.length()-4,inputString.length()-2) == nmea0183_checksum(inputString))) { 437 y=0; 438 for (s = 1; s < 11; s ++) { 439 y=inputString.indexOf(",",y); 440 switch (s) { 441 case 1: //time 442 z=inputString.indexOf(",",y+1); 443 if (z>(y+1)) { 444 t=inputString.substring(y+1,y+2+1).toFloat(); 445 t=t+StearingTZone; 446 if (t>23) { 447 t=t-24; 448 } 449 if (t<0) { 450 t=t+24; 451 } 452 tzone="0"+String(t,0); 453 nm_time=tzone.substring(tzone.length()-2)+":"+inputString.substring(y+1+2,y+4+1)+":"+inputString.substring(y+1+4,y+6+1); 454 } 455 y=z; 456 break; 457 case 2: //validity 458 z=inputString.indexOf(",",y+1); 459 if (z>(y+1)) { 460 nm_validity=inputString.substring(y+1,y+1+1); 461 } 462 y=z; 463 break; 464 case 3: //latitude 465 z=inputString.indexOf(",",y+1); 466 if (z>(y+1)) { 467 nm_latitude=inputString.substring(y+1,y+2+1)+""+inputString.substring(y+1+2,y+10+1)+"'"; 468 } 469 y=z; 470 break; 471 case 4: //north/south 472 z=inputString.indexOf(",",y+1); 473 if (z>(y+1)) { 474 nm_latitude=nm_latitude + inputString.substring(y+1,y+1+1); 475 } 476 y=z; 477 break; 478 case 5: //longitude 479 z=inputString.indexOf(",",y+1); 480 if (z>(y+1)) { 481 nm_longitude=inputString.substring(y+1,y+3+1)+""+inputString.substring(y+1+3,y+11+1)+"'"; 482 } 483 y=z; 484 break; 485 case 6: //east/west 486 z=inputString.indexOf(",",y+1); 487 if (z>(y+1)) { 488 nm_longitude=nm_longitude + inputString.substring(y+1,y+1+1); 489 } 490 y=z; 491 break; 492 case 7: //speed knots 493 z=inputString.indexOf(",",y+1); 494 if (z>(y+1)) { 495 nmf_knots=inputString.substring(y+1,z).toFloat(); 496 t=roundOneDec(nmf_knots); 497 nm_knots=String(t,1)+"kn"; 498 } 499 y=z; 500 break; 501 case 8: //true course 502 z=inputString.indexOf(",",y+1); 503 if (z>(y+1)) { 504 nmf_truecourse=inputString.substring(y+1,z).toFloat(); 505 d=nmf_truecourse; 506 nm_truecourse=d; 507 } 508 y=z; 509 break; 510 case 9: //date 511 z=inputString.indexOf(",",y+1); 512 if (z>(y+1)) { 513 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); 514 } 515 y=z; 516 break; 517 case 10: 518 // statements 519 break; 520 default: 521 // statements 522 break; 523 } 524 } 525 if ((isfirstfix == true) || (ispause == true)) { 526 nm_routetofollow=nm_truecourse; 527 nmf_routetofollow=nmf_truecourse; 528 isfirstfix=false; 529 } 530 ret=true; 531 } 532 return ret; 533} 534 535// increase(+) parameter value during setup 536void setupPlus() { 537 switch (SetupParameter) { 538 case 2: //interval 539 StearingInterval = (StearingInterval + 100); 540 if (StearingInterval > 5000) { 541 StearingInterval = 5000; 542 } 543 break; 544 case 3: //min. to move 545 StearingMinToMove = (StearingMinToMove + 1); 546 if (StearingMinToMove > 20) { 547 StearingMinToMove = 20; 548 } 549 break; 550 case 4: //max. move 551 StearingMaxMove = (StearingMaxMove + 10); 552 if (StearingMaxMove > 360) { 553 StearingMaxMove = 360; 554 } 555 break; 556 case 5: //speed out 557 StearingSpeedOut = (StearingSpeedOut + 1); 558 if (StearingSpeedOut > 100) { 559 StearingSpeedOut = 100; 560 } 561 break; 562 case 6: //speed back 563 StearingSpeedBack = (StearingSpeedBack + 1); 564 if (StearingSpeedBack > 100) { 565 StearingSpeedBack = 100; 566 } 567 break; 568 case 7: //KP 569 StearingKP = (StearingKP + 10); 570 if (StearingKP > 400) { 571 StearingKP = 400; 572 } 573 break; 574 case 8: //KI 575 StearingKI = (StearingKI + 10); 576 if (StearingKI > 400) { 577 StearingKI = 400; 578 } 579 break; 580 case 9: //KD 581 StearingKD = (StearingKD + 10); 582 if (StearingKD > 400) { 583 StearingKD = 400; 584 } 585 break; 586 case 10: //reverse 587 StearingReverse = (StearingReverse + 1); 588 if (StearingReverse > 1) { 589 StearingReverse = 1; 590 } 591 break; 592 case 11: //PWM 593 StearingPWM = (StearingPWM + 1); 594 if (StearingPWM > 255) { 595 StearingPWM = 255; 596 } 597 break; 598 case 12: //TZone 599 StearingTZone = (StearingTZone + 1); 600 if (StearingTZone > 12) { 601 StearingTZone = 12; 602 } 603 break; 604 } 605 delay(400); 606 RefreshDisplay(); 607} 608 609// decrease(-) parameter value during setup 610void setupMinus() { 611 switch (SetupParameter) { 612 case 2: //interval 613 StearingInterval = (StearingInterval - 100); 614 if (StearingInterval < 100) { 615 StearingInterval = 100; 616 } 617 break; 618 case 3: //min. to move 619 StearingMinToMove = (StearingMinToMove - 1); 620 if (StearingMinToMove < 0) { 621 StearingMinToMove = 0; 622 } 623 break; 624 case 4: //max. move 625 StearingMaxMove = (StearingMaxMove - 10); 626 if (StearingMaxMove < 10) { 627 StearingMaxMove = 10; 628 } 629 break; 630 case 5: //speed out 631 StearingSpeedOut = (StearingSpeedOut - 1); 632 if (StearingSpeedOut < 1) { 633 StearingSpeedOut = 1; 634 } 635 break; 636 case 6: //speed back 637 StearingSpeedBack = (StearingSpeedBack - 1); 638 if (StearingSpeedBack < 1) { 639 StearingSpeedBack = 1; 640 } 641 break; 642 case 7: //KP 643 StearingKP = (StearingKP - 10); 644 if (StearingKP < 0) { 645 StearingKP = 0; 646 } 647 break; 648 case 8: //KI 649 StearingKI = (StearingKI - 10); 650 if (StearingKI < 0) { 651 StearingKI = 0; 652 } 653 break; 654 case 9: //KD 655 StearingKD = (StearingKD - 10); 656 if (StearingKD < 0) { 657 StearingKD = 0; 658 } 659 break; 660 case 10: //reverse 661 StearingReverse = (StearingReverse - 1); 662 if (StearingReverse < 0) { 663 StearingReverse = 0; 664 } 665 break; 666 case 11: //PWM 667 StearingPWM = (StearingPWM - 1); 668 if (StearingPWM < 100) { 669 StearingPWM = 100; 670 } 671 break; 672 case 12: //TZone 673 StearingTZone = (StearingTZone - 1); 674 if (StearingTZone < -12) { 675 StearingTZone = -12; 676 } 677 break; 678 } 679 delay(400); 680 RefreshDisplay(); 681} 682 683// motor control (+)=forward (-)=backwards 684void gomotor(int stepsToMove) { 685 analogWrite(motorsABenablePin, StearingPWM); // on (PWM) 686 motor.step(stepsToMove); 687 analogWrite(motorsABenablePin, 0); // off 688} 689 690// refresh data on display 691void RefreshDisplay() { 692 if (isSetup == false) { //---------normal 693 lcd.clear(); 694 lcd.setCursor(0,0); 695 lcd.print("R"+nm_routetofollow); 696 lcd.write(0xDF); 697 lcd.print(" H"+nm_truecourse); 698 lcd.write(0xDF); 699 if (ispause == true) { 700 if (nm_validity=="V") { //not valid data, no sat fix 701 lcd.print(" sat?"); 702 } else { 703 lcd.print(" STOP"); 704 } 705 } else { 706 if (Stearing > 0) { 707 lcd.print(" +"); 708 } 709 if (Stearing == 0) { 710 lcd.print(" "); 711 } 712 if (Stearing < 0) { 713 lcd.print(" "); 714 } 715 lcd.print(int(Stearing)); 716 } 717 lcd.setCursor(0,1); 718 lcd.print(nm_time+" "+nm_knots); 719 } 720 if (isSetup == true) { //-----------setup 721 lcd.clear(); 722 lcd.setCursor(0,0); 723 lcd.print("setup: "); 724 switch (SetupParameter) { 725 case 1: //display sensors 726 readMuxSensors(); 727 lcd.print("V="); 728 lcd.print(SensorVBatt); 729 lcd.setCursor(1,1); 730 lcd.print("mA="); 731 lcd.print(int(SensormAmp)); 732 lcd.print(" "); 733 lcd.print(int(SensorTemp)); 734 lcd.write(0xDF); 735 lcd.print("C"); 736 break; 737 case 2: //interval 738 lcd.print("interval"); 739 lcd.setCursor(7,1); 740 lcd.print(StearingInterval); 741 lcd.print(" mSec"); 742 break; 743 case 3: //min. to move 744 lcd.print("minimum"); 745 lcd.setCursor(7,1); 746 lcd.print(StearingMinToMove); 747 lcd.write(0xDF); 748 break; 749 case 4: //max. move 750 lcd.print("max"); 751 lcd.setCursor(7,1); 752 lcd.print(StearingMaxMove); 753 lcd.write(0xDF); 754 break; 755 case 5: //speed out 756 lcd.print("speed Out"); 757 lcd.setCursor(7,1); 758 lcd.print(StearingSpeedOut); 759 break; 760 case 6: //speed back 761 lcd.print("speed Bk"); 762 lcd.setCursor(7,1); 763 lcd.print(StearingSpeedBack); 764 break; 765 case 7: //KP 766 lcd.print("coeff.P"); 767 lcd.setCursor(7,1); 768 lcd.print((float)StearingKP/100.0); 769 break; 770 case 8: //KI 771 lcd.print("coeff.I"); 772 lcd.setCursor(7,1); 773 lcd.print((float)StearingKI/100.0); 774 break; 775 case 9: //KD 776 lcd.print("coeff.D"); 777 lcd.setCursor(7,1); 778 lcd.print((float)StearingKD/100.0); 779 break; 780 case 10: //reverse 781 lcd.print("clockwise"); 782 lcd.setCursor(7,1); 783 if (StearingReverse==0) { 784 lcd.print("Direct"); 785 } else { 786 lcd.print("Reverse"); 787 } 788 break; 789 case 11: //PWM 790 lcd.print("PWM"); 791 lcd.setCursor(7,1); 792 lcd.print(StearingPWM); 793 break; 794 case 12: //TZone 795 lcd.print("TimeZone"); 796 lcd.setCursor(7,1); 797 if (StearingTZone > 0) lcd.print("+"); 798 lcd.print(StearingTZone); 799 break; 800 } 801 } 802} 803 804void serialEvent() { // it runs between each time loop(), multiple data may be available 805 while (Serial.available()) { 806 char inChar = (char)Serial.read(); 807 inputString += inChar; 808 if (inChar == '\n') { // if NL then an NMEA sentence is complete 809 stringComplete = true; 810 } 811 } 812 } 813 814String nmea0183_checksum(String nmea_data) { //calculate checksum of NMEA sentence 815 int crc = 0; 816 String chSumString = ""; 817 int i; 818 // ignore the first $ sign, checksum in sentence 819 for (i = 1; i < (nmea_data.length()-5); i ++) { // remove the - 5 if no "*" + cksum + cr + lf are present 820 crc ^= nmea_data[i]; 821 } 822 chSumString = String(crc,HEX); 823 if (chSumString.length()==1) { 824 chSumString="0"+chSumString.substring(0,1); 825 } 826 chSumString.toUpperCase(); 827 return chSumString; 828} 829 830//check RC which button is pressed 831int checkRfRC() { 832 int n = 0; 833 int res = 0; 834 n = analogRead(rfRemoteControlPin); 835 //Serial.println(n); 836 if ((n>350) and (n<460)) { // button A 837 res = RCleftbutton; 838 } 839 if ((n> 90) and (n<190)) { // button B 840 res = RCrightbutton; 841 } 842 if ((n>540) and (n<640)) { // button C 843 res = RCleft10button; 844 } 845 if ((n>225) and (n<325)) { // button D 846 res = RCright10button; 847 } 848 return res; 849} 850 851//check HW which button is pressed 852int checkHWButtons() { 853 int n = 0; 854 int res = 0; 855 n = analogRead(ButtonsPin); 856 //Serial.println(n); 857 if ((n>90) and (n<190)) { // button pause 143 858 res = HWpausebutton; 859 } 860 if ((n>220) and (n<320)) { // button right 265 861 res = HWrightbutton; 862 } 863 if ((n>340) and (n<440)) { // button left 388 864 res = HWleftbutton; 865 } 866 if ((n>480) and (n<580)) { // button right+10 532 867 res = HWright10button; 868 } 869 if ((n>670) and (n<770)) { // button setup 724 870 res = HWsetupbutton; 871 } 872 if ((n>960) and (n<1060)) { // button left-10 1023 873 res = HWleft10button; 874 } 875 return res; 876} 877 878void gosetup() { // setup button 879 if (isSetup == false) { 880 SetupParameter = 1; 881 isSetup = true; 882 } else { 883 if (SetupParameter < 12) { 884 SetupParameter ++; 885 } else { 886 if (prev_StearingInterval != StearingInterval || prev_StearingMinToMove != StearingMinToMove || prev_StearingMaxMove != StearingMaxMove 887 || prev_StearingSpeedOut != StearingSpeedOut || prev_StearingSpeedBack != StearingSpeedBack || prev_StearingKP != StearingKP 888 || prev_StearingKI != StearingKI || prev_StearingKD != StearingKD || prev_StearingReverse != StearingReverse 889 || prev_StearingPWM != StearingPWM || prev_StearingTZone != StearingTZone) { 890 lcd.clear(); 891 lcd.setCursor(0,0); 892 lcd.print("updating... "); 893 delay(1000); 894 goupdateEEPROM(); 895 if (EEerr) { 896 lcd.print("E="); 897 lcd.print(EEerr); 898 delay(1000); 899 } 900 prev_StearingInterval = StearingInterval; 901 prev_StearingMinToMove = StearingMinToMove; 902 prev_StearingMaxMove = StearingMaxMove; 903 prev_StearingSpeedOut = StearingSpeedOut; 904 prev_StearingSpeedBack = StearingSpeedBack; 905 prev_StearingKP = StearingKP; 906 prev_StearingKI = StearingKI; 907 prev_StearingKD = StearingKD; 908 prev_StearingReverse = StearingReverse; 909 prev_StearingPWM = StearingPWM; 910 prev_StearingTZone = StearingTZone; 911 } 912 isSetup = false; 913 } 914 } 915 NewTone (speakerPin,2000); 916 delay(400); 917 noNewTone(); 918 RefreshDisplay(); 919} 920 921void goupdateEEPROM() { 922 EEaddress = 0; //id1 923 EEdata[0] = EEid1; 924 EEbytedata = EEid1; 925 writeEEPROM (EEdisk, EEaddress, EEbytedata); 926 EEaddress = 1; //id2 927 EEdata[1] = EEid2; 928 EEbytedata = EEid2; 929 writeEEPROM (EEdisk, EEaddress, EEbytedata); 930 memcpy(bStearingInterval, &StearingInterval, sizeof(int)); 931 memcpy(bStearingMinToMove, &StearingMinToMove, sizeof(int)); 932 memcpy(bStearingMaxMove, &StearingMaxMove, sizeof(int)); 933 memcpy(bStearingSpeedOut, &StearingSpeedOut, sizeof(int)); 934 memcpy(bStearingSpeedBack, &StearingSpeedBack, sizeof(int)); 935 memcpy(bStearingKP, &StearingKP, sizeof(int)); 936 memcpy(bStearingKI, &StearingKI, sizeof(int)); 937 memcpy(bStearingKD, &StearingKD, sizeof(int)); 938 memcpy(bStearingReverse, &StearingReverse, sizeof(int)); 939 memcpy(bStearingPWM, &StearingPWM, sizeof(int)); 940 memcpy(bStearingTZone, &StearingTZone, sizeof(int)); 941 942 memcpy(EEdata+2,bStearingInterval,sizeof(int)); 943 memcpy(EEdata+4,bStearingMinToMove,sizeof(int)); 944 memcpy(EEdata+6,bStearingMaxMove,sizeof(int)); 945 memcpy(EEdata+8,bStearingSpeedOut,sizeof(int)); 946 memcpy(EEdata+10,bStearingSpeedBack,sizeof(int)); 947 memcpy(EEdata+12,bStearingKP,sizeof(int)); 948 memcpy(EEdata+14,bStearingKI,sizeof(int)); 949 memcpy(EEdata+16,bStearingKD,sizeof(int)); 950 memcpy(EEdata+18,bStearingReverse,sizeof(int)); 951 memcpy(EEdata+20,bStearingPWM,sizeof(int)); 952 memcpy(EEdata+22,bStearingTZone,sizeof(int)); 953 954 for (s = 2; s < EEbytes; s ++) { 955 EEaddress = s; //data 956 EEbytedata = EEdata[s]; 957 writeEEPROM (EEdisk, EEaddress, EEbytedata); 958 } 959} 960 961void goleft() { // left button/RC 962 if (ispause == false) { 963 nmf_routetofollow --; 964 if (nmf_routetofollow < 1) { 965 nmf_routetofollow = 360; 966 } 967 d=nmf_routetofollow; 968 nmf_routetofollow=d; 969 nm_routetofollow=d; 970 NewTone (speakerPin,400); 971 delay(200); 972 noNewTone(); 973 } else { 974 NewTone (speakerPin,1000); 975 delay(50); 976 noNewTone(); 977 } 978 RefreshDisplay(); 979} 980 981void goleft10() { // left 10x button/RC 982 if (ispause == false) { 983 for (s = 1; s < 11; s ++) { 984 nmf_routetofollow --; 985 if (nmf_routetofollow < 1) { 986 nmf_routetofollow = 360; 987 } 988 } 989 d=nmf_routetofollow; 990 nmf_routetofollow=d; 991 nm_routetofollow=d; 992 NewTone (speakerPin,400); 993 delay(200); 994 noNewTone(); 995 } else { 996 NewTone (speakerPin,1000); 997 delay(50); 998 noNewTone(); 999 } 1000 RefreshDisplay(); 1001} 1002 1003void goright() { // right button/RC 1004 if (ispause == false) { 1005 nmf_routetofollow ++; 1006 if (nmf_routetofollow > 360) { 1007 nmf_routetofollow = 1; 1008 } 1009 d=nmf_routetofollow; 1010 nmf_routetofollow=d; 1011 nm_routetofollow=d; 1012 NewTone (speakerPin,800); 1013 delay(200); 1014 noNewTone(); 1015 } else { 1016 NewTone (speakerPin,1000); 1017 delay(50); 1018 noNewTone(); 1019 } 1020 RefreshDisplay(); 1021} 1022 1023void goright10() { // right 10x button/RC 1024 if (ispause == false) { 1025 for (s = 1; s < 11; s ++) { 1026 nmf_routetofollow ++; 1027 if (nmf_routetofollow > 360) { 1028 nmf_routetofollow = 1; 1029 } 1030 } 1031 d=nmf_routetofollow; 1032 nmf_routetofollow=d; 1033 nm_routetofollow=d; 1034 NewTone (speakerPin,800); 1035 delay(200); 1036 noNewTone(); 1037 } else { 1038 NewTone (speakerPin,1000); 1039 delay(50); 1040 noNewTone(); 1041 } 1042 RefreshDisplay(); 1043} 1044 1045void gopause() { // pause button/RC 1046 if (ispause == true) { 1047 ispause=false; 1048 digitalWrite(ledpausePin, HIGH); 1049 NewTone (speakerPin,50); 1050 delay(200); 1051 NewTone (speakerPin,200); 1052 delay(800); 1053 noNewTone(); 1054 } else { 1055 ispause=true; 1056 digitalWrite(ledpausePin, LOW); 1057 NewTone (speakerPin,200); 1058 delay(200); 1059 NewTone (speakerPin,50); 1060 delay(800); 1061 noNewTone(); 1062 } 1063 RefreshDisplay(); 1064} 1065 1066// reading eeprom 1067byte readEEPROM (int diskaddress, unsigned int memaddress) { 1068 byte rdata = 0x00; 1069 Wire.beginTransmission (diskaddress); 1070 Wire.write (memaddress); 1071 if (Wire.endTransmission () == 0) { 1072 Wire.requestFrom (diskaddress,1); 1073 if (Wire.available()) { 1074 rdata = Wire.read(); 1075 } else { 1076 EEerr = 1; //"READ no data available" 1077 } 1078 } else { 1079 EEerr = 2; //"READ eTX error" 1080 } 1081 Wire.endTransmission (true); 1082 return rdata; 1083} 1084 1085// writing eeprom 1086void writeEEPROM (int diskaddress, unsigned int memaddress, byte bytedata) { 1087 Wire.beginTransmission (diskaddress); 1088 Wire.write (memaddress); 1089 Wire.write (bytedata); 1090 if (Wire.endTransmission () != 0) { 1091 EEerr = 3; //"WRITING eTX error" 1092 } 1093 Wire.endTransmission (true); 1094 delay(5); 1095} 1096 1097// round zero decimal 1098float roundZeroDec(float f) { 1099 float y, d; 1100 y = f*1; 1101 d = y - (int)y; 1102 y = (float)(int)(f*1)/1; 1103 if (d >= 0.5) { 1104 y += 1; 1105 } else { 1106 if (d < -0.5) { 1107 y -= 1; 1108 } 1109 } 1110 return y; 1111} 1112 1113// round one decimal 1114float roundOneDec(float f) { 1115 float y, d; 1116 y = f*10; 1117 d = y - (int)y; 1118 y = (float)(int)(f*10)/10; 1119 if (d >= 0.5) { 1120 y += 0.1; 1121 } else { 1122 if (d < -0.5) { 1123 y -= 0.1; 1124 } 1125 } 1126 return y; 1127} 1128 1129// round two decimals 1130float roundTwoDec(float f) { 1131 float y, d; 1132 y = f*100; 1133 d = y - (int)y; 1134 y = (float)(int)(f*100)/100; 1135 if (d >= 0.5) { 1136 y += 0.01; 1137 } else { 1138 if (d < -0.5) { 1139 y -= 0.01; 1140 } 1141 } 1142 return y; 1143} 1144
Downloadable files
Stepper plate by Andrew Barney
Stepper plate by Andrew Barney
PCB Autopilot2 top
PCB Autopilot2 top
PCB Stepalim topsilk
PCB Stepalim topsilk
PCB Autopilot2 bottom
PCB Autopilot2 bottom
PCB Autopilot2 topsilk
PCB Autopilot2 topsilk
Full project diagram both Autopilot2+Charger
Full project diagram both Autopilot2+Charger
Air hole guard
Air hole guard
PCB Stepalim bottom
PCB Stepalim bottom
Stepper 56mm pulley
Stepper 56mm pulley
PCB Autopilot2 bottomsilk
PCB Autopilot2 bottomsilk
Full circuit schematic diagram both Autopilot2+Charger
Full circuit schematic diagram both Autopilot2+Charger
PCB Stepalim top
PCB Stepalim top
Fan guard 30x30mm
Fan guard 30x30mm
L298 stepper driver schematic diagram
L298 stepper driver schematic diagram
Comments
Only logged in users can leave comments
marcozonca
0 Followers
•0 Projects
Table of contents
Intro
5
0