silver2row

Trying old stuff w/ EOL equipment

Nov 23rd, 2020
871
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. def WriteByte(Reg,Value):
  2.     data = [0 for i in range(2)]
  3.     data[0] = Reg
  4.     data[1] = Value
  5.     MotorBridge.write_i2c_block_data(0x4b, 1, data)
  6.  
  7. def WriteHalfWord(Reg,Value):
  8.     data = [0 for i in range(3)]
  9.     data[0] = Reg
  10.     data[1] = Value & 0xff
  11.     data[2] = (Value>>8) & 0xff
  12.     MotorBridge.write_i2c_block_data(0x4b, 1, data)
  13.  
  14. def WriteOneWord(Reg,Value):
  15.     data = [0 for i in range(5)]
  16.     data[0] = Reg
  17.     data[1] = Value & 0xff
  18.     data[2] = (Value>>8) & 0xff
  19.     data[3] = (Value>>16) & 0xff
  20.     data[4] = (Value>>24) & 0xff
  21.     MotorBridge.write_i2c_block_data(0x4b, 1, data)
  22.  
  23. def SetDefault():
  24.     WriteOneWord(CONFIG_VALID,0x00000000)
  25.  
  26. class MotorBridgeCape:
  27.     def __init__(self):
  28.         reset_pin.write_text('high')
  29.         time.sleep(1)
  30.  
  31.     # init stepper motor A
  32.     def StepperMotorAInit(self):
  33.         WriteByte(TB_1A_MODE,TB_SPM) #Stepper
  34.         time.sleep(DelayTime)
  35.         WriteHalfWord(TB_1A_DUTY,1000)    # voltage
  36.         time.sleep(DelayTime)
  37.  
  38.     # MoveSteps > 0 CW
  39.     # MoveSteps < 0 CCW
  40.     # StepDelayTime : delay time for every step. uint us
  41.     def StepperMotorAMove(self,MoveSteps,StepDelayTime):
  42.         if MoveSteps > 0:
  43.             WriteByte(TB_1A_DIR,TB_CW)   #CW
  44.         else:
  45.             WriteByte(TB_1A_DIR,TB_CCW)   #CW
  46.             MoveSteps = -MoveSteps
  47.         time.sleep(DelayTime)
  48.         WriteOneWord(TB_1A_SPM_SPEED,StepDelayTime)  # unit us
  49.         time.sleep(DelayTime)
  50.         WriteOneWord(TB_1A_SPM_STEP,MoveSteps)
  51.         time.sleep(DelayTime)
  52.  
  53.  
  54.     # init stepper motor B
  55.     def StepperMotorBInit(self):
  56.         WriteByte(TB_2A_MODE,TB_SPM) #Stepper
  57.         time.sleep(DelayTime)
  58.         WriteHalfWord(TB_2A_DUTY,1000)    # voltage
  59.         time.sleep(DelayTime)
  60.  
  61.     # MoveSteps > 0 CW
  62.     # MoveSteps < 0 CCW
  63.     # StepDelayTime : delay time for every step. uint us
  64.     def StepperMotorBMove(self,MoveSteps,StepDelayTime):
  65.         if MoveSteps > 0:
  66.             WriteByte(TB_2A_DIR,TB_CW)   #CW
  67.         else:
  68.             WriteByte(TB_2A_DIR,TB_CCW)   #CW
  69.             MoveSteps = -MoveSteps
  70.         time.sleep(DelayTime)
  71.         WriteOneWord(TB_2A_SPM_SPEED,StepDelayTime)  # unit us
  72.         time.sleep(DelayTime)
  73.         WriteOneWord(TB_2A_SPM_STEP,MoveSteps)
  74.         time.sleep(DelayTime)
  75.  
  76.     # Init DC Motor
  77.     def DCMotorInit(self,MotorName,Frequency):
  78.     # Init the DC Frequency
  79.         WriteOneWord(CONFIG_TB_PWM_FREQ,Frequency)
  80.         time.sleep(DelayTime)
  81.  
  82. from smbus2 import SMBus
  83. import time
  84. import pathlib
  85.  
  86. # reset pin is P9.23, i.e. gpio1.17
  87. reset_pin = pathlib.Path('/sys/class/gpio/gpio49/direction')
  88. reset_pin.write_text('low')
  89.  
  90. MotorBridge = SMBus('/dev/i2c-2')
  91.  
  92. ReadMode  = 0
  93. WriteMode = 1
  94. DeAddr    = 0X4B
  95. ConfigValid = 0x3a6fb67c
  96. DelayTime = 0.005
RAW Paste Data