'AP28X2R.BAS 01Dec17 "Romeo" of Snoopy's Picaxe 28X2 Autopilot Software. (c) Robin Lovelock www.GPSS.co.uk 'AP28X2R.BAS 06Dec17 "Romeo" rudder gearing constant MAXDERR = 30 to 60 after today's test 'AP28X2R.BAS 01Dec17 "Romeo" Only change is rudder gearing constant MAXDERR = 120 to 30 'based on 'AP28X2Q.BAS 31Jul17 "Quebec" Correct cross trackj error by comparing W24 CBWP Course Between Way Points 'AP28X2P.BAS 16Jul17 "Papa" remove spoken range bug when ~ 100m E-W heard on 12th July 17. '21Jul17 add "Time " to trigger VOX mode in PMR transmissions. See BlogX video. '16Jul17 "Papa" add and use "Playing Field Test" at Charter's School 51 23.656 N 0 38.886 W 'AP28X2O.BAS 12Jul17 "Oscar" GPS-Only after road test OK on 11Jul17. More SERTXD monitoring. 'AP28X2N.BAS 11Jul17 "Noddy" (was Hybrid) GPS-Only & Compass steering, using Course To Steer (CTS) in W26. '11Jul17 major changes to get old GPS-Only steering logic OK. e.g. w13 to w21 for speed. 'OLD Hybrid: start 30 sec control loop with Compass steering, then end with GPS Only based on GPS Heading. 'AP28X2N "Noddy" Revert to smaller rudder trim increments TRIMINC = 2 after 29th June tests. 'AP28X2N "Noddy" 05Jul17 look for bugs like rudder "Flip" and timing. After 3rd July tests. '06Jul17 "Noddy". Roy suggested (firmware) SERIN for Compass might effect rudder SERVOPOS. 'Rudder Flip - Looks Ok in subs limitrudder (GPS Only) and moverudder (compass) steering, and b21 rudder trim logic. 'Rudder timing: related to delays needed for Picaxe 28X2 speed switching needed for Servo control ? 'AP28X2M "Mike" recreate then fix bugs ? e.g. remove "Frig" of 21Oct16 for range; jibe. overflow risk. 'AP28X2L "Lima" Jibe turn logic added. Bug in at least range seen on 24 June 2017. 'AP28X2K "Kilo" fix steering bug - limit rudder to avoid flip over.in compass steering. 'AP28X2J recover memory by removing waypoints near Robin's house. Rudder problem on 03May17 Bray Test. '28Apr17 "Juliet" Changes to TTS so better able to test. Limit CTS correction to < 45 degrees. 15sec cycle. '26Apr17 "India" made Course to Steer (CTS) active. Failed miserably on Bray Lake - see the movie ! :-) '20Apr17 "Hotel" use W27 for what is spoken based on time seconds, including Course To Steer (CTS). 'AP28X2G.BAS "Golf" 3rd February 2017 at 16:30 pm, based on AP28X2F and Compass logic from AUTOPX1 5pm 22 August 2014. 'Earlier Atlantic Attempts such as 2016 used AUTOP9TTS ( www.gpss.co.uk/autop9~1.bas ) 'Old backups are in www.gpss.co.uk/ap28x2f.bas and www.gpss.co.uk/autopx1.bas 'AP28X2F Notes ... '03Feb17 Slower startup servo waggle to test SERVOMON.BAS. Remove 05Dec16 CTS = W1. '10Dec16 12 second control loop, includes 10sec steering loop with 0.5 sec rudder updates. 'Less talking because servo shows directional errors. Time + seconds spoken. '05Dec16 TESTING with just compass steering ( CTS = W1 ) '04Dec16 Correct bug in sub Mag2True ( Compass Magnetic->True Heading ). Change TTS voice. '02Dec16 Introduce W26 Course To Steer (CTS) - compass steering to compensate for drift. '30Nov16 W25 for Boat Heading ( W1 is Target Heading and W3 is GPS based course ) 'All Atlantic Attempts 2012 to 2016 were based on Picaxe 08M2 GPS Only autopilots. 'The 2017 Boat may use a Picaxe 28X2 Autopilot Computer with better BR355-S2 GPS + Compass. 'LOTS of testing will be needed to ensure it is as reliable as the old 08M2. 'Then we may try to improve the steering logic, such as that related to rudder bias. 'i.e. try and make the boat take a more direct path to the next waypoint, in all wind conditions. 'Maybe even compensate for hardware problems such as a slightly offset rudder. ' 'See www.gpss.co.uk/autop.htm , linked pages and www.picaxeforum.co.uk Thanks Hippy + Others :-) 'Early history on Picaxe Forum http://www.picaxeforum.co.uk/showthread.php?18952-PICAXE-handling-GPS-for-robot-boat ' 'IMPORTANT RESTRICTIONS: This autopilot is intended to work for the Atlantic 'between longitudes 0 (Greenwich) and 100 West (Americas) 'and latitudes north of the Equator. 'input is serial RS232 GPS data 'the GPS outputs text strings and includes ",A," after $GPRMC when ready 'BU355 e.g. $GPRMC,114138.000,A,5130.0011,N,00041.4333,W,1.25,134.28,021211,,*11 'Simulator e.g. $GPRMC,102802,A,5129.990,N,00041.456,W,4.4,191,041111,,*18 'output is servo controlling rudder to steer boat on a particular heading 'Second servo can be used to operate the clutch on a vane-rudder or play audible tomes 'The third servo position is used sense a wind direction sensor or input control. 'This is for use with the faster 08M2 chip 'servo 1 controls rudder, based on GPS heading to steer the boat. 'after the startup waggle of the two servo and test tones, it should monitor the GPS 'and control the rudder. Testing can be with real GPS or the GPS Simulator. 'If the GPS is tracking, then servo 1 is moved to steer boat according to GPS heading. 'It needed the LOW 1 to switch off SERVO before SERIN ' 'Use of registers: 'Picaxe 08M2 only had registers w0 to w13 (b0-b27) but 28X2 has W0 to W27 (b0 to b55). 'GPS Lat/Lon now used to update the target heading, held accurately to 1/1000ths of a minute 'b0 = latitude degrees North only i.e. 0 to 89N 'b1 = longitude degrees West only i.e, 0 to 120W ??? 'w1 = target heading 0 to 359 'w2 = GPS heading (true Course) (direction of boat movement, from the GPS) 'w3 = GPS speed then servo position 1j - then later, save in w13 (now W21 19Feb15) 'w4 = GPS accurate minutes of latitude 0 to 59999 lsd = 0.001 minutes. 'w5 = GPS accurate minutes of longitude 0 to 59999 lsd = 0.001 minutes. 'w6 /b12 b13 used as temp variable ' 'b14,b15 (w7) = destination lat, lon degrees. 'w8 = destination latitude minutes 0 to 59999 lsd = 0.001 minutes. 'w9 = destination longitude minutes 0 to 59999 lsd = 0.001 minutes. 'WAS b20 = rudder centred with trim - added on 26 Oct 11. 'b20 used as tacking counter in v1f 12 Jan 12. 'b21 = rudder centred with trim - was A or V = tracking or not tracking 'temporary variables b22 - b27 (W11,W12,W13) '29Feb15 register use from AUTOPX1: 'W20 = Range in metres if < 200m else 999 on Picaxe 28X2 from July 2014. 'W21 = GPS Speed in units of 0.1 knots '19Feb15 use W22 bytes b44 and b45 for speed 'W22 = Wind Direction from True North 0-359 (smoothed) 'Quebec WAS W23 = Steering loop counter for time between navigation checks 'W24 = CBWP Course Between Way Points Quebec Correct cross trackj error by comparing with direction to waypoint. 'w24 = (WAS ) tacking direction: 0=Not tacking. 1=left. 2=right 3=jibe left 4=jibe right 'w27 = SPARE? ( was temp for cos lat). 20Apr17 USE FOR TIME SECONDS FOR WHAT SPOKEN ' 'w24 = Compass steering loop counter - to limit how long in loop 'w25 = Compass Heading 0 to 359 degrees. 361 means Compass not available. 'w26 = Course To Steer (CTS) #picaxe 28X2 '18Feb15 was 08M2 setfreq M8 '19Feb15 was M16 Need to switch to M16 to read GPS symbol CENTRE = 150 'Servo middle position symbol THROW = 50 symbol LEFT = CENTRE + THROW 'Servo left symbol RIGHT = CENTRE - THROW 'Servo right symbol TRIMMAX = 40 'maximum trim on rudder - was 30 then 20 symbol TRIMMAXLEFT = CENTRE + TRIMMAX symbol TRIMMAXRIGHT = CENTRE - TRIMMAX symbol TRIMINC = 2 '30Jun17 was 5 19Jun17 was 2 increment of trim added on full rudder symbol MAXDERR = 60 'AP28X2R.BAS 06Dec was 30 01Dec17 was 120 maximum direction error - used in steering logic was 90, then 180 symbol FOR1SEC = 1000 '19Feb15 pause value at 16MHz for 1 sec was 4000 symbol FOR1P5SEC = 1500 'pause value at 16MHz for 1.5 sec was 6000 symbol FOR2SEC = 2000 'pause value at 16MHz for 2 sec was 8000 symbol FOR3SEC = 3000 'pause value at 16MHz for 3 sec was 12000 symbol FOR4SEC = 4000 'pause value at 16MHz for 4 sec was 16000 symbol FOR6SEC = 6000 'pause value at 16MHz for 6 sec symbol FORP5SEC = 500 'pause value at 16MHz for 0.5 sec was 2000 symbol FORP1SEC = 100 'pause value at 16MHz for 0.1 sec was 400 symbol FORP2SEC = 200 'pause value at 16MHz for 0.2 sec was 800 symbol FORP4SEC = 400 'pause value at 16MHz for 0.4 sec symbol FORP75SEC = 750 'pause value at 8MHz for 0.8 sec symbol FORP01SEC = 10 'pause value at 16MHz for 0.01 sec was 0.025 call waggle 'waggle servo 1 on startup to show that working - will control rudder SEROUT 2, T9600_8, ( "N2",10,"V16",10,"W300", 10) '29Apr17 TTS N2=Chipmonk, loudish, fast 300 PAUSE FOR1SEC SEROUT 2, T9600_8, ( "S Snoopy's Picaxe 28X2 GPS Only Autopilot.", 10 ) ' Speak via TTS PAUSE FOR4SEC '21Oct16 may be needed ? 28Nov16 was 1 sec. SEROUT 2, T9600_8, ( "S Software Version Rome ee oh. Last update 6th of December 2017. ",10 ) 'Speak software version via TTS PAUSE FOR4SEC '21Oct16 may be needed ? 28Nov16 was 1 sec. '06Jul17 b1 = 0 '04Dec16 Longitude 0W for readcompass '06Jul17 call readandsaycompass 'Juliet 28Apr17 returns w25 0 to 359 or 360 if no compass '06Jul17 call sayit 'e.g. "Compass 270" or "No Compass". '06Jul17 PAUSE FOR2SEC ''06Jul17 was 1sec 29Apr17 was 6 sec '06Jul17 "Noddy". Roy suggested (firmware) SERIN for Compass might effect rudder SERVOPOS. b14 = 99 '06Feb15 flag need to set default waypoint after waitrmc GPS data. gosub zerotrim 'zero rudder trim in b21. Rudder is centred. waitgps: 'wait until GPS ready and tracking $GPRMC,114801,A,5129.8944,N, etc,etc PAUSE FOR1SEC '3May16 make sure servo is centred before stopping it. call servostop 'stop servo pulses, because servo only works at M8 setfreq M16 '$GPRMC,122159.000,A,5129.9288,N,00041.4805,W,2.31,104.23,180215,,,D*76 SERIN B.3, N4800_16, ( "$GPRMC," ),b22,b23,b24,b25,b26,b27 '20Feb15 wait for $GPRMC sentence with time setfreq M8 call saybegin '19Feb15 begin Text-To-Speech (TTS) output call saytime '10Dec16 +seconds added. 19Feb15 Original. waitrmc: setfreq M16 '20Feb15 SERIN B.3, N4800_16, ( "$GPRMC," ) 'wait for $GPRMC sentence 'now we are near the latitude - start by reading 5129.89 into bytes when ,A, = GPS tracking SERIN B.3, N4800_16, ( ",A," ), b22,b23,b24,b25,b26,b26,b27,b5 '1v 1s first b26 is to skip the decimal point 'sertxd ( 10,13," RawLat= ",b22,b23,b24,b25,b26,b27,b5 ) 'trace output back to PC b22 = b22 - 48: b23 = b23 - 48: b24 = b24 - 48: b25 = b25 - 48 b26 = b26 - 48: b27 = b27 - 48: b5 = b5 - 48 'from ASCII b0 = b22 * 10 + b23 'degrees of latitude e.g 50 w4 = b24 * 10 + b25 'minutes e.g. 30 w4 = w4 * 1000 'convert minutes to 1/1000 ths minutes w6 = b26 * 100 'add 0.1 mins w4 = w4 + w6 'add 0.1 mins w6 = b27 * 10 w4 = w4 + w6 'add 0.01 mins w4 = w4 + b5 'add 0.001 mins - now minutes of latitude in 1/1000yhs mins 'read longitude ,N,00041.0771,W, SERIN B.3, N4800_16, ( ",N," ), b22,b23,b24,b25,b26,b27,b27,b5,b4 'first b27 is to skip the decimal point 'sertxd ( 10,13," RawLon=",b22,b23,b24,b25,b26,b27,b5,b4 ) 'trace output back to PC b22 = b22 - 48: b23 = b23 - 48: b24 = b24 - 48: b25 = b25 - 48 'from ASCII b26 = b26 - 48: b27 = b27 - 48: b5 = b5 - 48: b4 = b4 - 48 'from ASCII b1 = b22 * 100 w6 = b23 * 10 + b24 b1 = w6 + b1 'longitude degrees e.g 001 IF b0 <=24 AND b1 <=5 THEN goto waitgps '11Feb15 ignore any bad lat/lon from GPS at startup ENDIF if b14 = 99 then '06Feb15 need to set default waypoint, very first time. if b0 = 51 then 'set Bray Lake waypoint Alpha if 51N b46 = 1 '21Feb15 b14 = 51: b15 = 0: w8 = 29970: w9 = 41460 'Waypoint Alpha w24 = 225 'CBWP Course Between Waypoint Base to A ( 999 if none ) else 'set Mayflower as default waypoint after a power up restart. b46 = 25 '21Feb15 ???? b14 = 41: b15 = 70: w8 = 57580: w9 = 39730 'Mayflower Museum. w24 = 999 'CBWP Course Between Waypoint ( 999 if none ) Cross Track Correction OFF endif endif w5 = b25 * 10 + b26 'minutes e.g. 30 w5 = w5 * 1000 'convert minutes to 1/1000 ths minutes w6 = b27 * 100 'add 0.1 mins w5 = w5 + w6 'add 0.1 mins w6 = b5 * 10 'add 0.01 mins w5 = w5 + w6 'add 0.01 mins w5 = w5 + b4 'add 0.001 mins - is now scaled longitude 1/1000 minutes 'e.g. $GPRMC,114138.000,A,5130.0011,N,00041.4333,W,1.25,134.28,021211,,*11 SERIN B.3, N4800_16, ( ",W," ), b6 '1j read value speed of 1 until decimal point. i.e. 1 from 1.25 SERIN B.3, N4800_16, (".") ,b7 'read first fractional digit. i.e. the 2 from .25 SERIN B.3, N4800_16, (","), #w2 '1v the GPS heading the boat is travelling if w2 > 359 then '19Feb15 protect against bad GPS heading (don't allow 0) 'sertxd ( 10,13," ERROR hdg=",#w2," RawLon=",b22,b23,b24,b25,b26,b27,b5,b4 ) '1v trace output back to PC goto waitrmc 'go back and try next line of GPS data endif setfreq M8 '20Feb15 call servostart '11Jul17 b44 = b6: b45 = b7 '19Feb15 for use by sayspeed 25Ju17 spaces added b6 = b6 - 48 '1i to ascii b6 = b6 * 10 'scale speed value to units of 0.1 knots b7 = b7 - 48 'convert from ascii to a value b6 = b6 + b7 w21 = b6 '19Feb15 w21 was w3 is now value of speed of 12 from 1.25 if w21 > 50 then '19Feb15 limit to 5 knots max for if used with GPS Simulation. w21 = 50 endif 'FOLLOWING CODE SETS DESTINATION WAYPOINT LAT/LON BASED ON GPS LAT/LON. '24Dec14 revised UK-USA Route logic ********************************************* 'Route from near Boscombe Pier,UK, west to 12W, then Azores, then 60W, then USA. 'Channel = 50 30'N 1 40'W (unchanged) was '2a 1 50.50N 1.67W Channel 'Weymouth = 50 09'N 2 20'W (11' south) - new after last 2014 Atlantic Attempt 'Prawle = 49 55'N 3 40'W (10' south) '11Jan15 was 50' was '2a 1a 50N 3.667W Prawle Point 'Lizard = 49 20'N 5 15'W (20' south) was '2a 1b 49.667N 5.25W Lizard 'Start Line= 48 00'N 12 00'W was '2a 2 49N 7.5W Microtransat Start Line. '3 35N 22.5W Azores and Madiera '4 24.5N 60W Microtransat Finish Line. '5 32N 63W East of Bermuda '6 42N 68W East of USA '7 42.33720 -70.00000 20232 56664 Cape Cod '24Jan15 moved from -69.94440 '8 Plymouth Bay '25Jan15 41 59'N 70 35.7'W '9 41.98660 -70.66670 58998 40002 Kingston Bay '25Jan15 now 41 59'N 70 40'W '10 41.96096 -70.65070 57658 39042 Plymouth Breakwater '11 41.95966 -70.66217 57580 39730 Mayflower Museum '60 unit NS = 110m,80 EW = 110m so +-60NS80EW 240m side boxes 'Final destination Plymouth,MA,USA = 41.9596 -70.66216 = 41 57580 70 39730 '24Jan15 moved down if b0 < 51 and b1 < 70 then 'lat<51 only if south of 51N and East of 70W if b0 < 51 and b1 < 12 then '24Dec14 New UK->12W Start Line logic w24 = 999 'Quebec 31Jul17 start with Cross Track Correction OFF ( but see Weymouth ) 'Needles protection logic rewrite on 05Feb15 IF b0 = 50 AND w4 > 30000 AND b1 = 1 AND w5 < 36000 THEN prawle '05Feb15 IF b0 = 50 AND w4 > 30000 AND b1 < 2 THEN channel '05Feb15 IF b0 = 50 THEN '08Feb15 AND w4 <= 30000 See Blog RB6Feb15.ZIP Channel IF b1 = 1 THEN weymouth IF b1 = 2 AND w5 <= 28000 THEN weymouth '08Feb15 was 20000. Now switch nearer Portland. ENDIF '05Feb15 Only if >=48N. i.e. not in Bay of Biscay. Taa to Peter's Rig :-) if b0 >=48 then IF b1 = 2 AND w5 >= 20000 THEN prawle IF b1 = 3 AND w5 < 40000 THEN prawle IF b1 = 3 AND w5 >= 40000 THEN Lizard '05Feb15 IF b1 = 4 THEN Lizard IF b1 = 5 AND w5 < 15000 THEN Lizard endif '05Feb15 Only if >=48N. i.e. not in Bay of Biscay. '08Feb15 Extra Protection Box in Bay of Biscay. See Blog RB6FB15.ZIP BayofBiscay IF b0 >=43 AND b0 < 48 AND b1 < 9 THEN StartLine '08Feb15 Bay of Biscay Protection Box. '11Jan15 cover case where might reach start at 48N12W but gets blown east again if b0 >= 48 then '09Feb15 moved to AFTER Bay of Biscay box after Peter's rig test. goto startline else goto azores 'goto Azores if get south of 48N endif endif 'end of New UK->12W Start Line logic if b0 < 51 and b1 < 70 then 'lat<51 if south of 51N and East of 70W 24Jan15 moved here if b1 < 22 and b1 > 7 then 'if 7W < Lon < 22W goto azores endif if b1 = 22 and w5 < 30000 then 'if Lon < 22.5W goto azores endif '06Feb15 if NW of Azores WP at 35N 22.5W, then goto FinishLine. Taa Dick & Peter. IF b0 < 42 AND b0 >= 35 AND b1 < 60 THEN '08Feb15 See Blog RBRIG6FB.ZIP Finish IF b1 = 22 AND w5 >= 30000 THEN B4Finish 'was FinishLine IF b1 > 22 THEN B4Finish ENDIF if b0 < 35 and b1 < 60 then 'if lat < 35 Lon < 60W IF b0 >= 26 THEN B4Finish '08Feb15 See Blog RBRIG6FB.ZIP Finish Goto FinishLine endif '24Jan15 this code moved down from above if b1 >= 60 and b0 < 32 then '22Jan15 was > goto bermuda '22Jan15 endif if b0 >= 42 then if b1 >= 60 then goto capecod endif endif '05Feb15 better logic to Cape Cod endif 'if b0 < 51 and b1 < 70 'Final part of route to destination in USA '42.33720 -69.94440 20232 56664 Cape Cod '41.98330 -70.59505 58998 35703 Plymouth Bay '41.98660 -70.66670 58998 40002 Kingston Bay '25Jan15 now 41 59'N 70 40'W '41.96096 -70.65070 57658 39042 Plymouth Breakwater '41.95966 -70.66217 57580 39730 Mayflower Museum '30 NS = approx 50m 40EW = approx 50m. i.e. 100m boxes '09Feb15 < 70 08Feb15 extend box to west. See Blog and RBRIG6FB.ZIP East of USA. IF b1 >= 60 AND b1 < 70 AND b0 < 42 THEN eastofusa '08Feb15 extend box to west. IF b1 = 70 AND w5 < 35700 THEN '05Feb15 not far east of Plymouth Bay IF b0 = 41 OR b0 = 42 THEN Plymouthbay ENDIF if b0 = 41 and b1 = 70 then ' if at 41N 70W '60 unit NS = 110m,80 EW = 110m so +-60NS80EW 240m side boxes 'if at Plymouth Bay waypoint 41 59'N 70 35.7'W (was 58998 35703) to Kingston Bay '08Feb15 See Blog RBRIG6FB.ZIP Make Box bigger to west of Plymouth Bay '08Feb15 IF w4 > 58940 AND w4 < 59060 AND w5 > 34620 AND w5 < 36780 THEN Kingston IF w5 >= 35700 AND w5 < 38400 THEN Kingston '08Feb15 Box west of Plymouth Bay 'if at Kingston Bay set destination to Plymouth Breakwater IF w4 > 59140 AND w4 < 59260 AND w5 > 39920 AND w5 < 40080 THEN Breakwater 'if at 41 57.658'N 70 39.042'W Plymouth Breakwater set destination to Mayflower IF w4 > 57598 AND w4 < 57718 AND w5 > 38962 AND w5 < 39122 THEN Mayflower endif 'end of final route segment to USA 'End of UK-USA Route logic ************************************ ' 'Following tests are all for 51N 0W - to reduce memory used by program. if b0 = 51 and b1 = 0 then ' if at 51N 0W '11Jul17 Armitage Court logic back in IF w4 > 23816 AND w4 < 23822 AND w5 > 39596 AND w5 < 39604 THEN call sayrobin: goto lamppost ENDIF IF w4 > 23841 AND w4 < 23847 AND w5 > 39571 AND w5 < 39579 THEN call saylamppost: goto RobinsLawn ENDIF '16Jul17 "Papa" add "Playing Field Test" at Charter's School 51 23.656 N 0 38.886 W IF w4 > 23600 AND w4 < 23700 AND w5 > 38800 AND w5 < 39000 THEN call saycharters: goto Charters ENDIF '21Dec14 if near Clubhouse then set destination to waypoint Delta for fast test. IF w4 > 29999 AND w4 < 30016 AND w5 > 41364 AND w5 < 41390 THEN call sayclubhouse: goto brayd '05Feb15 ENDIF 'if in Bray Lake 24/7 test area at end of lake, then set destination to 24/7 WP. IF w4 > 30050 AND w4 < 30100 AND w5 > 41600 AND w5 < 41820 THEN goto bray247 '05Feb15 ENDIF 'at Bray Lake 24/7 test waypoint then speak "at waypoint" IF w4 > 30007 AND w4 < 30023 AND w5 > 41687 AND w5 < 41713 THEN call saybray247 goto bray247F ENDIF 'at Bray Lake 24/7 test waypoint F then speak "at waypoint foxtrot" IF w4 > 29952 AND w4 < 29968 AND w5 > 41687 AND w5 < 41713 THEN call saybray247f goto bray247 ENDIF 'if at Bray Lake waypoint A then set destination to waypoint B 05Feb15 IF w4 > 29962 AND w4 < 29978 AND w5 > 41447 AND w5 < 41473 THEN w24 = 270 'Quebec CBWP Course Between Waypoint A to B for Cross Track Correction. call sayalpha goto brayB ENDIF 'if at Bray Lake waypoint B then set destination to waypoint C '05Feb15 IF w4 > 29962 AND w4 < 29978 AND w5 > 41567 AND w5 < 41593 THEN w24 = 135 'Quebec CBWP Course Between Waypoint B to C for Cross Track Correction. call saybravo goto brayc ENDIF 'if at Bray Lake waypoint C then set destination to waypoint D IF w4 > 29932 AND w4 < 29948 AND w5 > 41507 AND w5 < 41533 THEN w24 = 90 'Quebec CBWP Course Between Waypoint C to D for Cross Track Correction. call saycharly goto brayd ENDIF 'if at Bray Lake waypoint D then set destination to waypoint Base IF w4 > 29932 AND w4 < 29948 AND w5 > 41387 AND w5 < 41413 THEN w24 = 0 'Quebec CBWP Course Between Waypoint D to Base for Cross Track Correction. call saydelta goto braybase ENDIF endif 'end of test if at 51N 0W goto endwaypoints '21Oct16 corner22 is test waypint on corner of Robin's drive corner22: b14 = 51: b15 = 0: w8 = 23819: w9 = 39590 : b46 = 99: goto b4endwaypoints LampPost: b14 = 51: b15 = 0: w8 = 23844: w9 = 39575 : b46 = 7: goto b4endwaypoints RobinsLawn: b14 = 51: b15 = 0: w8 = 23819: w9 = 39600: b46 = 8: goto b4endwaypoints Charters: b14 = 51: b15 = 0: w8 = 23656: w9 = 38886: b46 = 88: goto b4endwaypoints '16Jul17 'Bray Lake waypoints for the Bray Lake Test BrayB: b46 = 2 'Bray Lake waypoint B at 51 29.970'N 0 41.580W '05Feb15 b14 = 51: b15 = 0: w8 = 29970: w9 = 41580: goto b4endwaypoints BrayC: b46 = 3 'Bray Lake waypoint C at 51 29.940'N 0 41.520W '05Feb15 b14 = 51: b15 = 0: w8 = 29940: w9 = 41520: goto b4endwaypoints BrayD: b46 = 4 'Bray Lake waypoint D at 51 29.940'N 0 41.400'W near Island.'05Feb15 b14 = 51: b15 = 0: w8 = 29940: w9 = 41400: goto b4endwaypoints BrayBase: b46 = 5 'Bray Lake waypoint Base at 51 30.007'N 0 41.407'W '05Feb15 b14 = 51: b15 = 0: w8 = 30007: w9 = 41407: goto b4endwaypoints 'Bray247: b46 = 6 'Bray Lake waypoint for 24/7 test at 51 30.015'N 0 41.700'W 'b14 = 51: b15 = 0: w8 = 30015: w9 = 41700: goto endwaypoints 'does not speak Bray247: b46 = 6 '12Mar16 Bray Lake waypoint for 24/7 test at 51 30.015'N 0 41.700'W b14 = 51: b15 = 0: w8 = 30015: w9 = 41700: goto b4endwaypoints ' Bray247F: b46 = 26 '12Mar16 Bray Lake 2nd waypoint for 24/7 test at 51 29.960'N 0 41.700'W b14 = 51: b15 = 0: w8 = 29960: w9 = 41700: goto b4endwaypoints ' 'Waypoints for routing from launch, along Channel, to Microtransat start. channel: b46 = 11 '24Dec14 No Change: Channel = 50 30'N 1 40'W b14 = 50: b15 = 1: w8 = 30000: w9 = 40000 '2a set destination to 50.5N 1.67W Channel. was 50.50N 1.75W goto endwaypoints weymouth: b46 = 12 '24Dec14 Weymouth = 50 09'N 2 20'W b14 = 50: b15 = 2: w8 = 9000: w9 = 20000 'South of Weymouth w24 = 230 'Quebec 31Jul17 Course Between Waypoints Channel and Weymouth goto endwaypoints prawle: b46 = 13 '08Jan15 now 49 55'N 3 40'W was '24Dec14 Prawle = 49 50'N 3 40'W b14 = 49: b15 = 3: w8 = 55000: w9 = 40000 'South of Prawle w24 = 250 'Quebec 31Jul17 Course Between Waypoints Weymouth and Prawle goto endwaypoints lizard: b46 = 14 '24Dec14 Lizard = 49 20'N 5 15'W b14 = 49: b15 = 5: w8 = 20000: w9 = 15000 'South of Lizard w24 = 255 'Quebec 31Jul17 Course Between Waypoints Prawle and Lizard goto endwaypoints startline: b46 = 15 '24Dec14 Start Line= 48 00'N 12 00'W b14 = 48: b15 = 12: w8 = 0: w9 = 0 'New Microtransat Start Line w24 = 255 'Quebec 31Jul17 Course Between Waypoints Lizard and Start Line goto endwaypoints 'Waypoints for routing from Microtransat Start, via Azores, to Finish, then USA azores: b46 = 16 '1x b12 = 3: gosub playdesti 'play tune to show destination selected b14 = 35: b15 = 22: w8 = 0: w9 = 30000 'set destination to 35N 22.5W Azores goto endwaypoints b4finish: b46 = 17 '22Jan15 new waypoint before Finish Line at 24 30'N 59W b14 = 24: b15 = 59: w8 = 30000: w9 = 0 'set destination to 24.5N 59W before Finish line goto endwaypoints finishline: b46 = 18 '1x b12 = 4: gosub playdesti 'play tune to show destination selected b14 = 24: b15 = 60: w8 = 30000: w9 = 0 'set destination to 24.5N 60W Finish line goto endwaypoints bermuda: b46 = 19 '22Jan15 b14 = 32: b15 = 63: w8 = 0: w9 = 0 'set destination to east of Bermuda goto endwaypoints eastofusa: b46 = 20 '22Jan15 b14 = 42: b15 = 68: w8 = 0: w9 = 0 'set destination east of USA 42N 68W goto endwaypoints capecod: b46 = 21 '24Jan15 move to 70W was 69 56.664 W b14 = 42: b15 = 70: w8 = 20232: w9 = 0 'set destination Cape Cod goto endwaypoints plymouthbay: b46 = 22 '22Jan15 '24Jan15 name change 25Jan15 was 58998,35703 now 41 59'N 70 35.7'W b14 = 41: b15 = 70: w8 = 59000: w9 = 35700 'set destination Plymouth Bay goto endwaypoints kingston: b46 = 23 '26Jan15 41 59.2'N 70 40'W b14 = 41: b15 = 70: w8 = 59200: w9 = 40000 'Kingston Bay was 58998 40002 goto endwaypoints breakwater: b46 = 24 '24Jan15 b14 = 41: b15 = 70: w8 = 57658: w9 = 39042 'Plymouth Breakwater goto endwaypoints mayflower: b46 = 25 '24Jan15 b14 = 41: b15 = 70: w8 = 57580: w9 = 39730 'Mayflower Museum goto endwaypoints '08Feb15is needed now, to skip b4endwaypoints. 'END OF LOGIC TO SET DESTINATION WAYPOINT BASED ON GPS LAT/LON. b4endwaypoints: '05Feb15 ' call atwaypoint 'speak "at waypoint" for TTS on Bray Lake endwaypoints: 'END OF CODE THAT SETS DESTINATION LAT/LON BASED ON GPS LAT/LON 'calculate range of destination based on lat/lons.for sound output 'This is done if two positions have same lat and lon degrees and within 200m '08Feb15 Changes to squeeze in range in Km as well as in metres if < 200m 'At limit of memory on this 08M2 but easy on the 28X2, Could do in miles instead. '1 NM = 1.852 km = 60,000 units of w8 or w4 'Longitude is scaled by COS(Lat). If enough memory, would use COS(Lat) as 'in Calculation of heading below, or better. ' COS(50)=0.64 COS(45)=0.71. Code below used /9 equiv to 0.56 ? Better if /7=0.71? call coslat '09Feb15 can now use COSLAT to return b22=100 * COS(Lat) '21Oct16 changes below to try and get better spoken range. e.g. "13 metres" instead of "10 metres". if b0 = b14 and b1 = b15 then '1v if LatGPS=LatDest if w8 > w4 then 'Dest Lat > GPS Lat w12 = w8 - w4 'Diff Lat else w12 = w4 - w8 endif if w5 > w9 then '08Feb15 moved up from after if w12 w13 = w5 - w9 'Diff lon - which must be scaled by COS(Lat) else w13 = w9 - w5 endif '26Jun17 (not needed) w13 = w13 'approx units of 10 metres. '21Oct16 was /5 as for Latitude '27Jun17 original unprotected line below could give overflow for large w13 '27Jun17 w13 = w13 * b22 / 100 '09Feb15 scale diff lon by COS(Lat) if w13 < 600 then '18Jul17 was 60 w13 = w13 * b22 / 100 'scale diff lon by COS(Lat) else w13 = w13 / 100 * b22 'crude but safe endif '26Jun17 sertxd ( "CosLat=", #b22 ) '26Jun17 if w12 < 200 then 'only if < 200 metres '21Oct16 was <41 '21Oct16 was /5 as for Latitude w13 = w13 * b22 / 100 '09Feb15 scale diff lon by COS(Lat) w12 = w12 * 19 / 10 '16Jul17 FRIG for DLat from Charters Tests w13 = w13 * 3 '16Jul17 FRIG for DLon from Charters Tests replace old FRIG below. w12 = w12 * w12 w13 = w13 * w13 w12 = w12 + w13 'now need SQRT w6 = SQR w12 '22Feb15 use SQR instead of iterative calculation '16Jul17 put FRIG below back in as 56/27, Charters tests of short range spoken range '16Jul17 w6 = w6 * 56 / 27 '16Jul17 21Oct16 FRIG! scale 63 / 27 to get Lampost about 63m from 22AC '16Jul17 w6 = w6 * 63 / 27 '21Oct16 FRIG! scale to get Lampost about 63m from 22AC '16Jul17 b12 = w6 '15Jul17 was taken on on 26Jun17 - needed for sub saydist if w6 < 200 then '26Jun17 was b22 range less than 20 units = 200 metres '21Oct16 was <21 gosub saydist 'speak range in metres. 08Feb15 was playdist endif else 'further than 200m but in same degree '26Jun17 beware of overflow: 16 bits = max 65k. Here minutes * 1000 < 60,000 '26Jun17 w12 = w12 / 5: w13 = w13 / 5 '21Oct16 was earlier w12 = w12 / 200: w13 = w13 / 200 '26Jun17 to avoid overflow. w12 = w12 * w12 w13 = w13 * w13 w12 = w12 + w13 'now need SQRT w6 = SQR w12 ''22Feb15 use SQR function instead of iteration '26Jun17 w6 = w6 * 10: w6 = w6 / 161 '21Oct16 was * 100 convert Km to Miles by / 1.61 w6 = w6 * 38 / 100 '26Jun17 get right answer from Home to Alpha of about 11km. call saydistm '26Jun17 speak range in Km (was miles) (09Feb15 was kilometres) endif else endif 'calculate approximate target heading based on difference in lat/lons. 'This based on just minutes if two positions have same lat and lon degrees. 'If different degrees, then scale minutes and modify based on degrees. 'Scaling is used to avoid overflow when a long distance appart. 'This arithmetic overwrites the values of GPS and Destination lat/lons. 'Temp variables available: w6,w11,w12. '1v change minutes if not same values of degrees for GPS and Destination. if b0 <> b14 or b1 <> b15 then 'NOT same value of degrees in lat or lon 'work with units of 0.1 minutes of lat/lon i.e. 100x less accurate. w11 = b0 * 600: w12 = w4 / 100 '1t was *300 and /50 GPS lat/lon w4 = w11 + w12 'latitude LSD is 0.1 minutes = 1/600th degree w11 = b1 * 600: w12 = w5 / 100 w5 = w11 + w12 'longitude LSD is 0.1 minutes = 1/600th degree w11 = b14 * 600: w12 = w8 / 100 'Destination lat/lon w8 = w11 + w12 'latitude LSD is 0.1 minutes = 1/600th degree '26Jan15 Problem? w11 = b15 * 600: w12 = w9 / 100 w9 = w11 + w12 'longitude LSD is 0.1 minutes = 1/600th degree '26Jan15 Problem? endif '09Feb15 use new COSLAT function near end of program, instead of earlier approximation. '13Feb15 call coslat 'get 100 * Cos(Lat) where Lat is average of GPS and Destination latitudes. '13Feb15 b1 = b22 / 10 'original code using b1 can now be used.... 'now just the values of minutes (or scaled minutes) can be used ... if w8 > w4 then 'Dest Lat > GPS Lat w12 = w8 - w4 'Diff Lat if w9 < w5 then 'Dest Lon east of GPS Lon w13 = w5 - w9 'Diff lon gosub scalew12w13 'scale if either more than 1000 to avoid overflow else 'Dest Lon west of GPS Lon w13 = w9 - w5 'Diff lon gosub scalew12w13 'scale if either more than 1000 to avoid overflow w6 = 359 ''12Feb15 was 360 w13 = w6 - w13 'was w13 = 360 - w13 endif else 'Dest Lat < GPS Lat w12 = w4 - w8 'Diff Lat if w9 < w5 then 'Dest Lon east of GPS Lon w13 = w5 - w9 'Diff lon gosub scalew12w13 'scale if either more than 1000 to avoid overflow w6 = 180 w13 = w6 - w13 'was w13 = 180 - w13 else 'Dest Lon west of GPS Lon w13 = w9 - w5 'Diff lon gosub scalew12w13 'scale if either more than 1000 to avoid overflow '12Feb15 w6 = 180 w13 = w13 + 180 'was w13 = w6 + w13 'was w13 + 180 endif endif w1 = w13 'make this the target heading THIS MAKES THE METHOD ACTIVE 'W1 = Direction to target waypoint. e.g. 270 if directly west to Bravo from Alpha. 'W2 = GPS Heading/Course ( actual direction of travel ). e.g. 260 if drift south due to north wind goto afterscalesub '1t was x so subroutine near place called from - for clarity scalew12w13: '13Feb15 scale w12 and w13 to less than 600 so can * by up to 100 IF w12 < 600 AND w13 < 600 THEN endscale w12 = w12 / 2: w13 = w13 / 2 'make both smaller GOTO scalew12w13 'repeat until both less than 600 endscale: call coslat 'get 100 * Cos(Lat) where Lat is average of GPS and Destination latitudes. w13 = w13 * b22 / 100 '13Feb15 old code was w13 = w13 * b1 / 10 '1v b1 was COSLAT if w13 < w12 then '22Feb15 use ATAN function instead of approximation w13 * 45 / w12 w13 = w13 * 100 / w12 w13 = atan w13 else w12 = w12 * 100 / w13 w12 = atan w12 w13 = 90 - w12 endif return gpsonlysteer: '11Jul17 afterscalesub: '01May17 Target Heading now in W1 ready for steering logic (said W13 before ) 'W1 = Direction to target waypoint. e.g. 270 if directly west to Bravo from Alpha. 'W2 = GPS Heading/Course ( actual direction of travel ). e.g. 260 if drift south due to north wind. '11Jul17 w24 = 3 'cycles in control loop '11Jul17 "Noddy" Code from version "Hotel" AP28X2H.BAS below (+ mods ) '31Jul17 "Quebec" compare W1 with W24 CBWP to reduce Cross Track Error aftercalchdg: 'W1 now holds the Target Heading that the boat must steer towards 'Quebec 31Jul17 sertxd ( 13, 10, "TgtHdg=",#w13, " " ) '12Jul17 trace output back to PC PAUSE FOR1SEC '1v remove servo twitch ? 28Jan13 move here 1sec->2sec->1sec '11Jul17 "Noddy" SEROUT 2, T9600_16, ( #w1, ". " ) 'TTS output 15Feb15 without "degrees" (free memory) call saytgtheading ''11Jul17 say target heading in w1 call saycourse '11Jul17 e.g. say "GPS Heading 349" call sayspeed ''11Jul17 Speak speed. e.g. "Speed is 1.5 knots" 'call sayit '11Jul17 '31Jul17 "Quebec" compare W1 with W24 CBWP to reduce Cross Track Error if w1 > w24 then 'Trgt Hdg > CBWP ? e.g. 280 > 270 need to correct right w3 = w1 - w24 if w3 < 3 then 'skip cross-track correction if within 2 degrees goto endcrosstrack endif if w3 < 60 then 'limit correction w1 = w1 + 30 'correct by 30 degrees right endif else 'Trgt Hdg > CBWP ? e.g. 260 < 270 need to correct left w3 = w24 - w21 if w3 < 3 then 'skip cross-track correction if within 2 degrees goto endcrosstrack endif if w3 < 60 then 'limit correction w1 = w1 + 390 'correct by 30 degrees right endif endif if w1 > 359 then w1 = w1 - 359 'Target Heading now 0 to 359 and Cross Track Corrected endif endcrosstrack: 'MOVE RUDDER TO TURN TOWARDS TARGET HEADING. 'calculate the rudder position for steering logic 'Integers are unsigned, so cannot have any negative numbers. if w2 > w1 then 'GPSHdg > TgtHdg w3 = w2 - w1 w12 = w3 '28Jan13 hold error for TTS if w3 < 180 then 'turn left goto turnleft else 'turn right w3 = 360 - w3 w12 = w3 '28Jan13 hold error for TTS goto turnright endif else w3 = w1 - w2 w12 = w3 '28Jan13 hold error for TTS if w3 < 180 then 'turn right goto turnright else 'turn left w3 = 360 - w3 w12 = w3 '28Jan13 hold error for TTS goto turnleft endif endif 'Trim logic based on use of b21 holding rudder centre 'Servo throw is 50. A turn to max throw, increments centre by 5 if <30. 'Any turn in oppsite direction to trim, zeros the trim. '1j speed in 0.1 knots in w13 is now used to give less rudder time if faster turnleft: '15Jul17 sertxd ( ", Left ",#w12, " " ) '11Jul17 trace output back to PC SEROUT 2, T9600_8, ( " left ",#w12, 10 ) 'TTS output '18Oct16 if w3 > 30 then '1f rudder trim logic if b21 < TRIMMAXLEFT then 'add rudder trim to left b21 = b21 + TRIMINC endif '18Oct16 endif 'sertxd ( " LEFT=",#w3 ) 'trace output back to PC w3 = w3 * THROW / MAXDERR 'proportion to error w3 = CENTRE + w3 gosub limitrudder '1u if b21 < CENTRE then '11Jul17 was ABITLEFT gosub zerotrim 'if turn in opposite direction endif SERVOPOS 1,w3 'move servo 1 to steer boat based on GPS heading PAUSE FORP5SEC '1j hold rudder a minimum of 0.5 sec at high speed 'Quebec 31Jul17 sertxd ( ", Speed*10= ",#w21, " " ) '12Jul17 trace output back to PC if w21 < 10 then ''11Jul17 was w13 1j if speed <1 knots, hold rudder for extra 1 second PAUSE FOR1SEC '1x endif if w21 < 5 then ''11Jul17 was w13 1j if speed <0.5 knots, hold rudder for extra 1 second PAUSE FOR1SEC '1x endif PAUSE FOR1SEC '1x SERVOPOS 1,b21 '1v was CENTRE return rudder to biased centre 'SEROUT 2, T9600_16, ( "S left",#w12, 10 ) 'TTS output 'SEROUT 2, T9600_16, ( " left",#w12, 10 ) 'TTS output goto waitgps turnright: '15Jul17 sertxd ( ", Right ",#w12, " " ) '11Jul17 trace output back to PC SEROUT 2, T9600_8, ( " right ",#w12, 10 ) 'TTS output '18Oct16 if w3 > 30 then '1f rudder trim logic if b21 > TRIMMAXRIGHT then 'add rudder trim to right b21 = b21 - TRIMINC endif '18Oct16 endif 'sertxd ( " RIGHT=",#w3 ) 'trace output back to PC w3 = w3 * THROW / MAXDERR w3 = CENTRE - w3 gosub limitrudder '1u if b21 > CENTRE then '11Jul17 was ABITLEFT gosub zerotrim 'if turn in opposite direction endif SERVOPOS 1,w3 'move servo 1 to steer boat based on GPS heading PAUSE FORP5SEC '1j hold rudder a minimum of 0.5 sec at high speed 'Quebec 31Jul17 sertxd( ", Speed*10= ",#w21, " " ) '12Jul17 trace output back to PC if w21 < 15 then '11Jul17 was w13 1j if speed <1.5 knots, hold rudder for 1 second PAUSE FOR1SEC '1x endif if w21 < 10 then ''11Jul17 was w13 1j if speed <1 knots, hold rudder for 1 second PAUSE FOR1SEC '1x endif PAUSE FOR1SEC '1x SERVOPOS 1,b21 '1v was CENTRE return rudder to biassed centre. goto waitgps 'END OF RUDDER CONTROL LOOP. GO BACK TO READ GPS DATA AGAIN. limitrudder: '1u protection limit on rudder movement to max left or right 'symbol LEFT = CENTRE + THROW 'Servo left 'symbol RIGHT = CENTRE - THROW 'Servo right if w3 < RIGHT then w3 = RIGHT endif if w3 > LEFT then w3 = LEFT endif return zerotrim: b21 = CENTRE 'zero rudder trim. Rudder is centred. return '11Jul17 "Noddy" End of code from version "Hotel" '11Jul17 JIBE LOGIC REMOVED 19Jun17 Jibe logic added '******************************************* 'End of code imported from old AUTOP9TTS.BAS 'code below deleted from Juliet on 01May17 servostop: '29July14 switch servo pulses off LOW B.1 return servostart: '29July14 start servo pulses at centre 'call servoon SERVO B.1, CENTRE return 'Following are subroutines to speak things, waggle servo, etc. sayit: SEROUT B.2, T9600_8,( 10 ) 'output 10=LF to trigger buffered TTS return saybegin:' if b47 > 0 then '20Feb15 what to say counter SEROUT B.2, T9600_8, ( "S ") 'TTS output. return '11Jul17 atwaypoint: '11Jul17 ' if b47 > 0 then '22Feb15 '11Jul17 SEROUT B.2, T9600_8, ( " At waypoint." ) 'TTS output '11Jul17 ' endif '11Jul17 return saydist: '16Jul17 W6 (not b12) is range of destination in 1m steps (was 10m) '08Feb15 change name from playdist '16Jul17 w6 = b12 '* 10 '28Jan13 use w6 '21Oct16 no need for * 10 call saytarget SEROUT B.2, T9600_8, ( #w6," metres." ) 'TTS speak range. e.g. "20 metres" return saydistm: '26Jun17 Km. 09Feb15 now "Miles" 08Feb15 w6 is range of destination in km call saytarget if w6 > 0 then '26Jun17 SEROUT B.2, T9600_8, ( #w6," miles." ) '24Feb15 TTS speak range. e.g. "2 miles" SEROUT B.2, T9600_8, ( #w6," kilometres." ) '26Jun17 TTS speak range. e.g. "12 kilometres" endif return saytarget: '21Feb15 say target based on b46. e.g. "Target Alpha" if b46=1 SEROUT B.2, T9600_8, ( "Target " ) SELECT b46 CASE 88 call saycharters '16Jul17 FOR TEST ONLY CASE 99 call saycorner22 '21Oct16 FOR TEST ONLY CASE 1 call sayalpha CASE 2 call saybravo CASE 3 call saycharly CASE 4 call saydelta CASE 5 call saybase CASE 6 call saybray247 CASE 7 call saylamppost CASE 8 call sayrobin CASE 11 call saychannel CASE 12 call sayweymouth CASE 13 call sayprawle CASE 14 call saylizard CASE 15 call saystartline CASE 16 call sayazores CASE 17 call sayb4finish CASE 18 call sayfinish CASE 19 call saybermuda CASE 20 call sayeastofusa CASE 21 call saycapecod CASE 22 call sayplymouthbay CASE 23 call saykingstonbay CASE 24 call saybreakwater CASE 25 call saymayflower CASE 26 call saybray247f ' CASE 27 call sayalpha '29Apr16 GoldA ' CASE 28 call saybase '29Apr16 GoldBase END SELECT return saytgtheading: '19Feb15 speak target heading in w1 e.g. "351 degrees". SEROUT B.2, T9600_8, ( " at ", #w1," degrees. " ) 'TTS output return sayspeed: '19Feb15 use b42,b43 not b6 and b7 = ascii speed 1.5 SEROUT B.2, T9600_8,( " Speed ",b44,".",b45," knots." ) '19Feb15 speak speed (no S) return saytime: '20Feb15 speak time in b22,b23,b24,b25,b26,b27 as asci. e.g. "8 24" for 0824 if b47 > 0 then '20Feb15 test what to say counter b22 = b22 - 48: b23 = b23 - 48 b22 = b22 * 10 + b23 'hours b24 = b24 - 48: b25 = b25 - 48 SEROUT B.2, T9600_8, ( "GPS Time ",#b22," ") '21Jul17 TTS output hours '21Jul17 SEROUT B.2, T9600_8, ( #b22," ") 'TTS output hours 10Dec16 quicker if b24 = 0 then 'Minutes 00 to 09 SEROUT B.2, T9600_8, ( #0,#b25,". ") 'TTS output minutes else '10 to 59 b24 = b24 * 10 + b25 SEROUT B.2, T9600_8, ( #b24, ". ") 'TTS output minutes endif '10Dec16 add + seconds b26 = b26 - 48: b27 = b27 - 48 b26 = b26 * 10 + b27 'seconds SEROUT B.2, T9600_8, ( " ",#b26, " seconds. " ) 'TTS output seconds 'Juliet 'Juliet 28Apr17 SEROUT B.2, T9600_8, ( " + ",#b26, " seconds. " ) 'TTS output seconds w27 = b26 '20Apr17 hold time seconds for what is spoken w27 = w27 / 15 'values 0,1,2,3 return waggle: 'waggle servo 1 on startup to show that working - controls rudder SERVO 1,CENTRE 'start servo control process - middle PAUSE FOR1SEC '28Apr17 Juliet waggle speeded up gosub zerotrim SERVOPOS 1,LEFT 'move servo to position 1 PAUSE FOR1SEC '28Apr17 SERVOPOS 1,CENTRE 'move servo to middle position PAUSE FOR1SEC '28Apr17 Juliet SERVOPOS 1,RIGHT 'move servo to position 2 PAUSE FOR1SEC '28Apr17 Juliet SERVOPOS 1,CENTRE 'move servo to middle position PAUSE FOR1SEC '28Apr17 Juliet return coslat: '09Feb15 returns COS(Lat) * 100 in b22. e.g. COS(50)=0.64. b22=64. '13Feb15 COS=114-Lat e.g. 64 for Lat 51. Was 115 so less accurate at 51. 'Average Lat between GPS in b0 and Destination waypoint in b14 is used. b22 = b0 + b14: b22 = b22 / 2 'average. '22Feb15 b23 = 114: b22 = b23 - b22 'approximation in b22 is 115 - Lat in degrees. 13Feb15 b22 = COS b22 '22Feb15 use COS function instead of approx 115 - Lat return saycharters: 'FOR TEST ONLY SEROUT B.2, T9600_8, ( " Charters. " ) 'TTS output. return saycorner22: 'FOR TEST ONLY SEROUT B.2, T9600_8, ( " Corner 22. " ) 'TTS output. return saylamppost: SEROUT B.2, T9600_8, ( " Lamp post. " ) 'TTS output. return sayrobin: SEROUT B.2, T9600_8, ( " 22 Front. " ) 'TTS output.'17Oct16 was Robin's Lawn return saybackyard: SEROUT B.2, T9600_8, ( " Back Garden. " ) '17Oct16 was Robin's Back Yard. return sayalpha: SEROUT B.2, T9600_8, ( " Alpha. " ) 'TTS output. return saybravo: SEROUT B.2, T9600_8, ( " Braavo. " ) 'TTS output. return saycharly: SEROUT B.2, T9600_8, ( " Charly. " ) 'TTS output. return saydelta: SEROUT B.2, T9600_8, ( " Delta. " ) 'TTS output. return saybase: SEROUT B.2, T9600_8, ( " Base. " ) 'TTS output. return saybray247: '11Jul17 back in SEROUT B.2, T9600_8, ( " 24 7 North Position. " ) '12Mar16 TTS output return saybray247F: '11Jul17 back in SEROUT B.2, T9600_8, ( " 24 7 Position South. " ) '12Mar16 TTS output return saycorner: SEROUT B.2, T9600_8, ( " Snoopy Corner. " ) 'TTS output return sayisland: '11Jul17 back in SEROUT B.2, T9600_8, ( " Near Island. " ) 'TTS output return saydiversion: '11Jul17 back in SEROUT B.2, T9600_8, ( " Diversion. " ) 'TTS output return sayclubhouse: SEROUT B.2, T9600_8, ( " Club House. " ) 'TTS output return '21Feb15 UK-USA waypoints 11 Channel ... 25 Mayflower saychannel: SEROUT B.2, T9600_8, ( " Channel. " ) 'TTS output return sayweymouth: SEROUT B.2, T9600_8, ( " Weymouth. " ) 'TTS output return sayprawle: SEROUT B.2, T9600_8, ( " Prawle. " ) 'TTS output return saylizard: SEROUT B.2, T9600_8, ( " Lizard. " ) 'TTS output return saystartline: SEROUT B.2, T9600_8, ( " Start Line. " ) 'TTS output return sayazores: SEROUT B.2, T9600_8, ( " Azores. " ) 'TTS output return sayb4finish: '11Jul17 back in SEROUT B.2, T9600_8, ( " Before Finish Line. " ) 'TTS output return sayfinish: SEROUT B.2, T9600_8, ( " Finish Line. " ) 'TTS output return saybermuda: '11Jul17 back in SEROUT B.2, T9600_8, ( " Bermuda. " ) 'TTS output return sayeastofusa: SEROUT B.2, T9600_8, ( " East of USA. " ) 'TTS output return saycapecod: SEROUT B.2, T9600_8, ( " Cape Cod. " ) 'TTS output return sayplymouthbay: SEROUT B.2, T9600_8, ( " Plymouth Bay. " ) 'TTS output return saykingstonbay: SEROUT B.2, T9600_8, ( " Kingston Bay. " ) 'TTS output return saybreakwater: SEROUT B.2, T9600_8, ( " Breakwater. " ) 'TTS output return saymayflower: SEROUT B.2, T9600_8, ( " May Flower. " ) 'TTS output return '06Jul17 removed after suggestion that reading compass might upset servo control '06Jul17 'Subs readcompass and Mag2true taken from AUTOPX1.BAS + Timeout in SERIN '06Jul17 readcompass: 'read Compass heading 0 to 359 into W25. 360=No compass available. '06Jul17 IF W25 = 399 THEN '06Jul17 skip use of compass if not found on startup '06Jul17 GOTO endreadcompass '06Jul17 ENDIF '06Jul17 W25 = 361 'means no compass or not working '06Jul17 w12 = 3610 'should get overwritten 0 to 3590 if compass works '06Jul17 HIGH B.6 'switch ON the CMPS10 compass '06Jul17 PAUSE FORP1SEC 'give Compass 0.1 sec to start '06Jul17 SEROUT B.4, T9600_8, ( 19 ) '19 = 1x13 = get angle as two bytes 0-3590 '06Jul17 SERIN [1000], B.5, T9600_8, b25,b24 'get w12 0-3590 from two bytes was w6,b13,b12 '06Jul17 LOW B.6 'switch off compass '06Jul17 w12 = w12 / 10 'degrees 0-359 '06Jul17 'sertxd ( 13,10,"Mag:",#w12 ) '06Jul17 if w12 < 360 then '06Jul17 call mag2true 'convert compass magnetic heading to true heading in W25 '06Jul17 endif '06Jul17 'sertxd ( 13,10," True:",#w25 '06Jul17 endreadcompass: '06Jul17 return '06Jul17 mag2true: 'convert magnetic compass heading in w12 (was w6) to true heading using GPS Long in b1 ' values of correction from http://www.ngdc.noaa.gov/geomag-web/ 'Longitude: 70W 60W 20W 1W '50N 18.1 20.1 8.6 1.2 '40N 14.7 16.9 6.9 0.42 '20N 11.3 15.3 7.9 1.4 'Magnetic deviation if >59W then 15, else lon/4+1. 1.2 in UK, 15.3 @25N60W, 15.2@42N70W 'approximation: c = Lon / 5. True = Mag - c '06Jul17 w11 = b1 'Longitude degrees. e.g. 70 '06Jul17 if w11 > 59 then '06Jul17 w11 = 15 'correction is 15 if between 60W Microtransat Finish, and 42N70W USA. '06Jul17 else '06Jul17 w11 = w11 / 4 '06Jul17 w11 = w11 + 1 'correction is Lon/4 +1 if from UK to 60W. e.g. 1 in UK. '06Jul17 endif '06Jul17 w12 = w12 + 360 - w11 'apply correction '06Jul17 if w12 > 359 then '04Dec16 '06Jul17 w12 = w12 - 360 '06Jul17 endif '06Jul17 w25 = w12 'replace GPS* heading by Compass based True heading in W25 29Ap17 *Magnetic '06Jul17 return '06Jul17 removed after suggestion that reading compass might upset rudder servo control '06Jul17 steercompass: '02Dec16 steer on compass to CTS in W26 (not W1) to compensate for drift. 'Notes for future change, after a Bray Lake Test of CTS being used. 'W1 = Direction to target waypoint. e.g. 270 if directly west to Bravo from Alpha. 'W2 = GPS Heading/Course ( actual direction of travel ). e.g. 260 if drift south due to north wind. 'W25 = Compass Heading ( direction the boat is point ). e.g. 280 based on CTS to compensate. 'W26 = Course to Steer (CTS) '06Jul17 call readcompass ''Juliet 28Apr17 w25 holds boat heading from compass 0 to 359. 361 if bad '06Jul17 if W25 > 360 then '06Jul17 call sayit 'Juliet 28Apr17 '06Jul17 goto endofsteer 'quit if compass not working '06Jul17 endif '06Jul17 call sayit 'Juliet 28Apr17 '06Jul17 w24 = 13 'Counter used to limit time in compass steering loop to ~ 15 secs '06Jul17 W26 = W1 'Compass steering CTS directly to waypoint. '06Jul17 steercompass1: '06Jul17 w24 = w24 - 1 '06Jul17 if w24 = 0 then '06Jul17 goto endofsteer 'quit steering loop based on counter '06Jul17 endif '06Jul17 call readcompass 'w25 holds boat heading from compass 0 to 359. 361 if bad '06Jul17 if W25 > 360 then '06Jul17 goto endofsteer 'quit if compass not working '06Jul17 endif '06Jul17 if w25 > w26 then 'Compass > TgtHdg W26 was W1 '06Jul17 w12 = w25 - w26 '06Jul17 if w12 > 180 then '06Jul17 w25 = w12 '06Jul17 w12 = 360 - w25 'because can't do 360 - w12 :-( '06Jul17 goto goright '06Jul17 endif '06Jul17 goleft: '03May17 if w12 > 50 then 'limit to max servo throw at 50 degree error '03May17 w12 = 50 '03May17 endif '06Jul17 w12 = CENTRE + w12 'servo throw left proportional to direction error '06Jul17 call moverudder '03May17 limit rudder RIGHT > w12 < LEFT and move to avoid flip '03May17 SERVOPOS 1, w12 'move rudder servo '06Jul17 else '06Jul17 w12 = w26 - w25 '06Jul17 if w12 > 180 then '06Jul17 w25 = w12 '06Jul17 w12 = 360 - w25 '06Jul17 goto goleft '06Jul17 endif '06Jul17 goright: '03May17 if w12 > 50 then 'limit to max servo throw at 50 degree error '03May17 w12 = 50 '03May17 endif '06Jul17 w12 = CENTRE - w12 'servo throw right proportional to direction error '06Jul17 call moverudder '03May17 limit rudder RIGHT > w12 < LEFT and move to avoid flip '03May17 SERVOPOS 1, w12 'move rudder servo '06Jul17 endif '06Jul17 PAUSE FOR1SEC '1sec update rate '06Jul17 goto steercompass1 'continue steering loop saycourse: '19Feb15 speak boat course in w2 e.g. "355 degrees". 'Quebec 31Jul17 sertxd ( ", Course=",#w2, " " ) '11Jul17 trace output back to PC SEROUT B.2, T9600_8, ( " GPS Course ", #w2, "." ) 'TTS output return '11Jul17 saysteerto: '02Dec16 speak CTS Course To Steer '11Dec16 13 (CR) needed? SEROUT B.2, T9600_8, ( "S Steer to ", #w26, 13, 10 ) 'TTS output '11Jul17 SEROUT B.2, T9600_8, ( " Steer to ", #w26, 10 ) '29Apr17 TTS output 20Apr17 no S '11Jul17 return '06Jul17 readandsaycompass: 'Juliet 28Apr17. Needs Sayit or 10(LF) to speak it. '06Jul17 call readcompass 'get compass heading into W25 0 to 359 or 361 = No Compass '06Jul17 call saycompasshdg '06Jul17 return '06Jul17 saycompasshdg: '30Nov16 speak boat heading in w25 e.g. "351 degrees". '06Jul17 if w25 < 361 then '28Apr17 Juliet was 360 '06Jul17 SEROUT B.2, T9600_8, ( " Compass ", #w25, "." ) '29Apr17 TTS output '06Jul17 else '06Jul17 if W25 <> 399 THEN '06Jul17 don't say if no compass on startup '06Jul17 SEROUT B.2, T9600_8, ( " No Compass. " ) '28Apr17 Juliet '06Jul17 endif '06Jul17 endif '06Jul17 return 'THAT'S IT FOLKS - END OF THE PICAXE AUTOPILOT PROGRAM ! :-)