#chip tiny85,8 '8
   '#config OSC = RC, BODEN = OFF
    #config osc = int

    #define MOTOR PORTB.0
    #define LED PORTB.1
    #define SERVO PORTB.2
    #define WORK PORTB.4

    Dir MOTOR Out 'PB0, pin 5
    Dir LED Out   'PB1, pin 6
    Dir SERVO Out 'PB2, pin 7
    Dir WORK Out  'PB4, pin 3

    Dim PWMCounter as Word
    Dim MotorSpeed as Word
    Dim ServoRotation as Word

    Dim FTime as Byte
    Dim FMode as Byte

    #Define ASize 7

    Dim AMotorSpeed(ASize)
    Dim AWait(ASize)
    Dim interval as byte

    AMotorSpeed = 58, 60, 95, 125, 95, 60, 58
    AWait = 3, 1, 1, 5, 1, 1, 1

    'Load saved flight time
    EPRead 0, FTime
    EPRead 1, FMode

    if MCUSR.1=1 AND FMode=0 Then '
    FTime=FTime+1
    if FTime>9 Then FTime=1
    EPWrite 0, FTime
    End If

    MCUSR.1=0
    FMode=0
    EPWrite 1, FMode
    FlashLed FTime

    Set WORK On

    Wait 5000 ms
    FMode=255
    EPWrite 1, FMode

    'Call the initialisation routine
    InitMotorControl
    ServoRotation = 25

    'Main routine
    For interval = 1 to ASize

        'Hold speed
        MotorSpeed = AMotorSpeed(interval)
        FlashLed 1
        Wait AWait(interval) s

        ServoRotation = 125
        Wait 1 s
        Set WORK Off
    Next

        FMode=0
        EPWrite 1, FMode
        MotorSpeed = 0
    Do
    NOP 'Stop
    Loop

    'Setup routine
    Sub InitMotorControl

        'Clear variables
        MotorSpeed = 0
        ServoRotation = 0
        PWMCounter = 0

        'Add a handler for the interrupt
        On Interrupt Timer0Overflow Call PWMHandler

        'Set up the timer using the internal oscillator with a prescaler of 1/2 (Equates to 0)
        'Timer 0 starts automatically on a Microchip PIC microcontroller, therefore, StartTimer is not required.
        InitTimer0 Osc, PS_0_1
        SetTimer 0, 0
        StartTimer 0
    End Sub

    'PWM sub
    'This will be called when Timer 0 overflows
    Sub PWMHandler
        If MotorSpeed > PWMCounter Then
            Set MOTOR On
        Else
            Set MOTOR Off
        End If

        If ServoRotation > PWMCounter Then
            Set SERVO On
        Else
            Set SERVO Off
        End If


        PWMCounter += 1
        If PWMCounter = 1250  Then PWMCounter = 0 ' Период  625
    End Sub


    Sub FlashLed( Max )
    Dim Speed as byte

        Wait 1000 ms
        For Speed = 1 to Max
            Set LED On
            Wait 100 ms
            Set LED Off
            Wait 400 ms
        Next
    End Sub


'MOTOR ESC 62 - 125
'SERVO 25 - 150