i'm trying to build alternates of modbus drive functions with pybullet setJointMotorControl2. please,help me out. All the suggestons are most welcome
Code: Select all
import time,os
testOffline = os.getenv("TEST_OFFLINE", True)
import pybullet as p
import pybullet_data
if testOffline ==True:
import pybullet as p
import pybullet_data
else:
import minimalmodbus as modbus
#cartpole1 = p.loadURDF("cartpole.urdf")
if testOffline ==True:
print("pybullet model loaded")
else:
drive = ""
slave = ""
print("Module flyby-modbus loaded.")
##############################################################################
### CONTROL the Motor Drive ###
# All CONTROL functions return { True | False } based on success/fail result.#
##############################################################################
if testOffline ==True:
def connectp():
p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
global cartpole1
cartpole1 = p.loadURDF("cartpole.urdf")
try:
reset()
print("connected")
return cartpole1
except Exception as e:
print(e)
return False
else:
def connect(connectionPort='/dev/cu.SLAB_USBtoUART', slaveID=7, mode='ascii', baudRate=9600, byteSize=8):
global drive, slave
slave = slaveID
try:
drive = modbus.Instrument(connectionPort, int(slaveID))
drive.mode = mode
drive.serial.baudrate = baudRate
drive.serial.bytesize = byteSize
reset()
print(drive)
return True
except Exception as e:
print(e)
return False
if testOffline ==True:
def startp(controlMode='DIGITAL', direction='CW'):
if controlMode=='DIGITAL':
if direction=='CW':
p.setJointMotorControl2(cartpole1,1,
targetVelocity=6)
elif direction=='CCW':
p.setJointMotorControl2(cartpole1,1,
targetVelocity=-6)
elif controlMode=='POSITION':
p.setJointMotorControl2(cartpole1,1,
p.POSITION_CONTROL)
try:
if not drive:
connectp()
else:
return True
except Exception as e:
print(e)
return False
else:
def start(controlMode='DIGITAL', direction='CW'):
if controlMode=='DIGITAL':
if direction=='CW':
registerValue = 257
elif direction=='CCW':
registerValue = 265
elif controlMode=='POSITION':
registerValue = 513
try:
if not drive:
connect()
else:
result = drive.write_register(2, registerValue, functioncode=6)
return True
except Exception as e:
print(e)
return False
if testOffline ==True:
def disconnect():
p.disconnect(p.GUI)
try:
if not drive:
connectp()
else:
return True
except Exception as e:
print(e)
return False
else:
def stop(controlMode='DIGITAL', direction='CW'):
if controlMode=='DIGITAL':
if direction=='CW':
registerValue = 256
elif direction=='CCW':
registerValue = 264
elif controlMode=='POSITION':
registerValue = 512
try:
if not drive:
connect()
else:
result = drive.write_register(2, registerValue, functioncode=6)
return True
except Exception as e:
print(e)
return False
if testOffline ==True:
def brakep(controlMode='DIGITAL', direction='CW'):
if controlMode=='DIGITAL':
if direction=='CW':
p.setJointMotorControl2(cartpole1,1,
targetVelocity=6)
elif direction=='CCW':
p.setJointMotorControl2(cartpole1,1,
targetVelocity=-6)
try:
if not drive:
connectp()
else:
return True
except Exception as e:
print(e)
return False
else:
def brake(direction='CW'):
if direction=='CW':
registerValue = 260
elif direction=='CCW':
registerValue = 268
try:
if not drive:
connect()
else:
result = drive.write_register(2, registerValue, functioncode=6)
return True
except Exception as e:
print(e)
return False
def reset():
try:
if not drive:
connect()
else:
drive.write_register(2, 0, functioncode=6)
drive.write_register(3, 0, functioncode=6)
drive.write_register(4, 32, functioncode=6)
drive.write_register(6, 16, functioncode=6)
drive.write_register(8, 32, functioncode=6)
drive.write_register(10, 334, functioncode=6)
drive.write_register(12, 20000, functioncode=6)
drive.write_register(14, 2048, functioncode=6)
drive.write_register(16, 0, functioncode=6)
drive.write_register(18, 0, functioncode=6)
drive.write_register(20, 0, functioncode=6)
return True
except Exception as e:
print(e)
return False
##############################################################################
### READ from Modbus Registers ####
# All READ functions return { <value>|"ERROR" } based on success/fail result.#
##############################################################################
if testOffline ==True:
def speed(cartpole1):
while(1):
p.setGravity(0, 0, -10)
js = p.getJointState(cartpole1, 1)
# print("position=", js[0], "velocity=", js[1])
return js[1]
time.sleep(0.01)
else:
def readRegister(registerNumber, readNextCount=0):
try:
if not drive:
connect()
else:
result = drive.read_register(registerNumber, readNextCount, functioncode=3)
return str(result)
except Exception as e:
print(e)
return "ERROR"
def getSlaveID():
return readRegister(registerNumber=1, readNextCount=0)
def getControlMode():
return readRegister(registerNumber=2, readNextCount=0) + "|" + readRegister(registerNumber=3, readNextCount=0)
def getProportionalGain():
return readRegister(registerNumber=4, readNextCount=0)
def getIntegralGain():
return readRegister(registerNumber=6, readNextCount=0)
def getFeedForwardGain():
return readRegister(registerNumber=8, readNextCount=0)
def getLinesPerRotation():
return readRegister(registerNumber=10, readNextCount=0)
def getAcceleration():
return readRegister(registerNumber=12, readNextCount=0)
def getSpeed():
return readRegister(registerNumber=14, readNextCount=0)
def getSpeedFeedback():
# speed is a positive number between 0 (0x0000) and 65535 (0xFFFF)
# 0 to 18000 is the range of speed this motor supports, however, setting a speed of 18000 should be avoided in the first place
minRange = 0
maxRange = 65535
spdFdbk = readRegister(registerNumber=24, readNextCount=0)
# if the spdFdbk is less than 32767, that means motor is in CW direction -- it must be returned absolute
if int(spdFdbk) < 32767:
pass
# if the spdFdbk is more than 32767, that means motor is in CCW direction -- it must be adjusted accordingly
elif int(spdFdbk) > 32767:
spdFdbk = maxRange - int(spdFdbk)
return spdFdbk
def getPositionFeedback():
# position is a positive number between 0 to 4294967285
lsb = readRegister(registerNumber=20, readNextCount=0)
msb = readRegister(registerNumber=22, readNextCount=0)
# convert position feedback to hexadecimal with MSB and LSB joining to make 32-bits
lsbInHex = hex(int(lsb))[2:].zfill(4)
msbInHex = hex(int(msb))[2:].zfill(4)
pos = int("0x" + msbInHex + lsbInHex, 0)
return pos
###########################################################################
### WRITE to Modbus Registers ###
# All READ functions return { True | False } based on success/fail result.#
###########################################################################
if testOffline ==True:
global cartpole1
p.setJointMotorControl2(cartpole1,
1,
p.POSITION_CONTROL,
targetPosition=1000,
targetVelocity=6,
force=5,
positionGain=1.9,
velocityGain=0.25,
maxVelocity=10)
#return a[2]
else:
def writeRegister(registerNumber, registerValue):
try:
if not drive:
connect()
else:
result = drive.write_register(registerNumber, registerValue, functioncode=6)
return True
except Exception as e:
print(e)
return False
def setProportionalGain(proportionalGain=32):
return writeRegister(registerNumber=4, registerValue=int(proportionalGain))
def setIntegralGain(integralGain=16):
return writeRegister(registerNumber=6, registerValue=int(integralGain))
def setFeedForwardGain(feedForwardGain=32):
return writeRegister(registerNumber=8, registerValue=int(feedForwardGain))
def setLinesPerRotation(linesPerRotation=334):
return writeRegister(registerNumber=10, registerValue=int(linesPerRotation))
def setAcceleration(acceleration=20000):
return writeRegister(registerNumber=12, registerValue=int(acceleration))
def setSpeed(speed=2048):
# speed is a positive number between 0 (0x0000) and 65535 (0xFFFF)
# 0 to 18000 is the range of speed this motor supports, however, setting a speed of 18000 should be avoided in the first place
minRange = 0
maxRange = 65535
if minRange <= speed <= maxRange:
writeRegister(registerNumber=14, registerValue=int(speed))
return
# gotoPosition() instructs the motor drive to execute "immediately" by rotating it CW or CCW based on desired position requested.
def gotoPosition(position=0):
# position is a positive number between 0 (0x00000000) and 4294967285 (0xFFFFFFFF)
minRange = 0
maxRange = 4294967295
if minRange <= position <= maxRange:
# convert position to hexadecimal with 32-bits, before extracting msb & lsb
posInHex = hex(position)[2:].zfill(8)
msb = "0x" + posInHex[0:4]
lsb = "0x" + posInHex[4:8]
writeRegister(registerNumber=16, registerValue=int(lsb, 0))
writeRegister(registerNumber=18, registerValue=int(msb, 0))
#############################
##### Start the motor! #####
start(controlMode='POSITION')
# Keep waiting until the motor has reached the desired position
while getPositionFeedback()!=position:
pass
return