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