' This is the program for UAV navigation. ' Released under a Creative Commons attribution licence by Chris Anderson, DIYDrones.com ' Bug fixes by Wayne Garris ' {$STAMP BS2p} ' {$PBASIC 2.5} ' -----[ I/O Definitions ]------------------------------------------------- gio PIN 15 ' connects to GPS Module SIO pin sdat PIN 14 'serial io for servo chan5in PIN 13 'swich channel input ' -----[ Constants ]------------------------------------------------------- bauds CON 1021 'RX5in CON 5 Scale CON $0C0 ra CON 0 hipulse CON 1 lowpulse CON 0 ch4 CON 4 T4800 CON 500 Open CON $8000 Baud CON Open | T4800 ' Open mode to allow daisy chaining 'pin2 CON 9 ' GPS Module Commands GetLat CON $05 GetLong CON $06 GetHead CON $09 ' -----[ Variables ]------------------------------------------------------- TotalWaypoints VAR Nib: CurrentWayNum VAR Nib TempLat VAR Byte: TempLatMin VAR Word: TempLong VAR Byte: TempLongMin VAR Word CurrentWayLat VAR Byte: CurrentWayLatMin VAR Word: CurrentWayLong VAR Byte: CurrentWayLongMin VAR Word DeltaLat VAR Word: DeltaLong VAR Word workVal VAR Word ' for numeric conversions degrees VAR Byte ' latitude/longitude degrees minutes VAR Byte ' latitude/longitude minutes minutesD VAR Word ' latitude/longitude decimal minutes dir VAR Byte ' direction (latitude: 0 = N, 1 = S, longitude: 0 = E, 1 = W) heading VAR Word ' heading in 0.1 degrees ' ------[ Main ]------------- Main: DO ' We'll use workVal as a temporary variable here to save variable space PULSIN chan5in,1,workVal workVal= (workVal */ scale)/2 IF workVal > 750 THEN RUN 0'1900 is a safe value for a mid-point (if the value is greater than 1900, then the 'gear switch has been moved to the “manual” position). GET 68, CurrentWayNum GET 69, TotalWaypoints IF CurrentWayNum <= TotalWaypoints THEN GOSUB GetCurrentWay ELSE GOSUB GoHome GOSUB Get_Lat GOSUB Get_Long GOSUB Check_Proximity GOSUB Get_Heading GOSUB Get_Angle GOSUB Get_Rudder_Angle GOSUB Steer 'RUN 3 LOOP ' -----[ Subroutines ]----------------------------------------------------- Get_Angle: DeltaLong = -1 * DeltaLong 'reverse to conform with ArcTangent coordinate standards DeltaLat = DeltaLat ** 839'(128/9999)adjust them so the max values are +- 128, to conform with ArcTangent standards. DeltaLong = DeltaLong ** 839'=deltalong * (128 / 9999) workVal = DeltaLong ATN DeltaLat ' reuse workVal (to save variable memory) to hold binary radian result of ArcTangent DEBUG "angle", workVal,CR,11 RETURN ' ---------------------------------------------------- Get_Rudder_Angle: heading = 256 - heading + 64 ' Convert GPS brad frame of reference to Basic Stamp frame of reference (reversed and rotated) workVal = workVal - heading ' Rotate them both so Heading is at zero IF workVal < 0 THEN workVal = workVal + 256 'Make all angles positive. Left is less than 128; right is larger than 128 IF workVal < 128 THEN workVal = 128 - workVal ELSE workVal = 256 - workVal ' Adjust to reflect that 0 and 256 are maximum deflection ' for the servo; 128 is the neutral position. RETURN ' ---------------------------------------------------- Steer: ' We're going to reuse a lot of variables here; pay no attention to their names. ' Basically, we're turning the rudder by the value of workVal DEBUG "steer",DEC workval,CR,11 workval = workval + 500 SEROUT sdat, bauds+$8000,["!SC",ch4,ra,workval.LOWBYTE,workval.HIGHBYTE,CR] RETURN ' ---------------------------------------------------- Check_Proximity: 'we'll reuse CurrentWayNum here to save variable space CurrentWayNum = CurrentWayLat - TempLat 'we can safely assume that we're never more than one degree away from the next waypoint, so we can reduce the degree delta into ' 1, -1 or 0 and collapse degrees and minutes into one word, with one degree = 10,000 and minutes 1-9,999 IF CurrentWayNum = 1 THEN CurrentWayLatMin = CurrentWayLatMin + 10000 IF CurrentWayNum = -1 THEN TempLatMin = TempLatMin + 10000 DeltaLat = CurrentWayLatMin - TempLatMin 'now do the same for longitude CurrentWayNum = CurrentWayLong - TempLong IF CurrentWayNum = 1 THEN CurrentWayLongMin = CurrentWayLongMin + 10000 IF CurrentWayNum = -1 THEN TempLongMin = TempLongMin + 10000 DeltaLong = CurrentWayLongMin - TempLongMin 'set this for how close to the waypoints you want to get (these are set for one hundredth of a degree.) 'If we're within range, go to the incrementing subroutine and switch to the next program IF ABS DeltaLat <= 100 AND ABS DeltaLong <= 100 THEN GOTO Next_Waypoint DEBUG "lat to waypoint",DEC deltalat,CR,11 DEBUG "long to waypoint",DEC deltalong,CR,11 RETURN ' ---------------------------------------------------- Next_Waypoint: 'increment the waypoint counter and temprarily exit the navigation program CurrentWayNum = CurrentWayNum + 1 PUT 68, CurrentWayNum DEBUG "waypoint #",DEC currentwaynum,CR,11 ' RUN 3 RETURN ' ---------------------------------------------------- GetCurrentWay: GET (5+(3*CurrentWayNum)), CurrentWayLat GET (6+(3*CurrentWayNum)), Word CurrentWayLatMin GET (8+(3*CurrentWayNum)), CurrentWayLong GET (9+(3*CurrentWayNum)), Word CurrentWayLongMin RETURN ' ---------------------------------------------------- GoHome: GET 0, CurrentWayLat GET 1, Word CurrentWayLatMin GET 3, CurrentWayLong GET 4, Word CurrentWayLongMin RETURN ' ---------------------------------------------------- Get_Lat: SEROUT gio, Baud, ["!GPS", GetLat] SERIN gio, Baud, 3000, No_Response, [degrees, minutes, minutesD.HIGHBYTE, minutesD.LOWBYTE, dir] ' convert to decimal format workVal = (minutes * 1000 / 6) + (minutesD / 60) TempLat = degrees TempLatMin = workVal DEBUG "lat", DEC degrees,"deg",DEC minutes,".",DEC minutesd,"min",CR,11 RETURN ' ---------------------------------------------------- Get_Long: SEROUT gio, Baud, ["!GPS", GetLong] SERIN gio, Baud, 3000, No_Response, [degrees, minutes, minutesD.HIGHBYTE, minutesD.LOWBYTE, dir] ' convert to decimal format workVal = (minutes * 1000 / 6) + (minutesD / 60) TempLong = degrees TempLongMin = workVal DEBUG "long" ,DEC degrees,"deg",DEC minutes,".",DEC minutesd,"min",CR,11 RETURN ' ---------------------------------------------------- Get_Heading: SEROUT gio, Baud, ["!GPS", GetHead] SERIN gio, Baud, 3000, No_Response, [heading.HIGHBYTE, heading.LOWBYTE] DEBUG "heading" ,DEC heading ,CR ,11 heading = (heading*100)/141 'convert to binary degrees (1 degree = 1.41 binary degrees) RETURN ' ---------------------------------------------------- No_Response: PAUSE 1000 GOTO Main