ROS python stepper control and threading
Hi,
I am working on the control of my stepper motors for a differential drive (in python). When I try rotating one stepper motor with my code (with only left motor thread), it rotates at the specified speed (I used teleoperation twist keyboard to send linear and angular velocities) but when I add another thread for the second motor, both motors rotate twice slower. Could you please tell me why it is slower when I have two threads and how to solve this ?
Code:
class MotorThread(Thread):
def __init__(self,DIR,STEP):
super(MotorThread, self).__init__()
self.DIR = DIR
self.STEP = STEP
gpio.setmode(gpio.BCM)
# GPIO setups
def run(self):
if self.speed != 0 :
self.updateDelay()
GPIO.output(...HIGH)
sleep(self.delay/2)
GPIO.output(...LOW)
sleep(self.delay/2)
def updateDelay(self):
self.delay = abs(1/self.speed)
def updateSpeed(self,speed):
self.speed = speed
def stop(self):
self.updateSpeed(0.0)
gpio.cleanup()
self.join()
class ControllerNode:
def __init__(self):
self.x = 0.0
self.th = 0.0
self.leftMotor = MotorThread(20,21)
self.rightMotor = MotorThread(24,23)
self.leftMotor.setDaemon(True)
self.rightMotor.setDaemon(True)
def main(self):
self.leftMotor.start()
self.rightMotor.start()
rospy.init_node('controller_node')
rospy.Subscriber("cmd_vel",Twist,self.twistCb)
while not rospy.is_shutdown():
self.leftMotor.run()
self.rightMotor.run()
def twistCb(self,twist):
self.x = twist.linear.x
self.th = twist.angular.z
if self.x != 0 and self.th != 0:
lspeed = self.x + self.th * 0,1/2.0
rspeed = self.x - self.th * 0,1/2.0
elif self.x==0:
lspeed = self.th * 0,2/2.0
rspeed = -self.th * 0,2/2.0
elif self.th==0:
lspeed = self.x
rspeed = self.x
left_speed = (400*lspeed)/(2*pi*0,1) #speed in pulse per second
right_speed = (400*rspeed)/(2*pi*0,1)
self.leftMotor.updateSpeed(left_speed)
self.rightMotor.updateSpeed(right_speed)
def stop(self):
self.leftMotor.stop()
self.rightMotor.stop()
if __name__ == '__main__':
try:
node = ControllerNode()
node.main()
except rospy.ROSInterruptException:
pass
finally:
node.stop()
Thanks in advance,
Edit 1: it seems the issue comes from the second updateSpeed(speed) in the twistCallback() but I don't know to solve it. When I comment, for exemple, self.rightMotor.updateSpeed() function, the left motor rotates at the correct speed (for example : 1 m/s). When I uncomment it , it rotates slowly like 0,49 m/s instead of 1 m/s.
Are you sure your callback function is correct? I believe it should look something like this,
Also, you haven't assigned left_speed, right_speed in the callback? Also note, you are calling it right_motor and not right_speed but using right_speed in the updateSpeed function.