'MMAP1B.BAS Snoopy's MicroMite AutoPilot "Bravo" (c)2017 Robin Lovelock www.GPSS.co.uk based on MMAP1A.BAS Tuesday 19th September 2017.
  'MMAP1B.BAS add Horseshoe Lake Test waypoints "Buoy" and "Jetty". Fix bug - negative direction between Waypoint in CBWP%.
  'MMAP1A.BAS Snoopy's MicroMite AutoPilot "Alpha" (c)2017 Robin Lovelock www.GPSS.co.uk based on MMAUTOP2.BAS 09Jul17.
  'MMAP1A.BAS 19Sep17. After test on 19Sep17 with DoCTEC in wrong direction.
  'MMAP1A.BAS 12Sep17. Bigger Bray Lake Test to include the two 24/7 waypoints in array entries [9] and [10]
  'MMAP1A.BAS 07Sep17 Introduce Sub DOCTEC ( Do Cross Track Error Correction ). Waypoint "Back Garden" -> "Lamp Post".
  'MMAP1A.BAS 02Sep17. Waypoint Range 0.003 -> 0.015 and "Braavo". Steering delays and silent periods ???
  'MMAP1A.BAS 29Aug17 debug things like pauses in speech. Tested at Bray Lake on 31st August: Overshoot but did two circuits OK.
  'MMAP1A.BAS 28Aug17 Rudder Centred when long report every minute. Include nearest waypoint. e.g. "5 metres west of Robin's Corner".
  'MMAP1A.BAS 26Aug17 Tea-Tray testing on Board#1. Add "Target 070 " before "Steer left by 33".
  'MMAP1A.BAS 25Aug17 Check GPS data input for $GPRMC sentences and avoid lag in steering. Seems OK. Swap GPS on MM Board#2 for BR355-S4.
  'MMAP1A.BAS 24Aug17 introduce CBWP (Course Between Waypoint ) Cross Track Correction logic from Picaxe 28X2 Autopilot "Quebec" (AP28X2Q.BAS)
  'MMAP1A.BAS 23Aug17 Different waypoint data code to make easier to change. Speaks "Snoopy" on TTS to distinguish from Boat 11 Picaxe over radio link.
  'MMAUTOP2.BAS 09Jul17 Introduce RESTORE and SAVE for the destination waypoint. Make startup default Bray Lake "Alpha". TTS Voice.
  'MMAUTOP2.BAS 15Aug17 prepare for Bray Lake Tests with Roy's Board#2 e.g. Less speech but Australian. TT Test of 0940: bug in GPS direction fixed.
  'MMAUTOP2.BAS 15Aug17 speed up ~20 sec control loop to nearer 7 sec. Put in LEFT!, RIGHT!, CENTRE!, BIASED! BIASINC! for Rudder Bias logic
  'MMAUTOP2.BAS 18Aug17 After first sail on 16th: make rudder timing speed dependent. 1.5 sec + 1 sec if speed < 1.5 + 1 sec if < 1 (from AP28X2Q).
  'NEEDS MORE TEA TRAY TESTING TO PROVE RUDDER TIMING NOW CORRECT. e.g. Test at 0830 on 18th showed control loop too fast. Also at wrong place ?
  'MMAUTOP2.BAS 18Aug17 Speak GPS Time
  '
  ' introduce old convention of names followed by !,% or $. Avoid string arrays ? Arrays documented ?
  '14Aug17 after Roy's Board#2 working with USB Console I/F based on Peter's email. Ozify, etc :-)
  'MMAUTOP1.BAS 17Mar17 Normal TT pitch and voice. OPEN&CLOSE GPS to try and reduce buffering delay.
  'MMBAK10 17Mar17 1820
  '16Mar17 Add Waypoint array structures and look at Roy1.bas e.g. OPTION BASE 1 syntax of arrays, etc.
  '15Mar17 Speak speed and direction moving. Direction to Waypoint and rudder servo steering.
  'MMBAK9 16Mar17 backup at 1720 after filming. Walking Doorstep, Robin's Corner, Back Garden, etc.'MMAUTOP2.BAS 15Aug17
  'MMBAK8 15Mar17 backup at 2110. Speaking GPS speed and direction.
  'MMBAK7 10Mar17 backup at 1220 working GPS handling.
  '10Mar17 Less words when GPS settings lost based on loopcounter%
  'MMBAK6 07Mar17 backup at 0930.
  '07Mar17 Display all NMEA lines; Remove "Reading the GPS"; add Status$ = "A" etc.
  'MMBAK5 07Mar17 copy of Sunday 26Feb17 GPS Coms better. TTT until 6Mar17 (Kingston). Needs ,A, test. BR355-S2 Settings.
  'MMBAK4.BAS backup of version with a good Wisley Test on Sunday 19 Feb 2017. Unreliable GPS but in Km > 500m
  'See label Verytop below for version of this software. e.g. time and date of last update.
  '1215 Sunday 19Feb17 made backup in MMBAK3.BAS of copy used for first Wisley test. "~4 million metres", etc
  '1220 Friday 17Feb17 made backup in MMBAK2.BAS - see below.
  'MMBAK2.BAS is a backup for MMAUTOP1.BAS at 1215 Fri 17 Fev 2017. Roy's system started OK near patio door.
  'Seems to run as it should, but with periods where it's waiting for the GPS. Also TTS voice not same.
  'Lots of work needed to clean up GPS handling and TTS ( e.g. less delays, repeat TTS setup later )
  'Can then start to put in waypoints, in arrays, and automatically change target.
  'Soon after, can start puting in the rudder servo steering logic - equivalent of all earlier Picaxe Autopilots.
  'Can then get the Compass working, exploiting Roy's pioneering work. Lots of testing can start now. e.g. on the road.
  'MMAUTOP1.BAS Snoopy's MicroMite AutoPilot (c)2017 Robin Lovelock www.GPSS.co.uk
  
  
  'See label Verytop below for version of this software. e.g. time and date of last update.
  
  '17Feb17 put servo waggle earlier, so TTS has time to power up and have same voice as when connected to PC
  '17Feb17 remove delays put there to get GPS working on 16Feb17. i.e. speed up and improve everything.
  
  ' OPTION EXPLICIT         'all variables to be declared - not just yet Roy :-)
  OPTION BASE 1           '16Mar17 all arrays indexed from 1. Taa Roy :-)
  
  LEFT! = 2 : CENTRE! = 1.5: RIGHT! = 1: BIASED! = 1.5: BIASINC! = 0.05 'MMAUTOP2.BAS 15Aug17 Servo positions and variables for rudder bias logic.
  'MMAP1A.BAS 28Aug17 was 0.01 MMAUTOP2.BAS 15Aug17 BIASINC! was 0.05
  
  'MMAP1A 23Aug17 New Waypoint definition; all Bray Lake Test. Was only at Robin's home, then Bray Lake Base <--> Alpha.
  DIM INTEGER II%, JJ%, KK% 'MMAP2A adopt old convention of % for integer
  DIM INTEGER CBWP% 'MMAP1A.BAS 24Aug17 CBWP ( Course Between Waypoint ) used in Cross Track Correction logic
  CONST MAXNOWP = 12 'MMAP1B.BAS 22Sep17 was 10 Maximum number of waypoints held by program. i.e. array sizes for Waypoint Data.
  DIM STRING WPName$(MAXNOWP)
  DIM FLOAT WPlat!(MAXNOWP)  'MMAP1A.BAS 23Aug17  = (51.397000,51.396951,51.396995,51.4995, 51.5001) 'MMAUTOP2 9Aug17 WP Latitude
  DIM FLOAT WPlon!(MAXNOWP)  'MMAP1A.BAS 23Aug17 = (-0.659833,-0.660053,-0.660374,-0.6910,-0.6901) 'MMAUTOP2 9Aug17 WR Longitude
  DIM FLOAT WPRad!(MAXNOWP)  'MMAP1A.BAS 23Aug17  = (0.002,0.002,0.002,0.005,0.005) 'MMAUTOP2 9Aug17 WP Radius e.g. 0.005 = 5 metres
  DIM INTEGER WPNextWP%(MAXNOWP)  'MMAP1A.BAS 23Aug17  = ( 3, 1, 2, 5, 4 ) 'Next Waypoint to go to if within radius of current destination waypoint
  WPName$( 1 ) = "Robin's Corner" : WPlat!(1) = 51.397000 : WPlon!(1) = -0.659863 : WPRad!(1) = 0.002 : WPNextWP%( 1 ) = 3 '29Aug17 was 9833
  WPName$( 2 ) = "Robin's Doorstep" : WPlat!(2) = 51.396980 : WPlon!(2) = -0.660093 : WPRad!(2) = 0.002 : WPNextWP%( 2 ) = 1 '29Aug17 was 951 053
  'WPName$( 3 ) = "Back Garden" : WPlat!(3) = 51.396995 : WPlon!(3) = -0.660406 : WPRad!(3) = 0.002 : WPNextWP%( 3 ) = 2 
  WPName$( 3 ) = "Lamp Post" : WPlat!(3) = 51.39740 : WPlon!(3) = -0.65956 : WPRad!(3) = 0.003 : WPNextWP%( 3 ) = 2 
  WPName$( 4 ) = "Base" : WPlat!(4) = 51.5001 : WPlon!(4) = -0.6901 : WPRad!(4) = 0.003 : WPNextWP%( 4 ) = 5 'at Base go to Alpha 
  WPName$( 5 ) = "Alpha" : WPlat!(5) = 51.4995 : WPlon!(5) = -0.6910 : WPRad!(5) = 0.015 : WPNextWP%( 5 ) = 6 'at Alpha go to Bravo. 02Sep17 was 3m
  WPName$( 6 ) = "Braavo" :   WPlat!(6) = 51.4995 : WPlon!(6) = -0.6930 : WPRad!(6) = 0.015 : WPNextWP%( 6 ) = 9 'at Bravo go to 24/7 North 12Sep17
  WPName$( 7 ) = "Charlie" : WPlat!(7) = 51.4990 : WPlon!(7) = -0.6920 : WPRad!(7) = 0.015 : WPNextWP%( 7 ) = 8 'at Charlie go to Delta. 02Sep17 was 3m
  WPName$( 8 ) = "Delta" :   WPlat!(8) = 51.4990 : WPlon!(8) = -0.6900 : WPRad!(8) = 0.015 : WPNextWP%( 8 ) = 4 'at Delta go back to Base. 02Sep17 was 3m
  WPName$( 9 ) = "24 7 North" :   WPlat!(9) = 51.50025 : WPlon!(9) = -0.6950 : WPRad!(9) = 0.015 : WPNextWP%( 9 ) = 10 'at 24/7 North goto South.
  WPName$( 10 ) = "24 7 South" :   WPlat!(10) = 51.49933 : WPlon!(10) = -0.6950 : WPRad!(10) = 0.015 : WPNextWP%( 10 ) = 7 'at 24 7 South goto Charlie. 12Sep17 was 3m
  'MMAP1B.BAS 22Sep17 add Horseshoe Lake Test waypoints Jetty and Buoy
  WPName$( 11 ) = "Horseshoe Lake Jetty" :   WPlat!(11) = 51.35201 : WPlon!(11) = -0.82406 : WPRad!(11) = 0.010 : WPNextWP%( 11 ) = 12 'within 10m of Jetty goto Buoy
  WPName$( 12 ) = "Horseshoe Lake Buoy" :   WPlat!(12) = 51.35333 : WPlon!(12) = -0.825628 : WPRad!(12) = 0.005 : WPNextWP%( 12 ) = 11 'within 5m beyond Buoy goto Jetty

  
Sub CALDIR (xdif!, ydif!, idir%)
  If Abs(ydif!) < .001 Then
    If xdif! > 0 Then
      idir% = 90
    Else
      If xdif! < 0 Then idir% = 270
    End If
  Else
    ydif! = ydif! + 0.0001 '07Mar17 need to protect from division by zero + 0.01m
    tanval! = xdif! / ydif!
    If xdif! > 0 Then
      If ydif! > 0 Then
        adir! = Atn(tanval!)
      Else
        adir! = 3.14159 + Atn(tanval!)
      End If
    Else
      If ydif! > 0 Then
        adir! = 2 * 3.14159 + Atn(tanval!)
      Else
        adir! = 3.14159 + Atn(tanval!)
      End If
    End If
    adir! = adir! * 180 / 3.14159
    idir% = adir!
  End If
  If idir% >= 360 Then '22Sep17 Blocks needed for -ve CBWP% ?
    idir% = idir% - 360
  ENDIF
  If idir% < 0 Then '22Sep17 Blocks needed for -ve CBWP% ?
    idir% = idir% + 360
  ENDIF
End Sub
  
Sub LLLLTORB (lat0!, lon0!, lat1!, lon1!, r!, brg!)
  'calculate range and bearing from lat longs. This is a simple approximation
  rads! = 3.14159 / 180
  dlon! = lon1! - lon0!
  h180! = 0
  If Abs(dlon!) > 180 Then
    dlon! = Sgn(dlon!) * (360 - Abs(dlon!))
    h180! = 180
  End If
  If lat0! = lat1! Then
    r! = 111.05 * Abs(dlon!) * Cos(lat1! * rads) '111 to 111.05 v5.6c
    If lon1! > lon0! Then brg! = 90 Else brg! = -90
  Else
    dx! = (dlon! * Cos(lat1! * rads!)) * 111.05
    dy! = (lat1! - lat0!) * 111
    r! = Sqr(dx! * dx! + dy! * dy!)
    If h180! = 180 Then dy! = -dy!
    CALDIR (dx!, dy!, i%)
    brg! = i% + h180!
    If brg! > 360 Then 
      brg! = brg! - 360
    ENDIF
    IF brg! < 0 THEN '22Sep17 Blocks needed for -ve CBWP% ?
      brg! = brg! + 360 '02Sep17
    ENDIF
  End If
End Sub
  
SUB DIR2NSEW ( GPSHDG%, NSEW$ )
  'converts a direction 0 to 359 into text. e.g. 270 = "West".
  IF GPSHDG% > 338 OR GPSHDG% <= 22 THEN
    nsew$ = "North"
  ENDIF
  IF GPSHDG% > 22 AND GPSHDG% <= 67 THEN
    nsew$ = "North East"
  ENDIF
  IF GPSHDG% > 67 AND GPSHDG% <= 112 THEN
    nsew$ = "East"
  ENDIF
  IF GPSHDG% > 112 AND GPSHDG% <= 157 THEN '02Sep17
    nsew$ = "South East"
  ENDIF
  IF GPSHDG% > 157 AND GPSHDG% <= 202 THEN
    nsew$ = "South"
  ENDIF
  IF GPSHDG% > 202 AND GPSHDG% <= 247 THEN
    nsew$ = "South West"
  ENDIF
  IF GPSHDG% > 247 AND GPSHDG% <= 292 THEN
    nsew$ = "West"
  ENDIF
  IF GPSHDG% > 292 AND GPSHDG% <= 338 THEN
    nsew$ = "North West"
  ENDIF
END SUB

SUB DoCTEC ( CBWP%, CDTWP%, CCTWP% ) 'MMAP1A.BAS 07Sep17 DOCTEC = Do Cross Track Error Correction
  'from CBWP% = Course Between Waypoints and CDTWP% = Course Direct To Waypoint. Output CCTWP% = Corrected Course to Waypoint.
  'All Courses in degrees clockwise from true North. e.g. 90=East, 180=South, 225=South West, 270=West. Beware either side of 0=North.
  'Note that using ERROR% as a variable gives a syntax error, so use HERROR% instead
  CCTWP% = CDTWP% 'default is for output corrected heading to be direct heading
  IF CBWP% > 359 THEN '19Sep17 skip if no Course Between WayPoint
    GOTO endofsub
  ENDIF
  HERROR% = CDTWP% - CBWP% 'eg. +45 if heading 180=south from Base instead of 225=South West to Alpha
    IF ABS( HERROR% ) < 3 THEN 'Don't apply correction if within 2 degrees of track
    GOTO endofsub
  ENDIF
  IF HERROR% < 0 THEN 
    HERROR% = HERROR% + 360
  ENDIF
  IF HERROR% > 180 THEN
    HERROR% = 360 - HERROR%
  ENDIF 'Now -180 > ERROR% > 180
  IF ABS( HERROR% ) < 90 THEN 'Don't apply correction if more than 90 degrees off track
    IF HERROR% > 0 THEN
      CCTWP% = CDTWP% + 30 '19Sep17 Aim right by 30 degrees to come on track
    ELSE
      CCTWP% = CDTWP% - 30 '19Sep17 Aim left by 30 degrees to come on track
    ENDIF
  ENDIF
  IF CCTWP% >= 360 THEN
    CCTWP% = CCTWP% - 360 'correct to keep in range 0 to 359 degrees
  ENDIF
  IF CCTWP% < 0 THEN
    CCTWP% = CCTWP% + 360
  ENDIF 'CCTWP% now Course Corrected to Waypoint for steering logic 
 endofsub:
  PRINT "DoCTEC (", str$( CBWP% ),", ", str$( CDTWP% ),", ", str$( CCTWP% ), " )"
END SUB
  
  'Lots of superfluos speech to amuse and to give early warning of memory limitations, etc :-)
verytop:
  OPTION AUTORUN ON 'so that the loaded program will start on power up, when not connected to the softeare development PC
  intro$ = "Good Day Mate ! This is Snoopy's Micro Mite Auto Pilot. The Micro Mite computer is Australian. " '15Aug17

  'version$ = "Snoopy's Experimental Micro Mite Auto pilot version Alpha. Last software update was on Tuesday the 19th of September at 9am U K time."  '19Feb17
  version$ = "Snoopy's Experimental Micro Mite Auto pilot version Braavo. Last software update was on Friday the 22nd of September at 10am U K time."  '19Feb17
 
  'MMAUTOP2.BAS 15Aug17 copyright$ = "This Snoopy Auto pilot software is the Intelectual Property and Copyright of Robin Lovelock of www dot GPSS dot co dot U K " '19Feb17
  thanks$ = "Robin would like to thank all his Grumpy Old Friends, particularly Roy, for their help with the Micro Mite."
  '15Aug17 :-) whoisrobin$ = "Robin is a lazy old bugger who tries to get everybody to do things for him. He says that he is too busy to do paid work."
  '30Aug17 PRINT intro$ '15Aug17
  print version$ 'display on PC console if connected to one for software development and load.
  'MMAUTOP2.BAS 15Aug17 print copyright$
  '30Aug17 PRINT thanks$ 'at least lots of code like this will flag up problems earlier, like memory :-)
  '15Aug17 PRINT whoisrobin$
  '30Aug17 PRINT "Test of speech ... "
  PRINT " "
  
  PAUSE 3000 'Give time on power up for all devices to be up, including the Parallax TTS unit.
  OPEN "COM2:9600" As #2 'Open COM2 for output to the Parallax EMIC Text-To-Speech TTS
  
  PRINT #2, "N2": PRINT #2, "V16": PRINT #2, "W300" '09Aug17 set up Parallax EMIC TTS sound to normal pitch, volume loud, fast 300
  PAUSE 1000 ' 1 sec delay to give TTS unit time to process last strings.
   
  PRINT #2, "S " + intro$ 'MMAUTOP2.BAS 15Aug17 
  PAUSE 7000 'MMAP1A.BAS 25Aug17  was 10000 'give time for TTS speech to be completed, before sending more to the TTS below.
 
  PRINT #2, "S " + version$ 'speak startup details like time and date of last software update.
  PAUSE 10000 '17Mar17 still 10000 give time for TTS speech to be completed, before sending more to the TTS below.
    
  PRINT #2, "S " + thanks$
  PAUSE 7000 'MMAP1A.BAS 25Aug17  was 10000 'give time for TTS speech to be completed, before sending more to the TTS below.
  
  'MMAUTOP2.BAS 15Aug17 PRINT #2, "S " + whoisrobin$
  'MMAUTOP2.BAS 15Aug17 PAUSE 10000 'give time for TTS speech to be completed, before sending more to the TTS below.

  CBWP% = 999 'MMAP1A.BAS 24Aug17 CBWP ( Course Between Waypoint ) for Cross Track Error Correction. 999 = Switched Off at start
  
  '17Feb17 put servo waggle earlier, so TTS has time to power up and have same voice as when connected to PC.
  'test the servo by moving it to centre, left, centre, right, centre
  PRINT "Testing the Rudder Servo.." '15Aug17
  PRINT #2, "S Testing the Rudder Servo. First Centre." 'test waggle of servo: centre, left, centre, right, centre. '15Aug17
  SERVO 1, CENTRE! '1.5 'centre
  PAUSE 4000
  PRINT #2, "S Left."
  SERVO 1, LEFT! '2.0 'left
  PAUSE 2000
  PRINT #2, "S Centre."
  SERVO 1, CENTRE! '1.5 'centre
  PAUSE 2000
  PRINT #2, "S Right."
  SERVO 1, RIGHT! '1.0 'right
  PAUSE 2000
  PRINT #2, "S Centre."
  SERVO 1, CENTRE! '1.5 'centre
  PAUSE 3000 'Pauses were added when this code was created on 17Feb17
  
  
  status$ = "V" 'NMEA $GPRMC Staus A=Active V=Void as in $GPRMC,114138.000,A,5130.0011,N, etc. GPS tracking when A.
  
  'Default target destination on power up is Bray Lake Alpha. Changed by VAR RESTORE
  '22Sep17 named$ = WPName$( 5 ): latd! = WPLat!( 5 ):  Lond! = WPLon!( 5 ): radWP! = WPRad!( 5 ) '30Aug17 Bray Lake Alpha 
  named$ = WPName$( 11 ): latd! = WPLat!( 11 ):  Lond! = WPLon!( 11 ): radWP! = WPRad!( 11 ) '22Sep17 Horseshoe Lake Jetty is default

  VAR RESTORE 'MMAUTOP2 09Aug17 Restore target destination from non volatile memory.
  PAUSE 1000 '30Aug17
  PRINT "Initial Target after RESTORE is named$ = ", named$, " at Lat/Lon = ", latd!, lond! '16Mar16
  PRINT #2, "S The last target waypoint stored was " + named$ '30Aug17

  PAUSE 5000  'MMAP1A.BAS 29Aug17 was 3000
 
  loopcounter% = 0 '10Mar17 for shorter "GPS Settings" Warning after startup phase.
  talkcounter% = 0 'MMAUTOP2.BAS 30Aug17 counter used to shorten the control loop - after RESTORE
  biased! = CENTRE! '30Aug17
  
  OPEN "COM1:4800,INV" AS #1 'open COM1 for input from the Globalsat BR355 GPS in NMEA $GPRMC format. Taa Roy for ,INV :-)
  'PAUSE 1000
  
  goto readgps 'MMAUTOP2.BAS 15Aug17
  
  
  
top:
  talkcounter% = talkcounter% + 1 '15Aug17 speed up control loop by speaking less often
  if talkcounter% > 6 then 'MMAP1A.BAS 30Aug17 was 6 then 4 29Aug17 was 3
    talkcounter% = 1
    PAUSE 1000 '29Aug17 see if this delay gets it talking regular report
  endif
  LLLLTORB (latgps!,longps!,latd!,lond!,r!,b! ) '19Sep17 back in
  CDTWP% = b! '19Sep17 Course Direct To WayPoint
  '30Aug17  print "range ", r!, "Km, bearing ", b!, " degrees."
  '02Sep17 brg% = b!: brgt$ = STR$(brg%) 'bearing to destination in degrees clockwise from north as integer and text
  '02Sep17 d% = r!*1000 'distance in metres as integer.
  '19Feb17 speak in miles if more than 500 metres
  IF talkcounter% > 1 then '25Aug17 was one line THEN ENDIF
    goto aftersaytgt '15Aug17 speed up control loop by speaking less often
  ENDIF
  

  
  IF d% < 501 THEN
    dtxt$ = STR$( d% ) 'distance as text in metres
    tmp$ = "Target is " + named$ + " , " + dtxt$ + " metres, at " + brgt$ + " degrees."
    D2TARGET$ = "somewhere"
    DIR2NSEW ( brg%, D2TARGET$ ) 'get direction to target. e.g. "North East"
    tmp$ = tmp$ + " To our " + D2TARGET$ + "."
    PRINT #2, "S " + tmp$: PRINT tmp$   'speak and display. e.g. "Robin's Corner is 16 metres at 70 degrees."
  ELSE '19Feb17
    d% = d% / 100 'in 10om steps
    dtxt$ = STR$( d% / 10 ) 'distance as text in Km e.g. 1.2 Km
    tmp$ = "Target is " + named$ + " , " + dtxt$ + " Kilometres, at " + brgt$ + " degrees."
    D2TARGET$ = "somewhere"
    DIR2NSEW ( brg%, D2TARGET$ ) 'get direction to target. e.g. "North East"
    tmp$ = tmp$ + " To our " + D2TARGET$ + "."
    PRINT #2, "S " + tmp$: PRINT tmp$   'speak and display. e.g. "Robin's Corner is 16 metres at 70 degrees."
  ENDIF
  '02Sep17 PAUSE 8000  '17Mar17 was 5000
aftersaytgt:
  
  '15Mar17 PRINT #2, "S Reading the GPS."   '10Mar17 code moved back from below label readgps:
  '15Mar17 PAUSE 3000
  
readgps:
  loopcounter% = loopcounter% + 1 '10Mar17 Loopcounter% in the Control Loop of ~ 7 second ballpark
  if loopcounter% > 600 THEN 'reset counter roughly every hour ???
    loopcounter% = 1
  ENDIF
'MMAP1A.BAS 25Aug17   PRINT "Loopcounter%=", loopcounter%
  
'MMAP1A.BAS 25Aug17   print "Reading the GPS ..." '07Mar17
  if loopcounter% < 3 then '15Mar17 was 30 then 6
'MMAUTOP2.BAS 15Aug17     PRINT #2, "S Reading the GPS ..."
'MMAUTOP2.BAS 15Aug17     PAUSE 3000
  ENDIF
  
  CLOSE #1 '17Mar17
  PAUSE 1000
  OPEN "COM1:4800,INV" AS #1 'open COM1 for input from the Globalsat BR355 GPS in NMEA $GPRMC format. 17Mar17 less buffering delay
  PAUSE 1000

  
  NMEA$ = "$" 'will hold the whole NMEA sentence
  'e.g. $GPRMC,114138.000,A,5130.0011,N,00041.4333,W,1.25,134.28,021211,,*11 for the Globalsat BR355 (and BR355-S2 )
  DO WHILE INPUT$(1,1) <> "$": LOOP  'search for start of next string Taa Roy :-)
  DO WHILE LEN(NMEA$) < 60 'c$  <> "*" '17Mar17 was 50. Buffer NMEA String until the Asterisk before checksum
    c$ = INPUT$(1,1) '26Feb17
    NMEA$ = NMEA$ + c$
  LOOP
  
 'MMAP1A.BAS 25Aug17  PRINT NMEA$ '07Mar17 earlier so see all NMEA lines input
  if len( NMEA$ ) < 60 then '17Mar17 was 50.
    '17Mar17 CLOSE #1 '17Mar17 
    goto readgps
  ENDIF
  
  'e.g. $GPRMC,114138.000,A,5130.0011,N,00041.4333,W,1.25,134.28,021211,,*11 for the Globalsat BR355 (and BR355-S2 )
  type$ = MID$( NMEA$, 2, 5 ) 'MMBASIC arrays start with 0 for $ - or does it ?
 'MMAP1A.BAS 25Aug17  print "Type = " type$ '26Feb17
  if type$ <> "GPRMC" THEN '19Feb17 try and skip garbage from GPS
'MMAP1A.BAS 25Aug17     PRINT "Not $GPRMC so garbage"
    IF loopcounter% < 2 THEN '15Mar17 still 2
   'MMAUTOP2.BAS 15Aug17    PRINT #2, "S Bad GPS data. Stupid Robin. Setup the GPS, or Fix your software. You need to do more than 5 minutes work on it."
   'MMAUTOP2.BAS 15Aug17    PAUSE 10000 'give time for TTS speech to be completed, before sending more to the TTS below.
    ENDIF
    IF loopcounter% < 4 THEN
  'MMAUTOP2.BAS 15Aug17     PRINT #2, "S Remember to Setup this GPS, Robin. You know it should only output G P R M C sentences."  '15Mar15
  'MMAUTOP2.BAS 15Aug17     PAUSE 8000 'give time for TTS speech to be completed, before sending more to the TTS below.
    ENDIF
GetRMC: '10Mar17 was goto readgps
    '10Mar17 OPEN "COM1:4800,INV" AS #1 'open COM1 for input from the Globalsat BR355 GPS in NMEA $GPRMC format. Taa Roy for ,INV :-)
    '10Mar17 PAUSE 1000
    NMEA$ = "$" '10Mar17 search for $GPRMC sentence MMBASIC names case sensitive ?
    DO WHILE c$ <> "$" '10Mar17 search for $ just as Roy suggested :-)
      c$ = INPUT$(1,1) '26Feb17
    LOOP
    DO WHILE c$ <> "*"
      c$ = INPUT$(1,1) '26Feb17
      NMEA$ = NMEA$ + c$
    LOOP
    NMEA$ = NMEA$ + "*"
    c$ = INPUT$(1,1) 'get checksum two character Hex after the Asterisk *
    NMEA$ = NMEA$ + c$
    c$ = INPUT$(1,1)
    NMEA$= NMEA$ + c$
    '30Aug17 PRINT "NMEA= " + NMEA$ 'MMAP1A.BAS 30Aug17 
    type$ = MID$( NMEA$, 2, 5 ) '10Mar17 Note that arrays start 0 in MMBASIC  - NO :-)
    'MMAP1A.BAS 30Aug17 print "Type (e.g. GPRMC ) = ", type$
    if type$ <> "GPRMC" then
      GOTO GetRMC: '10Mar17 skip other NMEA until $GPRMC
    ENDIF
  ENDIF
  'e.g. $GPRMC,114138.000,A,5130.0011,N,00041.4333,W,1.25,134.28,021211,,*11 for the Globalsat BR355 (and BR355-S2 )
  
  status$ = mid$( NMEA$, 19,1 ) '10Mar17 "A" or "V" Note that arrays start 0 in MMBASIC - or do they ? :-)
  'MMAP1A.BAS 25Aug17  PRINT "NMEA Status$ ( A or V ) = ", status$
  PRINT " " '30Aug17 extra linefeed before NMEA data
  PRINT NMEA$ 'MMAP1A.BAS 25Aug17 only print $GPRMC sentences
  
  if status$ <> "A" then
    tmp$ = "GPS is not yet tracking. Go outside to get better GPS signals ?"
    print tmp$
    print #2, "S " + tmp$
    PAUSE 10000
    GOTO readgps
  endif
  
  '17Mar17 CLOSE #1 '17Mar17 Close GPS to reduce buffering delay

  
speakrange:
  latdeg$ = mid$( NMEA$, 21, 2) '10Mar17
  latmin$ = MID$( NMEA$, 23, 7) '10Mar17
  
  'PRINT "GPS NMEA Latitude = ", latdeg$, ",", latmin$
  
  latgps! = VAL( latdeg$ ) + VAL( latmin$ ) / 60.0
  
  'MMAP1A.BAS 30Aug17  PRINT "GPS NMEA Latitude = ", latdeg$, ",", latmin$, " = Decimal degrees ", latgps!
  
  londeg$ = MID$( NMEA$, 33, 3) '10Mar17
  
  'PRINT "Type$=",type$, "Lat=",latdeg$+latmin$," = ",latgps!
  
  lonmin$ = MID$( NMEA$, 36, 7) '10Mar17
  'PRINT "Lon=", londeg$, " ", lonmin$
  
  longps! = VAL( londeg$ ) + VAL( lonmin$ ) / 60.0
  longps! = - longps  'REMEMBER THE ,W, :-)
  'PRINT " = ",longps!
  'MMAP1A.BAS 30Aug17  PRINT "GPS NMEA Longitude = ", londeg$, ",", lonmin$, " = Decimal degrees ", longps!
  
  LLLLTORB (latgps!,longps!,latd!,lond!,r!,b! ) '02Sep17 moved down from near top, so latest GPS Lat/Lon
  CDTWP% = b! '19Sep17 put back in at top so this code duplicated (in case it was missed)
  '30Aug17  print "range ", r!, "Km, bearing ", b!, " degrees."
  brg% = b!: brgt$ = STR$(brg%) 'bearing to destination in degrees clockwise from north as integer and text
  d% = r!*1000 'distance in metres as integer.

  
  '15Mar15 get GPS Speed in knots GPSSPEED! and direction of movement GPSHDG%
  'e.g. $GPRMC,114138.000,A,5130.0011,N,00041.4333,W,1.25,134.28,021211,,*11 for the Globalsat BR355 (and BR355-S2 )
  tmp$ = MID$( NMEA$, 46, 4 )
  GPSSPEED! = VAL( tmp$ )
  GPSSPEED! = INT( GPSSPEED! * 10 ) / 10 'to 0.1 of a knot
  IF GPSSPEED < 10 THEN 'e.g. ,W,1.25,134.28,
    tmp$ = MID$( NMEA$, 51, 3 )
  ELSE 'e.g. ,W,10.25,134.28, NOTE COULD EASILY COPE WITH >= 100 KNOTS HERE :-)
    tmp$ = MID$( NMEA$, 52, 3 )
  ENDIF
  
  tmp$ = MID$( NMEA$, 44, 20 ) 'MMAUTOP2 15Aug17 Bug was here in not getting GPSHDG% - THIS FOR TEST
  'MMAP1A.BAS 30Aug17  print "NMEA $GPRMC End = ", tmp$

  tmp$ = MID$( NMEA$, 51, 5 ) 'MMAUTOP2 15Aug17 Bug was here in not getting GPSHDG%
  GPSHDG% = VAL( tmp$ ) + 0.5 'GPS direction of movement to 1 degree is good enough - rounded.
  '30Aug17  print "GPS Heading =", GPSHDG%
  
  DIR2NSEW ( GPSHDG%, NSEW$ ) 'e.g. 270 to "West"
  
  'MMAUTP3.BAS 15Aug17 bug in value spoken to be fixed
  if GPSSPEED! > 0 THEN
    'MMAUTP3.BAS 15Aug17  tmp$ = "Moving " + nsew$ + " at " + STR$( GPSSPEED! ) + " knots. GPS Heading is " + STR$( GPSHDG% ) + " degrees."  
    tmp$ = "Moving " + nsew$ + " at " + STR$( GPSSPEED! ) + " knots. GPS Heading is " + STR$( GPSHDG% ) + " degrees."
  ELSE
    tmp$ = "Not moving." '17Mar17
  ENDIF
  PRINT tmp$
  
  'THIS IS WHERE INFORMATION IS SPOKEN: Time now too.
  IF talkcounter% <> 2 then '02Sep17 was > 1
    goto aftersayvelocity 'MMAP1A.BAS 26Aug17 was >1  15Aug17 speed up control loop by speaking less often
  ENDIF
  'MMAP1A.BAS 26Aug17  IF talkcounter% <> 2 then goto aftersayvelocity 'MMAP1A.BAS 26Aug17 was >1  15Aug17 speed up control loop by speaking less often
  
  SERVO 1, CENTRE! 'MMAP1A.BAS 28Aug17 centre servo when speaking long report with time, etc.
  
    '28Mar17 Scan waypoint data to see which is nearest and where we are
  rnearest! = 9999 : nnearest$ = "anywhere": bnearest% = 359 'this will be replaced by nearest waypoint
  FOR II% = 1 to MAXNOWP
    '02sep17  LLLLTORB( latgps!, longps!, WPLat!( II% ), WPLon!( II% ), r!, btmp! )
    LLLLTORB(  WPLat!( II% ), WPLon!( II% ), latgps!, longps!, r!, btmp! ) '02sep17 
    if r! < rnearest! THEN '
      nnearest$ = WPName$( II% )
      rnearest! = r!
      dnearest% = btmp!
    ENDIF
  nEXT II% 
  if  rnearest! < 0.05 THEN 'if nearest within 50 metres
    rnear% = rnearest! * 1000
    DIR2NSEW ( dnearest%, D2NEAREST$ ) 'get direction to target. e.g. "North East"
    '02sep17 tmpnear$ = nnearest$ + " is " + STR$(rnear%) + " metres " + D2NEAREST$ + " at "   +  STR$(dnearest%)  '
    tmpnear$ = "We are " + STR$(rnear%) + " metres " + D2NEAREST$ + " of " + nnearest$   '02Sep17
  ELSE
    tmpnear$ = " "
  ENDIF
  '30Aug17 PRINT "Nearest: ", tmpnear$
  
  
  ' 'MMAP1A.BAS 23Aug17 say "Snoopy's time"  'MMAUTOP2.BAS 18Aug17 Speak GPS time from data $GPRMC,114138.000,A,5130.0011,N, etc.
  gpshours$ = MID$( NMEA$, 8, 2 ) 
  gpsminutes$ = MID$( NMEA$, 10, 2 ) 
  gpsseconds$ = MID$( NMEA$, 12, 2 )
  gpstime$ = "Snoopy's time is " + gpshours$ + ":" + gpsminutes$ + " and " + gpsseconds$ + "seconds. " 'e.g. Spoken "GPS time 09:15 and 23 seconds"
  '30Aug17 PRINT NMEA$ 'trace NMEA data
  '30Aug17 PRINT gpstime$ 'trace time

  PRINT #2, "S " gpstime$ + tmp$ + tmpnear$ '28Aug17 Speak time, distance and direction to target, etc
  
  '18Aug17 Pause that was 7000 ( 7 secs ) needs to be modified for speed dependent rudder timing logic. This code is duplicated further below.
  'MMAUTOP2.BAS 18Aug17 After first sail on 16th: make rudder timing speed dependent. 1.5 sec + 1 sec if speed < 1.5 + 1 sec if < 1 (from AP28X2Q).
  'MMAUTOP2.BAS 18Aug17 was PAUSE 5000 '5 second delay for speech and to hold rudder there
  PAUSE 1500 'Pause 1.5 sec at high speed
  tdelay% = 7000 'delay added at end after return of rudder to biased centre, so total delay in control cycle is same.
  IF GPSSPEED! < 1 THEN
    PAUSE 1000
    tdelay% = tdelay% - 1000
  ENDIF
  IF GPSSPEED! < 1.5 THEN
    PAUSE 1000  
    tdelay% = tdelay% - 1000
  ENDIF
  SERVO 1, CENTRE! '1.5 'centre
  '30Aug17 SERVO 1, BIASED! '1.5 'recentre the servo after delay for speech '15Aug17 - to BIASED position
  
  PAUSE tdelay% 'put total delay back to 7 secs
  '18Aug17 was PAUSE 7000 '17Mar17 was 10000 
  
aftersayvelocity:
  DoCTEC ( CBWP%, CDTWP%, CCTWP% ) '19Sep17 to correct for Cross Track Error. CDTWP% is there.

 '19Sep17 IF CBWP% < 360 THEN 'If we have a Course Between Waypoints ( starts at 999 )
 '19Sep17   CDTWP% = brg% 'bearing before correction, Course Direct To WayPoint
 '19Sep17   DoCTEC( CBWP%, CDTWP%, CCTWP% )
 '19Sep17   brg% = CCTWP% 'corrected bearing to remove cross track error
 '19Sep17 ENDIF

  hdgerror% = GPSHDG% + 360 - brg%  '15Mar15 GPS Heading error to be corrected by rudder servo
  if hdgerror% > 359 then 'get into range 0 to 359
    hdgerror% = hdgerror% - 360
  ENDIF
  IF hdgerror% > 180 then 'get into range -179 through 0 to + 179
    hdgerror% = - ( 359 - hdgerror% ) '17Mar17
  ENDIF
  
  '30Aug17 PRINT "HdgError = ", hdgerror% '17Mar17 to check calculation
 
  'MMAP1A.BAS 26Aug17 
  if hdgerror% > 0 then 'get hdgerror% in range -180 to +180
    tmpsteer$ = "Steer Left by " + STR$( ABS( hdgerror% ) )
  ENDIF
  IF hdgerror% < 0 THEN
    tmpsteer$ = "Steer Right by " + STR$( - hdgerror% )
  ENDIF
  IF hdgerror% = 0 THEN
    tmpsteer$ = "Straight Ahead."
  ENDIF
  '02Sep17 tmpsteer$ = "Target " + STR$(brg%) + ". " + tmpsteer$ 'MMAP1A.BAS 26Aug17 now says "Target 70. Steer Right by 20."
  tmpsteer$ = "Target " + named$ + " at " + STR$(brg%) + ". " + tmpsteer$ 'MMAP1A.BAS 2Sep17 now says "Target Alpha at 70. Steer Right by 20."
  tmpsteer$ = "Heading " + STR$( GPSHDG% ) + ". " + tmpsteer$ '02Sep17
  
  IF talkcounter% > 2 THEN '29Aug17 was 1. don't speak "Target 70. Steer Right by 20" if still speaking time, distance & direction to target, etc.
    PRINT tmpsteer$
    PRINT #2, "S " + tmpsteer$
  ENDIF
  
  'MMAP1A.BAS 25Au17 move rudder proportional to error, if less than 90 error, else full rudder.
  servopos! = 1.5 + hdgerror% * 0.5 / 90 'MMAP1A.BAS 25Au17 was 1.5 + hdgerror% * 1.0 / 180 
  if servopos! < 1 then
    servopos! = 1 'Full Right rudder
  ENDIF
  IF servopos! > 2 THEN
    servopos! = 2 'Full Left rudder
  ENDIF
  
  IF servopos! < 1.5 THEN '30Aug17
    tmp$ = " RIGHT "
  ELSE
    tmp$ = " LEFT "
  ENDIF
  
  PRINT NMEA$ '30Aug17  
  PRINT "GPSHdg: ", GPSHDG%, " Tgt:", STR$(brg%), tmp$, STR$(hdgerror%)
  SERVO 1, servopos!  'servo position is 2.0 for left, 1.5 for centre and 1.0 for right.
  
  'MMAUTOP2.BAS 15Aug17 add rudder bias logic. 
  IF servopos! > CENTRE! then 'left of centre
    BIASED! = BIASED! + BIASINC! 
    IF BIASED! > LEFT! THEN
      BIASED! = LEFT! 'limit the bias to max left
    ENDIF
    IF BIASED! < CENTRE! THEN 'zero bias if turning in other direction
      BIASED! = CENTRE!
    ENDIF
    '30Aug17 PRINT "Rudder ", STR$((BIASED! - CENTRE!) * 100 / 0.5), " % LEFT." 
  ELSE 'right of centre
    BIASED! = BIASED! - BIASINC!
    IF BIASED! < RIGHT! THEN
      BIASED! = RIGHT! 'limit the bias to max right
    ENDIF
    IF BIASED! > CENTRE! THEN 'zero bias if turning in other direction
      BIASED! = CENTRE!
    ENDIF
    '30Aug17  PRINT "Rudder ", STR$((CENTRE! - BIASED!) * 100 / 0.5), " % RIGHT." 
  ENDIF
  
  'MMAP1A.BAS 02Sep17 copy this block of code from above
  PAUSE 1500 'Pause 1.5 sec at high speed
  tdelay% = 7000 'delay added at end after return of rudder to biased centre, so total delay in control cycle is same.
  IF GPSSPEED! < 1 THEN
    PAUSE 1000
    tdelay% = tdelay% - 1000
  ENDIF
  IF GPSSPEED! < 1.5 THEN
    PAUSE 1000  
    tdelay% = tdelay% - 1000
  ENDIF
  SERVO 1, CENTRE! '1.5 'centre


ENDIF

'FOR TEST ONLY latgps! = WPLAT!( 1 ): longps! = WPLON!( 1 ) 'FOR TEST ONLY SET GPS LAT/LON TO BASE 'FOR TEST ONLY 

'16Mar17 Scan waypoint data to see if should change waypoint. 
'30Aug17  PRINT "Scan waypoints to see if close to any ..."
'MMAP1A.BAS 25Aug17  PRINT "MAXNOWP =", MAXNOWP
lastnamed$ = named$ '07Sep17 so know if it will change
FOR II% = 1 to MAXNOWP
  LLLLTORB( latgps!, longps!, WPLat!( II% ), WPLon!( II% ), r!, btmp! )
'MMAP1A.BAS 25Aug17   PRINT "Waypoint ", II%, " is ", r!*1000, " metres.", WPLat!( II% )," ",WPLon!( II% )
  if r! < WPRad!( II% ) THEN 'MMAP1A.BAS 24Aug17 SHOULD BE FROM ARRAY VALUE was 0.003 = 3 metres
    KK% = II% 'MMAP1A.BAS 13Sep17 so correct CBWP% Course Between Waypoints below calculated with LLLLTORB 
    name$ = WPName$( II% ) 'MMAP1A.BAS 23Aug17 waypoint we are near
    JJ% = WPNextWP%( II% ) 'Index of Next Target Destination Waypoint
    latd! = WPLAT!( JJ% ) 'MMAP1A.BAS 23Aug17 needed % and !
    lond! = WPLON!( JJ% )
    named$ = WPName$( JJ% ) 'MMAP1A.BAS 23Aug17        
    tmp$ = "S We are at waypoint " + name$  'MMAUTOP2.BAS 15Aug17 + named$ '09Aug17
    talkcounter% = 2 'MMAP1A.BAS 24Aug17
    goto endofwpscan 'MMAP1A.BAS 24Aug17 don't like GOTOs ? Tough Titty :-)    
  ENDIF
nEXT II%
endofwpscan:
IF named$ <> lastnamed$ THEN '07Sep17 destination has changed
  PRINT #2, tmp$ + ". Target is now " + named$ '09Aug17
  PAUSE 5000 
  LLLLTORB (WPLAT!( KK% ),WPLON!( KK% ),WPLAT!( JJ% ),WPLON!( JJ% ),r!, b! ) 'MMAP1A.BAS 13Sep17  CBWP ( Course Between Waypoint )
  CBWP% = b! 'MMAP1A.BAS 24Aug17 CBWP ( Course Between Waypoint ) for Cross Track Error Correction
  tmp$ = "S Course between waypoints is " + STR$(CBWP%) + " degrees. " 'MMAP1A.BAS 24Aug17
  PRINT #2, tmp$  'MMAP1A.BAS 24Aug17
  PAUSE 5000 '07Sep17 was 3sec MMAP1A.BAS 24Aug17 was 10000 
  VAR SAVE latd!, Lond!, named$, CBWP% ''MMAUTOP2 24Au17 and CBWP% 09Aug17 Save target destination to non volatile memory.
ENDIF


goto top



END