' This is the main UAV program.
' Released under a Creative Commons attribution licence by Chris Anderson, DIYDrones.com

' This first program just passes RX rudder and elevator commands through the Basic Stamp and watches for the gear
' switch to change state. When that changes this program transfers control to the initialization program for autonomous control.
' Based on demonstration code created by Dan DeGard (http://www.seattlerobotics.org/encoder/200304/AirmailStamp.htm)

'{$Stamp BS2p, initialize, navigation, altitude}''Initialization program is loaded in program slot 1; navigation in 2; altitude hold in 3
' {$PBASIC 2.5}
''pin 15 = gps io
''pin 14 = servo io
''pin 13 = chan5 ,switch
''pin 12 = chan4 ,rudder
''pin 11 = chan3 ,throttle
''pin 10 = chan2 ,elevator
''pin 9 = chan1 , aileron


' --------[Variables

'permission VAR Word: CntrlInput VAR Word: Position VAR Word
'Lbyte VAR Byte: Ubyte VAR Byte: interim VAR Word
'RX5in CON 5: RX4in CON 7: RX2in CON 6

'These constants are used by the FT639

'pin1 CON 8: pin2 CON 9: baud CON 17405: header CON 103
'pulse CON 90 :

chan2in PIN 10 'channel 1 input
chan3in PIN 11
chan4in PIN 12 'channel 2 input
chan5in PIN 13   'swich channel input
sdat PIN 14
bauds CON  1021       'baud rate
ra CON 0
hipulse CON 1
lowpulse CON 0
ch2 CON 2
ch4 CON 4
 Scale       CON     $0C0
chan2 VAR Word   'channel 1 value
chan4 VAR Word   ' channel 2 value
chan5 VAR Word   ' swich value
'GOSUB setup 'these steps initialize the FT639.
'GOSUB setpulse '
'GOSUB setheader '
'GOSUB setactive '

'The following subroutine first checks to see if the “gear” switch has been changed. Then, it reads the length of the
' square-wave pulse from the R/C receiver. The time value of the pulse is converted arithmetically to a number
' between 0 and 128. This position number is then split into two 4-Bit nibbles and sent to the FT639 chip (all this
' is just to create the required syntax for the FT chip). Note: it's possible to generate a square wave and drive the servos
' with the Basic Stamp chip directly, but it's a computational drain and uses more scarce memory. We offload that
' to the FT639 chip to simplify matters.

check:

  PULSIN chan5in,1,chan5
  chan5 = (chan5 */ scale)/2

  DEBUG DEC chan5,TAB

  IF chan5 > 750  THEN GOSUB through_put ELSE GOSUB Auto
  'IF Permission < 1900 THEN Autonomous '1900 is a safe value for a mid-point (if the value is less than 1900, then the
    'gear switch has been moved to the “autonomous” position).
 'throughput sub
 through_put:

 PULSIN chan2in , HIpulse , chan2
 chan2 = (chan2 */ scale)/2
 DEBUG DEC chan2 ,TAB
 SEROUT  sdat, bauds+$8000,["!SC",ch2,ra,chan2.LOWBYTE,chan2.HIGHBYTE,CR]


 PULSIN chan4in , HIpulse , chan4
 chan4 = (chan4 */ scale)/2
 DEBUG DEC chan4 ,CR,11
 SEROUT  sdat, bauds+$8000,["!SC",ch4,ra,chan4.LOWBYTE,chan4.HIGHBYTE,CR]





GOTO check

Auto:

  DEBUG "auto on",CR,11
  RUN 1
END