# -*- coding: utf-8 -*-

"""
author    Benoit Dubois
copyright FEMTO Engineering
licence   LGPL v3
brief     Specific controller composed of 2 servos.
details
            -------
   ADC1 >---|PID 1|---> VNH509
            -------

            -------
   ADC1 >---|PID 2|---> VNH509
            -------
"""
import pyb

from pid import PIDfixed
from vnh5019 import Vnh5019
from servo2to2_core import Servo1to1, Servo2to2Core

# Avoid use of timer 2, 3, 5 and 6
# see http://docs.micropython.org/en/latest/pyboard/library/pyb.Timer.html
TIMER_PWM_ID = 2

TIMER_PWM_FREQ = 1000

TIMER_PWM1_CH = 1
TIMER_PWM2_CH = 3

PIN_IN1 = 'PC5'
PIN_IN2 = 'PC1'

PIN_INA1 = 'PD6'
PIN_INB1 = 'PD2'
PIN_INA2 = 'PD5'
PIN_INB2 = 'PD1'
PIN_PWM1 = 'PA15'
PIN_PWM2 = 'PB10'

#----------------------------------------------------------
def build_servo():
    in_1 = pyb.ADC(pyb.Pin(PIN_IN1))
    in_2 = pyb.ADC(pyb.Pin(PIN_IN2))
    pid_1 = PIDfixed()
    pid_2 = PIDfixed()
    out_1 = Vnh5019(in_a=PIN_INA1, in_b=PIN_INB1, pwm=PIN_PWM1,
                    timer=TIMER_PWM_ID, freq=TIMER_PWM_FREQ,
                    ch=TIMER_PWM1_CH)
    out_2 = Vnh5019(in_a=PIN_INA2, in_b=PIN_INB2, pwm=PIN_PWM2,
                    timer=TIMER_PWM_ID, freq=TIMER_PWM_FREQ,
                    ch=TIMER_PWM2_CH)
    servo1 = Servo1to1(in_1, pid_1, out_1)
    servo2 = Servo1to1(in_2, pid_2, out_2)
    return (servo1, servo2)

#----------------------------------------------------------
def main():
    servo = build_servo()
    servo_core = Servo2to2Core(servo)
    servo_core.run()

#----------------------------------------------------------
if __name__ == '__main__':
    main()
