ROS python stepper control and threading

asked 2020-07-27 05:48:12 -0500

Shanika gravatar image

updated 2020-07-27 07:25:15 -0500

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.

edit retag flag offensive close merge delete

Comments

Are you sure your callback function is correct? I believe it should look something like this,

def twistCb(self, data):
         ...

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.

praskot gravatar image praskot  ( 2020-07-27 07:12:15 -0500 )edit