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

"""
author  Benoit Dubois
licence GPL v3+
brief   Specific controller composed of 2 inputs 2 servo and 1 output:
details
            -------
   MCP1 >---|PID 1|---|
            -------   |
                      |-----> VNH509
            -------   |
   MCP2 >---|PID 2|---|
            -------
"""
import pyb

from serial_com import SerialCom
from mcp3551 import Mcp3551
from pid import PIDfixed
from vnh5019 import Vnh5019
from servo2to1_core import Servo2to1, Servo2to1Core

# 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_SERVO_ID = 4

TIMER_PWM_FREQ = 1000
TIMER_SERVO1_FREQ = 1
TIMER_SERVO2_FREQ = 10

TIMER_PWM_CH = 1

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' # MISO SPI 1
#ADC2_PIN_RDY_B = 'PB14' # MISO SPI 2

PIN_INA = 'PD6'
PIN_INB = 'PD2'
PIN_PWM = 'PA15'

#----------------------------------------------------------
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 = Vnh5019(in_a=PIN_INA, in_b=PIN_INB, pwm=PIN_PWM,
                    timer=TIMER_PWM_ID, freq=TIMER_PWM_FREQ,
                    ch=TIMER_PWM_CH)
    timer = pyb.Timer(TIMER_SERVO_ID, freq=TIMER_SERVO1_FREQ)
    servo = Servo2to1(in_1, in_2, pid_1, pid_2, out, timer,
                      TIMER_SERVO1_FREQ)
    servo.ctrl = 0
    return servo

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

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