def __init__(self):
rospy.init_node("motor_pid")
self.nodeName = rospy.get_name()
# ROS Parameter
self.Kp = rospy.get_param('~Kp', 100)
self.Ki = rospy.get_param('~Ki', 150)
self.Kd = rospy.get_param('~Kd', 0)
self.rate = rospy.get_param('~rate',20)
self.timeout = rospy.Duration(rospy.get_param("~timeout", 0.2))
self.pinNameBwd = rospy.get_param('~pinNameFwd','')
self.pinNameFwd = rospy.get_param('~pinNameBwd','')
self.pinNameCmd = rospy.get_param('~pinNameCmd','')
self.minPwm = rospy.get_param('~minPwm',40)
if self.pinNameBwd=='' or self.pinNameFwd=='' or self.pinNameCmd=='':
rospy.logfatal("Parameters pinNameFwd, pinNameBwd and pinNameCmd are required.")
return
self.timeoutTime = None
# BeagleBone pin setup
GPIO.setup(self.pinNameFwd,GPIO.OUT,initial=0)
GPIO.setup(self.pinNameBwd,GPIO.OUT,initial=0)
self.pwmCmd = PWM(self.pinNameCmd,frequency=100,dutyCycle=0,enable=1)
# ROS publisher/subscribers
rospy.Subscriber("wheel_vel",Float64,self.wheelVelUpdate)
rospy.Subscriber("cmd_vel" ,Float64,self.cmdVelUpdate)
self.initialized = True
rospy.loginfo("%s started",self.nodeName)
def __init__(self):
rospy.init_node('wheel_encoder')
self.nodeName = rospy.get_name()
# ROS parameters
self.rate = rospy.get_param('~rate',20)
tpr = rospy.get_param('~ticks_per_rotation',0)
radius = float(rospy.get_param('~wheel_radius',0))
self.pinNameA = rospy.get_param('~pinNameA','')
self.pinNameB = rospy.get_param('~pinNameB','')
if self.pinNameA=='' or self.pinNameB=='' or tpr==0 or radius==0:
rospy.logfatal("Parameters pinNameA, pinNameB, wheel_radius and ticks_per_rotation are required")
return
self.m_per_tick = 2*math.pi*radius/tpr
self.tot_ticks = 0
# BeagleBone pin setup
self.cur_ticks = 0
GPIO.setup(self.pinNameA,GPIO.IN)
GPIO.setup(self.pinNameB,GPIO.IN)
GPIO.add_event_detect(self.pinNameA,GPIO.RISING,callback=self._tickUpdateA)
GPIO.setup(self.pinNameB,GPIO.IN)
# ROS publishers
self.pub_vel = rospy.Publisher('wheel_vel',Float64,queue_size=1)
self.pub_dst = rospy.Publisher('wheel_dst',Float64,queue_size=1)
self.initialized = True
rospy.loginfo("%s started",self.nodeName)
def writeDirection(self,direction):
rospy.logdebug("%s : going %s",self.nodeName,('Fwd' if direction>0 else 'Bwd'))
if direction>0:
GPIO.output(self.pinNameFwd,1)
GPIO.output(self.pinNameBwd,0)
else:
GPIO.output(self.pinNameFwd,0)
GPIO.output(self.pinNameBwd,1)
def stop(self): GPIO.output(self.pinNameFwd,0) GPIO.output(self.pinNameBwd,0) self.pwmCmd.setDutyCycle(0)
def _tickUpdateA(self,channel): pinB = GPIO.input(self.pinNameB) self.cur_ticks += 1 if pinB==0 else -1
