Blame view
software/servo/servo2to1/servo2to1_adc_vnh.py
1.53 KB
14b996da3
First commit
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 |
# -*- coding: utf-8 -*-
"""
author Benoit Dubois
licence GPL v3+
brief Specific controller composed of 2 inputs 2 servo and 1 output:
details
-------
ADC1 >---|PID 1|---|
------- |
|-----> VNH509
------- |
ADC2 >---|PID 2|---|
-------
"""
import pyb
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 = 2
TIMER_PWM_CH = 1
PIN_IN1 = 'PC5'
PIN_IN2 = 'PC1'
PIN_INA = 'PD6'
PIN_INB = 'PD2'
PIN_PWM = 'PA15'
#----------------------------------------------------------
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 = 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()
|