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

"""
author  Benoit Dubois
brief   Specific controller composed of 2 servos.
details
               -------
   MCP3551 >---|PID 1|---> VNH509
               -------

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

from pid import PIDfixed
from mcp3551 import Mcp3551
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

ADC1_SPI_CH = 1
ADC2_SPI_CH = 2
ADC1_V_REF = 2.5
ADC2_V_REF = 2.5
ADCx_SPI_BAUDRATE = 1000000

ADC1_PIN_CS = 'PA4'
ADC2_PIN_CS = 'PB12'
ADC1_PIN_RDY_B = 'PA6'
ADC2_PIN_RDY_B = 'PB14'

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

#----------------------------------------------------------
def build_servo():
    in_1 = Mcp3551(bus=ADC1_SPI_CH, baudrate=ADCx_SPI_BAUDRATE,
                   rdy_b_pin=ADC1_PIN_RDY_B, cs_pin=ADC1_PIN_CS,
                   v_ref=ADC1_V_REF)
    in_2 = Mcp3551(bus=ADC2_SPI_CH, baudrate=ADCx_SPI_BAUDRATE,
                   rdy_b_pin=ADC2_PIN_RDY_B, cs_pin=ADC2_PIN_CS,
                   v_ref=ADC2_V_REF)
    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()
