Closed WllTW closed 4 years ago
code
#import IMD_Parameter
import RPi.GPIO as GPIO
import smbus
import time
import struct
popo = 0
#####################################################
MotorA_Address = 0x05
MotorB_Address = 0x07
MotorC_Address = 0x06
MotorD_Address = 0x03
MotorA_DirectionPin = 35
MotorB_DirectionPin = 36
MotorC_DirectionPin = 37
MotorD_DirectionPin = 38
####################### Motor/Function Init(2)
HeaderCommand_HB = 0x09 # 0x09 start for python
HeaderCommand_LB = 0x30 # 0x30 clear IRQ State
####################### IRQ(3)
Command_AllParaReset = 0x20
Command_ClockOutputFunction = 0x22
Command_SetDividerLevel = 0x24
####################### EEPROM(4) not support
Command_ClearIRQState = 0x30
####################### Motor Wait Mode(5)
Command_WriteAddress = 0x45
Command_ReadAddress = 0x46
####################### Motor Direct Mode(6)
Command_CW_WaitMode = 0x50
Command_CCW_WaitMode = 0x52
####################### Motor_Mode(7)
Command_DirectMode = 0x61
#######################
Command_MotorBrake = 0x71
Command_MotorBrakeV2 = 0x72
Waitspeed = 250
directspeed = 500000
testarray = [0,1,2,3,4,5]
waitarray = [Command_CW_WaitMode,0,Waitspeed,0,100,5,6,7,8,0x55]
directarray = [Command_DirectMode,0,1,0,100,5,6,7,8,0x55]
directarray_Zero = [Command_DirectMode,0,1,0,0,5,6,7,8,0x55]
Cleararray = [0x30,0,150,3,4,5,6,7,8,0x55]
i2c = None
def initmpu():
global i2c0
global i2c1
global i2c5
global i2c6
i2c0 = smbus.SMBus(0)
time.sleep(1)
i2c1 = smbus.SMBus(1)
time.sleep(1)
i2c5 = smbus.SMBus(5)
i2c6 = smbus.SMBus(6)
# to use Raspberry pi board pin numbers
GPIO.setmode(GPIO.BOARD)
# set up GPIO output channel, we set GPIO to OUTPUT
GPIO.setup(MotorA_DirectionPin,GPIO.OUT)
GPIO.setup(MotorB_DirectionPin,GPIO.OUT)
GPIO.setup(MotorC_DirectionPin,GPIO.OUT)
GPIO.setup(MotorD_DirectionPin,GPIO.OUT)
def Direction_Test():
i2c0.write_block_data(MotorA_Address,0xAA,directarray)
print("MotorA_Address-directarray")
i2c1.write_block_data(MotorB_Address,0xAA,directarray)
print("MotorB_Address-directarray")
i2c5.write_block_data(MotorC_Address,0xAA,directarray)
print("MotorC_Address-directarray")
i2c6.write_block_data(MotorD_Address,0xAA,directarray)
print("MotorD_Address-directarray")
time.sleep(0.01)
i2c0.write_block_data(MotorA_Address,0xAA,Cleararray)
print("MotorA_Address-Cleararray")
i2c1.write_block_data(MotorB_Address,0xAA,Cleararray)
print("MotorB_Address-Cleararray")
i2c5.write_block_data(MotorC_Address,0xAA,Cleararray)
print("MotorC_Address-Cleararray")
i2c6.write_block_data(MotorD_Address,0xAA,Cleararray)
print("MotorD_Address-Cleararray")
time.sleep(0.5)
while (True):
initmpu()
print("init")
while (True):
i2c0.write_block_data(MotorD_Address,0xAA,testarray)
#i2c1.write_block_data(MotorB_Address,0xAA,Cleararray)
#i2c0.write_byte(MotorD_Address,0xaa);
print(popo)
popo = popo + 1
time.sleep(0.1)
#time.sleep(1)
Sorry for the delayed response- did you find a fix to this? I would take a wild guess that it's a wiring or signal issue.
Sorry for the delayed response- did you find a fix to this? I would take a wild guess that it's a wiring or signal issue.
Hi Gadgetoid Thank your reply. When I open the bus txt file, it can work!