$regfile = "M32DEF.DAT"

$crystal = 16000000

$hwstack = 128
$swstack = 64
$framesize = 32


Config Lcdpin = Pin , Db4 = Portc.2 , Db5 = Portc.3 , Db6 = Portc.4 , Db7 = Portc.5 , E = Portc.1 , Rs = Portc.0
Config Lcd = 16 * 2
Cursor Off

Config Timer1 = Timer , Prescale = 8                        'counts 0.5µs
Stop Timer1
On Timer1 Timer1isr

Config Adc = Single , Prescaler = 64                        '250kHz
Start Adc

'The PPM signal is a frame of 25ms (timer1 50000), pause is low
'Start to fill up the 25ms
'Pause always 0.3ms (600) (counts to full signal lenth of 1 to 2 ms)
'Channel 1 to 8 neutral 1.2ms (2400), +/-500µs (1000) plus setting to 140% +/-700µs

Ddrb.3 = 1
Swon Alias Portb.3                                          'the main switch for power
Button Alias Pinb.4                                         'the pushbutton for ON

Ddrc.7 = 1                                                  'Output PPM
Ppm Alias Portc.7

Ddrc.6 = 1
Led Alias Portc.6

Portd = 255                                                 'Pullups Dig in 0-10
Portb.0 = 1
Portb.1 = 1
Portb.2 = 1

Menu Alias Pind.0                                           'Dig in 0-10
Ok Alias Pind.1
Plus Alias Pind.2
Minus Alias Pind.3
Din4 Alias Pind.4
Din5 Alias Pind.5
Din6 Alias Pind.6
Din7 Alias Pind.7
Din8 Alias Pinb.0
Din9 Alias Pinb.1
Din10 Alias Pinb.2



Dim Battery As Word
Dim State As Byte
Dim Stateid As Byte

Dim Value(9) As Long
Dim Ch As Byte

Dim Stickup(8) As Integer
Dim Stickdown(8) As Integer
Dim Neutral(8) As Integer
Dim Invert(8) As Byte
Dim Modelno As Byte
Dim Modelnodummy As Byte

Dim Rateup(64) As Byte
Dim Ratedown(64) As Byte

Dim E_stickup(8) As Eram Integer
Dim E_stickdown(8) As Eram Integer
Dim E_neutral(8) As Eram Integer
Dim E_invert(8) As Eram Byte
Dim E_modelno As Eram Byte

Dim E_rateup(64) As Eram Byte                               'index = (modelno - 1) * 8 + channel
Dim E_ratedown(64) As Eram Byte

Dim I As Byte                                               'just counting
Dim J As Byte                                               'just counting
Dim K As Byte                                               'just counting

Dim Updown As Bit                                           'up=0, down=1

Dim Adcchannel As Byte
Dim Ratechannel As Byte
Dim Dummyrate As Byte
Dim Krate As Byte

Dim Startbitdone As Byte
Dim Buttoncount As Byte
Dim Batterycount As Byte
Dim Batteryten As Word
Dim Batteryone As Word
Dim Menucount As Byte

Dim Hardchannel As Byte
Dim Hardadc As Byte
Dim Setreverse As Bit

'####### Init #######

Swon = 1
Wait 1
Cls
Lcd "    WELCOME!"

Do
Waitms 100
Loop Until Button = 0

Modelno = E_modelno                                         'read model number from Eram
If Modelno = 255 Then Gosub Fill_eram                       'fill up with default values at first start
Gosub Read_eram                                             'get saved values from eram

Gosub Main_screen

Start Timer1
Enable Interrupts
Enable Timer1

'######################################
'############### Main Loop ############
'######################################

Do
   Do
   nop
   Loop Until Startbitdone = 1
   Startbitdone = 0

   Incr Batterycount

   If Batterycount >= 40 Then                               '1 second
      Batterycount = 0
      Battery = Getadc(7)
      Battery = Battery * 50
      Shift Battery , Right , 9
      Battery = Battery + 6                                 'the diode
      If Battery > 99 Then Battery = 99                     'should never happen anyway
      Batteryten = Battery / 10
      Locate 1 , 10
      Lcd Batteryten
      Locate 1 , 12
      Batteryten = Batteryten * 10
      Batteryone = Battery - Batteryten
      Lcd Batteryone
   End If

   If Button = 1 Then
                  Incr Buttoncount
                  If Buttoncount >= 80 Then                 'after 2 seconds
                     Cls
                        Lcd "    BYE-BYE!"
                        Wait 1
                        Cls
                        Swon = 0
                        Wait 10
                  End If
             Else
                  Buttoncount = 0
   End If

   If Menu = 0 Then
               Incr Menucount
               If Menucount >= 20 Then
                  Gosub Main_menu
                  Menucount = 0
                  Gosub Main_screen
                  Do
                     Waitms 100
                  Loop Until Menu = 1
               End If
           Else
               Menucount = 0
   End If

Loop

'###############################
'####### Subroutines ###########
'###############################

Timer1isr:
   Incr State
   If State = 19 Then State = 1                             '1...18

   Stateid = State Mod 2
   If Stateid = 0 Then
                  Gosub 300us                               'state even -> pause
               Else
                  Ch = State / 2                            'Channel 0, 1,...8
                  If Ch = 0 Then
                        Gosub Startbit
                     Else
                        Gosub Getchannel
                  End If
   End If
Return

'########## PPM Times ###############

300us:
   Ppm = 0
   Timer1 = 65535 - 600                                     '300µs
Return

Startbit:
   Ppm = 1
   Value(9) = 24000
   For I = 1 To 8
      Value(9) = Value(9) - Value(i)
   Next I
   Timer1 = 65535 - Value(9)
   Startbitdone = 1
Return

Getchannel:
   Ppm = 1
   Value(ch) = Value(ch) + 2400                             'value1 is the travel up/down in 0,5µs, 2400µs is neutral
   Timer1 = 65535 - Value(ch)                               'preload timer

   Adcchannel = Ch - 1
   If Adcchannel = 7 Then
            Value(ch) = 0
      Else

            K = Modelno - 1
            K = K * 8
            K = K + Ch                                      'rateindex
            Value(ch) = Getadc(adcchannel)
            Value(ch) = Value(ch) - Neutral(ch)             'now pos/neg travel from neutral
            Value(ch) = Value(ch) * 1000                    'upscaled to to 500µs travel
            If Value(ch) >= 0 Then
                        Value(ch) = Value(ch) / Stickup(ch) 'scaled to upper stick range
                        Value(ch) = Value(ch) * Rateup(k)   'Rate of channel up %
                        If Value(ch) > 160000 Then Value(ch) = 140000       'limit to the max allowed value
                     Else
                        Value(ch) = Value(ch) / Stickdown(ch)       'scaled to lower stick range
                        Value(ch) = Value(ch) * Ratedown(k) 'Rate of channel down %
                        If Value(ch) < -160000 Then Value(ch) = -140000       'limit to the max allowed value
            End If
            Value(ch) = Value(ch) / 100                     'Rate was in %
            If Invert(ch) = 1 Then Value(ch) = Value(ch) * -1       'invert
   End If
Return

'##############################################
'############## Programming Menues ############
'##############################################

Main_screen:
   Cls
   Lcd "Voltage:  . V"
   Battery = Getadc(7)
   Battery = Battery * 50
   Shift Battery , Right , 9                                'calculate to 1/10V
   Battery = Battery + 6
   If Battery > 99 Then Battery = 99                        'should never happen anyway
   Batteryten = Battery / 10
   Locate 1 , 10
   Lcd Batteryten
   Locate 1 , 12
   Batteryten = Batteryten * 10
   Batteryone = Battery - Batteryten
   Lcd Batteryone
   Locate 2 , 1
   Lcd "Model No.: "
   Lcd Modelno
Return

Main_menu:
   Cls
   Lcd "Servo Rate"
   Do
      Waitms 100
   Loop Until Menu = 1

   Do
      Rate:
      Do
      Waitms 100
      Loop Until Minus = 1
      Cls
      Lcd "Servo Rate"
      Do
         If Menu = 0 Then Return
         If Ok = 0 Then
            Gosub Servorate
            Cls : Lcd "Servorate"
            Do
               Waitms 100
            Loop Until Menu = 1
         End If
         If Minus = 0 Then Goto Battery
      Loop Until Plus = 0
      Do
         Waitms 100
      Loop Until Plus = 1

      Reverse:
      Do
      Waitms 100
      Loop Until Minus = 1
      Cls
      Lcd "Servo Reverse"
      Do
         If Menu = 0 Then Return
         If Ok = 0 Then
            Gosub Servoreverse
            Cls : Lcd "Servoreverse"
            Do
               Waitms 100
            Loop Until Menu = 1
         End If
         If Minus = 0 Then Goto Rate
      Loop Until Plus = 0
      Do
         Waitms 100
      Loop Until Plus = 1

      Model:
      Do
      Waitms 100
      Loop Until Minus = 1
      Cls
      Lcd "Chose Model"
      Do
         If Menu = 0 Then Return
         If Ok = 0 Then
            Gosub Chosemodel
            Cls : Lcd "Chose Model"
            Do
               Waitms 100
            Loop Until Menu = 1
         End If
         If Minus = 0 Then Goto Reverse
      Loop Until Plus = 0
      Do
         Waitms 100
      Loop Until Plus = 1


      Hardware:
      Do
      Waitms 100
      Loop Until Minus = 1
      Cls
      Lcd "Hardware Setup"
      Do
         If Menu = 0 Then Return
         If Ok = 0 Then
            Gosub Hardwaresetup
            Cls : Lcd "Hardwaresetup"
            Do
               Waitms 100
            Loop Until Menu = 1
         End If
         If Minus = 0 Then Goto Model
      Loop Until Plus = 0
      Do
         Waitms 100
      Loop Until Plus = 1

      Battery:
      Do
      Waitms 100
      Loop Until Minus = 1
      Cls
      Lcd "Battery Setup"
      Do
         If Menu = 0 Then Return
         If Ok = 0 Then
            Gosub Batterysetup
            Cls : Lcd "Battery Setup"
            Do
               Waitms 100
            Loop Until Menu = 1
         End If
         If Minus = 0 Then Goto Hardware
      Loop Until Plus = 0
      Do
         Waitms 100
      Loop Until Plus = 1

   Loop
Return                                                      'never happens

'###########################################
'################# Settings ################
'###########################################


Servorate:
   Cls
   Lcd "Set Rate Ch 1"
   Ratechannel = 1
   Do
      Waitms 100
   Loop Until Ok = 1

   Do
      If Plus = 0 Then
         Do
            Incr Ratechannel
            If Ratechannel = 9 Then Ratechannel = 1
            If Ratechannel = 6 Then Ratechannel = 7         'Ch 6 is multinaut
            Locate 1 , 13 : Lcd Ratechannel                 'now 1 to 8
            Waitms 500
         Loop Until Plus = 1
      End If

      If Minus = 0 Then
         Do
            Decr Ratechannel
            If Ratechannel = 0 Then Ratechannel = 8
            If Ratechannel = 6 Then Ratechannel = 5
            Locate 1 , 13 : Lcd Ratechannel                 'now 1 to 8
            Waitms 500
         Loop Until Minus = 1
      End If

      If Ok = 0 Then
                  Gosub Settherate
                  Do
                     Waitms 100
                  Loop Until Menu = 1
                  Cls
                  Lcd "Set Rate Ch  "
                  Locate 1 , 13 : Lcd Ratechannel
      End If

   Loop Until Menu = 0

Return

Settherate:

      Krate = Modelno - 1
      Krate = Krate * 8
      Krate = Krate + Ratechannel
      Updown = 0
      Cls
      Lcd "CH   up        %"
      Locate 1 , 4
      Lcd Ratechannel
      Locate 1 , 13
      Disable Interrupts
      Dummyrate = Rateup(krate)
      Enable Interrupts
      If Dummyrate < 100 Then Lcd " "
      If Dummyrate < 10 Then Lcd " "
      Lcd Dummyrate

      Locate 2 , 1
      Lcd "OK for up<->down"
      Do
      Waitms 100
      Loop Until Ok = 1

      Do
         If Ok = 0 Then
               Do
               Waitms 100
               Loop Until Ok = 1
               Toggle Updown
               Locate 1 , 6
               If Updown = 0 Then
                     Disable Interrupts
                     Dummyrate = Rateup(krate)
                     Enable Interrupts
                     Lcd "up        "
                     Locate 1 , 13
                     If Dummyrate < 100 Then Lcd " "
                     If Dummyrate < 10 Then Lcd " "
                     Lcd Dummyrate
                  Else
                     Disable Interrupts
                     Dummyrate = Ratedown(krate)
                     Enable Interrupts
                     Lcd "down      "
                     Locate 1 , 13
                     If Dummyrate < 100 Then Lcd " "
                     If Dummyrate < 10 Then Lcd " "
                     Lcd Dummyrate
               End If
         End If

         If Plus = 0 Then
               Incr Dummyrate
               If Dummyrate >= 151 Then Dummyrate = 150
               Locate 1 , 13
               If Dummyrate < 100 Then Lcd " "
               If Dummyrate < 10 Then Lcd " "
               Lcd Dummyrate
               Startbitdone = 0
               If Updown = 0 Then
                           Disable Interrupts
                           Rateup(krate) = Dummyrate
                           Enable Interrupts
                         Else
                           Disable Interrupts
                           Ratedown(krate) = Dummyrate
                           Enable Interrupts
               End If
               Waitms 150
         End If

         If Minus = 0 Then
               Decr Dummyrate
               If Dummyrate = 255 Then Dummyrate = 0
               Locate 1 , 13
               If Dummyrate < 100 Then Lcd " "
               If Dummyrate < 10 Then Lcd " "
               Lcd Dummyrate
               If Updown = 0 Then
                           Disable Interrupts
                           Rateup(krate) = Dummyrate
                           Enable Interrupts
                         Else
                           Disable Interrupts
                           Ratedown(krate) = Dummyrate
                           Enable Interrupts
               End If
               Waitms 150
         End If
      Loop Until Menu = 0
      Do
         Waitms 100
      Loop Until Menu = 1

      Startbitdone = 0
      Do
      Loop Until Startbitdone = 1
      Disable Interrupts
      E_rateup(krate) = Rateup(krate)
      E_ratedown(krate) = Ratedown(krate)
      Enable Interrupts

Return

'############################

Servoreverse:
   Cls
   Lcd "Rev. CH :   No"
   Hardchannel = 1
   Locate 1 , 8
   Lcd Hardchannel
   Locate 2 , 1
   Lcd "OK to toggle"
   Do
      Waitms 100
   Loop Until Ok = 1

   Setreverse = Invert(1)

   Do
      If Plus = 0 Then
         Do
            Waitms 100
         Loop Until Plus = 1
         Incr Hardchannel
         If Hardchannel = 9 Then Hardchannel = 1
         If Hardchannel = 6 Then Hardchannel = 7            'Ch 6 is multinaut
         Locate 1 , 8 : Lcd Hardchannel                     'now 1 to 8
      End If

      If Minus = 0 Then
         Do
            Waitms 100
         Loop Until Minus = 1
         Decr Hardchannel
         If Hardchannel = 0 Then Hardchannel = 8
         If Hardchannel = 6 Then Hardchannel = 7            'Ch 6 is multinaut
         Locate 1 , 8 : Lcd Hardchannel                     'now 1 to 8
      End If

      If Ok = 0 Then
         Do
            Waitms 100
         Loop Until Ok = 1
         Toggle Setreverse
         Locate 1 , 13
         If Setreverse = 0 Then
                  Lcd "No "
            Else
                  Lcd "Yes"
         End If

         Startbitdone = 0
         Do
         Loop Until Startbitdone = 1
         Disable Interrupts
         Invert(hardchannel) = Setreverse
         E_invert(hardchannel) = Invert(hardchannel)
         Enable Interrupts
      End If

   Loop Until Menu = 0

Return

'############################

Chosemodel:
   Cls
   Lcd "Model No.: "
   Lcd Modelno
   Do
      Waitms 100
   Loop Until Ok = 1
   Do
      If Plus = 0 Then
         Incr Modelnodummy
         If Modelnodummy = 9 Then Modelnodummy = 1
         Locate 1 , 12 : Lcd Modelnodummy
         Do
            Waitms 100
         Loop Until Plus = 1
      End If
      If Minus = 0 Then
         Decr Modelnodummy
         If Modelnodummy = 0 Then Modelnodummy = 8
         Locate 1 , 12 : Lcd Modelnodummy
         Do
            Waitms 100
         Loop Until Minus = 1
      End If
   Loop Until Menu = 0
   Do
         Waitms 100
   Loop Until Menu = 1
   Startbitdone = 0
   Do
   Loop Until Startbitdone = 1
   Disable Interrupts
   Modelno = Modelnodummy
   E_modelno = Modelno
   Enable Interrupts
Return

'############################################

Hardwaresetup:
   Do
      Waitms 100
   Loop Until Ok = 1

   Hardchannel = 1

      Cls
      Lcd "Calibrate CH1"

      Do
         If Plus = 0 Then
                     Incr Hardchannel
                     If Hardchannel = 9 Then Hardchannel = 1
                     If Hardchannel = 6 Then Hardchannel = 7
                     Locate 1 , 13
                     Lcd Hardchannel
                     Waitms 500
         End If

         If Minus = 0 Then
                     Decr Hardchannel
                     If Hardchannel = 0 Then Hardchannel = 8
                     If Hardchannel = 6 Then Hardchannel = 5
                     Locate 1 , 13
                     Lcd Hardchannel
                     Waitms 500
         End If
      Loop Until Ok = 0

      Do
         Waitms 200
      Loop Until Ok = 1

      '##### Neutral #####
      Cls
      Lcd "Set CH  neutral"
      Locate 1 , 7
      Lcd Hardchannel
      Locate 2 , 1
      Lcd "then press ok"
      Do
      Loop Until Ok = 0
      If Hardchannel < 6 Then Hardadc = Hardchannel - 1
      If Hardchannel >= 7 Then Hardadc = Hardchannel - 2
      Startbitdone = 0
      Do
      Loop Until Startbitdone = 1
      Disable Interrupts
      Neutral(hardchannel) = Getadc(hardadc)
      E_neutral(hardchannel) = Neutral(hardchannel)
      Enable Interrupts
      Do
         Waitms 100
      Loop Until Ok = 1

      '##### Up #####
      Cls
      Locate 1 , 7
      Lcd "DONE!"
      Wait 2
      Cls
      Lcd "Set CH  up     "
      Locate 1 , 7
      Lcd Hardchannel
      Locate 2 , 1
      Lcd "then press ok"
      Do
      Loop Until Ok = 0
      Startbitdone = 0
      Do
      Loop Until Startbitdone = 1
      Disable Interrupts
      Stickup(hardchannel) = Getadc(hardadc)
      Stickup(hardchannel) = Stickup(hardchannel) - Neutral(hardchannel)
      Stickup(hardchannel) = Abs(stickup(hardchannel))
      E_stickup(hardchannel) = Stickup(hardchannel)
      Enable Interrupts
      Do
         Waitms 100
      Loop Until Ok = 1

      '##### Down #####
      Cls
      Locate 1 , 7
      Lcd "DONE!"
      Wait 2
      Cls
      Lcd "Set CH  down   "
      Locate 1 , 7
      Lcd Hardchannel
      Locate 2 , 1
      Lcd "then press ok"
      Do
      Loop Until Ok = 0
      Startbitdone = 0
      Do
      Loop Until Startbitdone = 1
      Disable Interrupts
      Stickdown(hardchannel) = Getadc(hardadc)
      Stickdown(hardchannel) = Stickdown(hardchannel) - Neutral(hardchannel)
      Stickdown(hardchannel) = Abs(stickdown(hardchannel))
      E_stickdown(hardchannel) = Stickdown(hardchannel)
      Enable Interrupts
      Do
         Waitms 100
      Loop Until Ok = 1

      Cls
      Locate 1 , 7
      Lcd "DONE!"
      Wait 2

Return

'#####################################

Batterysetup:
   Cls
   Do
      Waitms 100
   Loop Until Ok = 1

Return

'####################################################




Fill_eram:
   For I = 1 To 8                                           'for 8 models
   '#### set default values ####
      Stickup(i) = 250
      Stickdown(i) = 250
      Neutral(i) = 512

      For J = 1 To 8                                        'for 8 Channels
         K = I - 1                                          '0 to 7
         K = K * 8                                          'base address (Modelno,Channel)
         K = K + J
         Rateup(k) = 100
         Ratedown(k) = 100
      Next J


   '####  store values in eram ####
      Disable Interrupts
      E_stickup(i) = Stickup(i)
      E_stickdown(i) = Stickdown(i)
      E_neutral(i) = Neutral(i)
      Enable Interrupts
      For J = 1 To 8
         Disable Interrupts
         K = I - 1                                          '0 to 7
         K = K * 8                                          'base address (Modelno,Channel)
         K = K + J
         E_rateup(k) = Rateup(k)
         E_ratedown(k) = Rateup(k)
         Enable Interrupts
      Next J


   Next I
   Modelno = 1
   E_modelno = Modelno
Return

'#####
Read_eram:
      Modelno = E_modelno
      For I = 1 To 8                                        'for 8 Modelno.
         Stickup(i) = E_stickup(i)
         Stickdown(i) = E_stickdown(i)
         Neutral(i) = E_neutral(i)

         For J = 1 To 8                                     'for 8 Channels
            K = I - 1                                       '0 to 7
            K = K * 8                                       'base address (Modelno,Channel)
            K = K + J
            Rateup(k) = E_rateup(k)
            Ratedown(k) = E_ratedown(k)
         Next J

      Next I
Return