bdubois / stm32upy

Blame view

software/servo/servo2to2/servo2to2_mcp_vnh.py 2 KB
edit raw normal view history
14b996da3   benny   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
65
66
67
68
69
70
71
72
73
74
75
76
77
 
  # -*- 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()