Python rospy.logdebug() Examples

The following are 30 code examples of rospy.logdebug(). You can vote up the ones you like or vote down the ones you don't like, and go to the original project or source file by following the links above each example. You may also want to check out all available functions/classes of the module rospy , or try the search function .
Example #1
Source File: arduino (copy).py    From ROS-Programming-Building-Powerful-Robots with MIT License 6 votes vote down vote up 
def _InitializeBatteryMonitor(self):
	
#		rospy.logdebug("Initializing battery monitor. self._VoltageLowLowlimitvoltageTooLowLimit = " + str(self._VoltageLowLowlimit))
		rospy.logdebug("Initializing battery monitor. voltageTooLowLimit = " + str(self._VoltageLowlimit))
		
		temp_high_v = self._VoltageHighlimit * 1000
		self._VoltageHighlimit = temp_high_v
		temp_low_v = self._VoltageLowlimit * 1000
		self._VoltageLowlimit = temp_low_v 

		
		message = 'bm %d %d\r' % (self._VoltageHighlimit,self._VoltageLowlimit)
		rospy.logdebug("Sending battery monitor params message: " + message)
		self._WriteSerial(message)
		self._VoltageHighlimit = 12
		self._VoltageLowlimit = 11
Example #2
Source File: lpid_velocity.py    From ROS-Programming-Building-Powerful-Robots with MIT License 6 votes vote down vote up 
def wheelCallback(self, msg):
    ######################################################
        enc = msg.data
        if (enc < self.encoder_low_wrap and self.prev_encoder > self.encoder_high_wrap) :
            self.wheel_mult = self.wheel_mult + 1
            
        if (enc > self.encoder_high_wrap and self.prev_encoder < self.encoder_low_wrap) :
            self.wheel_mult = self.wheel_mult - 1
           
         
        self.wheel_latest = 1.0 * (enc + self.wheel_mult * (self.encoder_max - self.encoder_min)) / self.ticks_per_meter 
        self.prev_encoder = enc
        
        
#        rospy.logdebug("-D- %s wheelCallback msg.data= %0.3f wheel_latest = %0.3f mult=%0.3f" % (self.nodename, enc, self.wheel_latest, self.wheel_mult))
    
    ######################################################
Example #3
Source File: head_lift_joy.py    From cozmo_driver with Apache License 2.0 6 votes vote down vote up 
def _joy_cb(self, msg):
        """
        The joy/game-pad message callback.

        :type   msg:    Joy
        :param  msg:    The incoming joy message.

        """
        if msg.buttons[self._head_button] == 1:
            angle_deg = ((msg.axes[self._head_axes] + 1.0) / 2.0) * SUM_HEAD_ANGLE + MIN_HEAD_ANGLE
            rospy.logdebug('Send head angle: {} degrees.'.format(angle_deg))
            self._head_pub.publish(data=angle_deg)

        if msg.buttons[self._lift_button] == 1:
            lift_mm = abs(msg.axes[self._lift_axes]) * SUM_LIFT_HEIGHT + MIN_LIFT_HEIGHT
            rospy.logdebug('Send lift height: {} mm.'.format(lift_mm))
            self._lift_pub.publish(data=abs(msg.axes[self._lift_axes]))
Example #4
Source File: pid_velocity.py    From differential-drive with GNU General Public License v3.0 6 votes vote down vote up 
def wheelCallback(self, msg):
    ######################################################
        enc = msg.data
        if (enc < self.encoder_low_wrap and self.prev_encoder > self.encoder_high_wrap) :
            self.wheel_mult = self.wheel_mult + 1
            
        if (enc > self.encoder_high_wrap and self.prev_encoder < self.encoder_low_wrap) :
            self.wheel_mult = self.wheel_mult - 1
           
         
        self.wheel_latest = 1.0 * (enc + self.wheel_mult * (self.encoder_max - self.encoder_min)) / self.ticks_per_meter 
        self.prev_encoder = enc
        
        
#        rospy.logdebug("-D- %s wheelCallback msg.data= %0.3f wheel_latest = %0.3f mult=%0.3f" % (self.nodename, enc, self.wheel_latest, self.wheel_mult))
    
    ######################################################
Example #5
Source File: arduino (copy).py    From ROS-Programming-Building-Powerful-Robots with MIT License 6 votes vote down vote up 
def _InitializeDriveGeometry(self):
		wheelDiameter = rospy.get_param("~driveGeometry/wheelDiameter", "0.9")
		trackWidth = rospy.get_param("~driveGeometry/trackWidth", "0.1")
		countsPerRevolution = rospy.get_param("~driveGeometry/countsPerRevolution", "2000")
		

		wheelDiameter= wheelDiameter * 1000
		trackWidth=trackWidth * 1000


		#countsPerRevolution=countsperRevolution*1000;
		#wheelDiameterParts = self._GetBaseAndExponent(wheelDiameter)
		#trackWidthParts = self._GetBaseAndExponent(trackWidth)

		message = 'dg %d %d %d\r' % (int(wheelDiameter), int(trackWidth), int(countsPerRevolution))
		rospy.logdebug("Sending drive geometry params message: " + message)
		self._WriteSerial(message)
Example #6
Source File: arduino_bkup.py    From ROS-Programming-Building-Powerful-Robots with MIT License 6 votes vote down vote up 
def _InitializeSpeedController(self):
		velocityPParam = rospy.get_param("~speedController/velocityPParam", "0")
		velocityIParam = rospy.get_param("~speedController/velocityIParam", "0")
		turnPParam = rospy.get_param("~speedController/turnPParam", "0")
		turnIParam = rospy.get_param("~speedController/turnIParam", "0")
		commandTimeout = self._GetCommandTimeoutForSpeedController()

		message = 'sc %.2f %.2f %.2f %.2f %.2f\r' % (velocityPParam, velocityIParam, turnPParam, turnIParam, commandTimeout)
		#message = 'sc %f %f %f\r' % (velocityPParam, velocityIParam, turnPParam)
		rospy.logdebug("Sending differential drive gains message: " + message)
		self._WriteSerial(message)


		#speedControllerParams = (velocityPParam, velocityIParam, turnPParam, turnIParam, commandTimeout)
		#rospy.loginfo(str(speedControllerParams))
		#self._WriteSpeedControllerParams(speedControllerParams)
Example #7
Source File: arduino.py    From ROS-Programming-Building-Powerful-Robots with MIT License 6 votes vote down vote up 
def _InitializeSpeedController(self):
		
		velocityPParam = rospy.get_param("~speedController/velocityPParam", "1")
		velocityIParam = rospy.get_param("~speedController/velocityIParam", "1")
		turnPParam = rospy.get_param("~speedController/turnPParam", "1")
		turnIParam = rospy.get_param("~speedController/turnIParam", "1")
		commandTimeout = self._GetCommandTimeoutForSpeedController()
		

		velocityPParam=velocityPParam * 1000
		velocityIParam= velocityIParam * 1000
		turnPParam = turnPParam * 1000
		turnIParam=turnIParam * 1000 


		message = 'sc %d %d %d %d %d\r' % (int(velocityPParam), int(velocityIParam), int(turnPParam), int(turnIParam), int(commandTimeout))
		#message = 'sc %f %f %f\r' % (velocityPParam, velocityIParam, turnPParam)
		rospy.logdebug("Sending differential drive gains message: " + message)
		self._WriteSerial(message)


		#speedControllerParams = (velocityPParam, velocityIParam, turnPParam, turnIParam, commandTimeout)
		#rospy.loginfo(str(speedControllerParams))
		#self._WriteSpeedControllerParams(speedControllerParams)
Example #8
Source File: arduino.py    From ROS-Programming-Building-Powerful-Robots with MIT License 6 votes vote down vote up 
def _InitializeBatteryMonitor(self):
	
#		rospy.logdebug("Initializing battery monitor. self._VoltageLowLowlimitvoltageTooLowLimit = " + str(self._VoltageLowLowlimit))
		rospy.logdebug("Initializing battery monitor. voltageTooLowLimit = " + str(self._VoltageLowlimit))
		
		temp_high_v = self._VoltageHighlimit * 1000
		self._VoltageHighlimit = temp_high_v
		temp_low_v = self._VoltageLowlimit * 1000
		self._VoltageLowlimit = temp_low_v 

		
		message = 'bm %d %d\r' % (self._VoltageHighlimit,self._VoltageLowlimit)
		rospy.logdebug("Sending battery monitor params message: " + message)
		self._WriteSerial(message)
		self._VoltageHighlimit = 12
		self._VoltageLowlimit = 11
Example #9
Source File: arduino.py    From ROS-Programming-Building-Powerful-Robots with MIT License 6 votes vote down vote up 
def _InitializeSpeedController(self):
		
		velocityPParam = rospy.get_param("~speedController/velocityPParam", "1")
		velocityIParam = rospy.get_param("~speedController/velocityIParam", "1")
		turnPParam = rospy.get_param("~speedController/turnPParam", "1")
		turnIParam = rospy.get_param("~speedController/turnIParam", "1")
		commandTimeout = self._GetCommandTimeoutForSpeedController()
		

		velocityPParam=velocityPParam * 1000
		velocityIParam= velocityIParam * 1000
		turnPParam = turnPParam * 1000
		turnIParam=turnIParam * 1000 


		message = 'sc %d %d %d %d %d\r' % (int(velocityPParam), int(velocityIParam), int(turnPParam), int(turnIParam), int(commandTimeout))
		#message = 'sc %f %f %f\r' % (velocityPParam, velocityIParam, turnPParam)
		rospy.logdebug("Sending differential drive gains message: " + message)
		self._WriteSerial(message)


		#speedControllerParams = (velocityPParam, velocityIParam, turnPParam, turnIParam, commandTimeout)
		#rospy.loginfo(str(speedControllerParams))
		#self._WriteSpeedControllerParams(speedControllerParams)
Example #10
Source File: arduino_bkup.py    From ROS-Programming-Building-Powerful-Robots with MIT License 6 votes vote down vote up 
def _InitializeSpeedController(self):
		velocityPParam = rospy.get_param("~speedController/velocityPParam", "0")
		velocityIParam = rospy.get_param("~speedController/velocityIParam", "0")
		turnPParam = rospy.get_param("~speedController/turnPParam", "0")
		turnIParam = rospy.get_param("~speedController/turnIParam", "0")
		commandTimeout = self._GetCommandTimeoutForSpeedController()

		message = 'sc %.2f %.2f %.2f %.2f %.2f\r' % (velocityPParam, velocityIParam, turnPParam, turnIParam, commandTimeout)
		#message = 'sc %f %f %f\r' % (velocityPParam, velocityIParam, turnPParam)
		rospy.logdebug("Sending differential drive gains message: " + message)
		self._WriteSerial(message)


		#speedControllerParams = (velocityPParam, velocityIParam, turnPParam, turnIParam, commandTimeout)
		#rospy.loginfo(str(speedControllerParams))
		#self._WriteSpeedControllerParams(speedControllerParams)
Example #11
Source File: pid_velocity.py    From ROS-Programming-Building-Powerful-Robots with MIT License 6 votes vote down vote up 
def wheelCallback(self, msg):
    ######################################################
        enc = long(msg.data)
#	rospy.logwarn(enc)
        if (enc < self.encoder_low_wrap and self.prev_encoder > self.encoder_high_wrap) :
            self.wheel_mult = self.wheel_mult + 1
            
        if (enc > self.encoder_high_wrap and self.prev_encoder < self.encoder_low_wrap) :
            self.wheel_mult = self.wheel_mult - 1
           
         
        self.wheel_latest = 1.0 * (enc + self.wheel_mult * (self.encoder_max - self.encoder_min)) / self.ticks_per_meter 
        self.prev_encoder = enc
        
        
#        rospy.logdebug("-D- %s wheelCallback msg.data= %0.3f wheel_latest = %0.3f mult=%0.3f" % (self.nodename, enc, self.wheel_latest, self.wheel_mult))
    
    ######################################################
Example #12
Source File: lpid_velocity.py    From ROS-Programming-Building-Powerful-Robots with MIT License 6 votes vote down vote up 
def wheelCallback(self, msg):
    ######################################################
        enc = msg.data
        if (enc < self.encoder_low_wrap and self.prev_encoder > self.encoder_high_wrap) :
            self.wheel_mult = self.wheel_mult + 1
            
        if (enc > self.encoder_high_wrap and self.prev_encoder < self.encoder_low_wrap) :
            self.wheel_mult = self.wheel_mult - 1
           
         
        self.wheel_latest = 1.0 * (enc + self.wheel_mult * (self.encoder_max - self.encoder_min)) / self.ticks_per_meter 
        self.prev_encoder = enc
        
        
#        rospy.logdebug("-D- %s wheelCallback msg.data= %0.3f wheel_latest = %0.3f mult=%0.3f" % (self.nodename, enc, self.wheel_latest, self.wheel_mult))
    
    ######################################################
Example #13
Source File: rpid_velocity.py    From ROS-Programming-Building-Powerful-Robots with MIT License 6 votes vote down vote up 
def wheelCallback(self, msg):
    ######################################################
        enc = msg.data
        if (enc < self.encoder_low_wrap and self.prev_encoder > self.encoder_high_wrap) :
            self.wheel_mult = self.wheel_mult + 1
            
        if (enc > self.encoder_high_wrap and self.prev_encoder < self.encoder_low_wrap) :
            self.wheel_mult = self.wheel_mult - 1
           
         
        self.wheel_latest = 1.0 * (enc + self.wheel_mult * (self.encoder_max - self.encoder_min)) / self.ticks_per_meter 
        self.prev_encoder = enc
        
        
#        rospy.logdebug("-D- %s wheelCallback msg.data= %0.3f wheel_latest = %0.3f mult=%0.3f" % (self.nodename, enc, self.wheel_latest, self.wheel_mult))
    
    ######################################################
Example #14
Source File: arduino.py    From ROS-Programming-Building-Powerful-Robots with MIT License 6 votes vote down vote up 
def _InitializeDriveGeometry(self):
		wheelDiameter = rospy.get_param("~driveGeometry/wheelDiameter", "0.9")
		trackWidth = rospy.get_param("~driveGeometry/trackWidth", "0.1")
		countsPerRevolution = rospy.get_param("~driveGeometry/countsPerRevolution", "2000")
		

		wheelDiameter= wheelDiameter * 1000
		trackWidth=trackWidth * 1000


		#countsPerRevolution=countsperRevolution*1000;
		#wheelDiameterParts = self._GetBaseAndExponent(wheelDiameter)
		#trackWidthParts = self._GetBaseAndExponent(trackWidth)

		message = 'dg %d %d %d\r' % (int(wheelDiameter), int(trackWidth), int(countsPerRevolution))
		rospy.logdebug("Sending drive geometry params message: " + message)
		self._WriteSerial(message)
Example #15
Source File: pid_velocity_sim.py    From ROS-Programming-Building-Powerful-Robots with MIT License 6 votes vote down vote up 
def wheelCallback(self, msg):
    ######################################################
        enc = long(msg.data)
#	rospy.logwarn(enc)
        if (enc < self.encoder_low_wrap and self.prev_encoder > self.encoder_high_wrap) :
            self.wheel_mult = self.wheel_mult + 1
            
        if (enc > self.encoder_high_wrap and self.prev_encoder < self.encoder_low_wrap) :
            self.wheel_mult = self.wheel_mult - 1
           
         
        self.wheel_latest = 1.0 * (enc + self.wheel_mult * (self.encoder_max - self.encoder_min)) / self.ticks_per_meter 
        self.prev_encoder = enc
        
        
#        rospy.logdebug("-D- %s wheelCallback msg.data= %0.3f wheel_latest = %0.3f mult=%0.3f" % (self.nodename, enc, self.wheel_latest, self.wheel_mult))
    
    ######################################################
Example #16
Source File: arduino (copy).py    From ROS-Programming-Building-Powerful-Robots with MIT License 6 votes vote down vote up 
def _InitializeDriveGeometry(self):
		wheelDiameter = rospy.get_param("~driveGeometry/wheelDiameter", "0.9")
		trackWidth = rospy.get_param("~driveGeometry/trackWidth", "0.1")
		countsPerRevolution = rospy.get_param("~driveGeometry/countsPerRevolution", "2000")
		

		wheelDiameter= wheelDiameter * 1000
		trackWidth=trackWidth * 1000


		#countsPerRevolution=countsperRevolution*1000;
		#wheelDiameterParts = self._GetBaseAndExponent(wheelDiameter)
		#trackWidthParts = self._GetBaseAndExponent(trackWidth)

		message = 'dg %d %d %d\r' % (int(wheelDiameter), int(trackWidth), int(countsPerRevolution))
		rospy.logdebug("Sending drive geometry params message: " + message)
		self._WriteSerial(message)
Example #17
Source File: arduino (copy).py    From ROS-Programming-Building-Powerful-Robots with MIT License 6 votes vote down vote up 
def _InitializeSpeedController(self):
		
		velocityPParam = rospy.get_param("~speedController/velocityPParam", "1")
		velocityIParam = rospy.get_param("~speedController/velocityIParam", "1")
		turnPParam = rospy.get_param("~speedController/turnPParam", "1")
		turnIParam = rospy.get_param("~speedController/turnIParam", "1")
		commandTimeout = self._GetCommandTimeoutForSpeedController()
		

		velocityPParam=velocityPParam * 1000
		velocityIParam= velocityIParam * 1000
		turnPParam = turnPParam * 1000
		turnIParam=turnIParam * 1000 


		message = 'sc %d %d %d %d %d\r' % (int(velocityPParam), int(velocityIParam), int(turnPParam), int(turnIParam), int(commandTimeout))
		#message = 'sc %f %f %f\r' % (velocityPParam, velocityIParam, turnPParam)
		rospy.logdebug("Sending differential drive gains message: " + message)
		self._WriteSerial(message)


		#speedControllerParams = (velocityPParam, velocityIParam, turnPParam, turnIParam, commandTimeout)
		#rospy.loginfo(str(speedControllerParams))
		#self._WriteSpeedControllerParams(speedControllerParams)
Example #18
Source File: arduino.py    From ROS-Programming-Building-Powerful-Robots with MIT License 6 votes vote down vote up 
def _InitializeBatteryMonitor(self):
	
#		rospy.logdebug("Initializing battery monitor. self._VoltageLowLowlimitvoltageTooLowLimit = " + str(self._VoltageLowLowlimit))
		rospy.logdebug("Initializing battery monitor. voltageTooLowLimit = " + str(self._VoltageLowlimit))
		
		temp_high_v = self._VoltageHighlimit * 1000
		self._VoltageHighlimit = temp_high_v
		temp_low_v = self._VoltageLowlimit * 1000
		self._VoltageLowlimit = temp_low_v 

		
		message = 'bm %d %d\r' % (self._VoltageHighlimit,self._VoltageLowlimit)
		rospy.logdebug("Sending battery monitor params message: " + message)
		self._WriteSerial(message)
		self._VoltageHighlimit = 12
		self._VoltageLowlimit = 11
Example #19
Source File: arduino_bkup.py    From ROS-Programming-Building-Powerful-Robots with MIT License 5 votes vote down vote up 
def _InitializeBatteryMonitor(self):
		rospy.logdebug("Initializing battery monitor. voltageTooLowLimit = " + str(self._VoltageLowLowlimit))

		message = 'bm %f\r' % self._VoltageLowLowlimit
		rospy.logdebug("Sending battery monitor params message: " + message)
		self._WriteSerial(message)
Example #20
Source File: rpid_velocity.py    From ROS-Programming-Building-Powerful-Robots with MIT License 5 votes vote down vote up 
def doPid(self):
    #####################################################
        pid_dt_duration = rospy.Time.now() - self.prev_pid_time
        pid_dt = pid_dt_duration.to_sec()
        self.prev_pid_time = rospy.Time.now()
        
        self.error = self.target - self.vel
        self.integral = self.integral + (self.error * pid_dt)
        # rospy.loginfo("i = i + (e * dt):  %0.3f = %0.3f + (%0.3f * %0.3f)" % (self.integral, self.integral, self.error, pid_dt))
        self.derivative = (self.error - self.previous_error) / pid_dt
        self.previous_error = self.error
    
        self.motor = (self.Kp * self.error) + (self.Ki * self.integral) + (self.Kd * self.derivative)
    
        if self.motor > self.out_max:
            self.motor = self.out_max
            self.integral = self.integral - (self.error * pid_dt)
        if self.motor < self.out_min:
            self.motor = self.out_min
            self.integral = self.integral - (self.error * pid_dt)
      
        if (self.target == 0):
            self.motor = 0
    
        rospy.logdebug("vel:%0.2f tar:%0.2f err:%0.2f int:%0.2f der:%0.2f ## motor:%d " % 
                      (self.vel, self.target, self.error, self.integral, self.derivative, self.motor))
    
    


    #####################################################
Example #21
Source File: pid_velocity.py    From ROS-Programming-Building-Powerful-Robots with MIT License 5 votes vote down vote up 
def doPid(self):
    #####################################################
        pid_dt_duration = rospy.Time.now() - self.prev_pid_time
        pid_dt = pid_dt_duration.to_sec()
        self.prev_pid_time = rospy.Time.now()
        
        self.error = self.target - self.vel
        self.integral = self.integral + (self.error * pid_dt)
        # rospy.loginfo("i = i + (e * dt):  %0.3f = %0.3f + (%0.3f * %0.3f)" % (self.integral, self.integral, self.error, pid_dt))
        self.derivative = (self.error - self.previous_error) / pid_dt
        self.previous_error = self.error
    
        self.motor = (self.Kp * self.error) + (self.Ki * self.integral) + (self.Kd * self.derivative)
    
        if self.motor > self.out_max:
            self.motor = self.out_max
            self.integral = self.integral - (self.error * pid_dt)
        if self.motor < self.out_min:
            self.motor = self.out_min
            self.integral = self.integral - (self.error * pid_dt)
      
        if (self.target == 0):
            self.motor = 0
    
        rospy.logdebug("vel:%0.2f tar:%0.2f err:%0.2f int:%0.2f der:%0.2f ## motor:%d " % 
                      (self.vel, self.target, self.error, self.integral, self.derivative, self.motor))
    
    


    #####################################################
Example #22
Source File: rpid_velocity.py    From ROS-Programming-Building-Powerful-Robots with MIT License 5 votes vote down vote up 
def targetCallback(self, msg):
    ######################################################
        self.target = msg.data
        self.ticks_since_target = 0
        # rospy.logdebug("-D- %s targetCallback " % (self.nodename))
Example #23
Source File: pid_velocity.py    From ROS-Programming-Building-Powerful-Robots with MIT License 5 votes vote down vote up 
def calcVelocity(self):
    #####################################################
        self.dt_duration = rospy.Time.now() - self.then
        self.dt = self.dt_duration.to_sec()
        rospy.logdebug("-D- %s caclVelocity dt=%0.3f wheel_latest=%0.3f wheel_prev=%0.3f" % (self.nodename, self.dt, self.wheel_latest, self.wheel_prev))
        
        if (self.wheel_latest == self.wheel_prev):
            # we haven't received an updated wheel lately
            cur_vel = (1 / self.ticks_per_meter) / self.dt    # if we got a tick right now, this would be the velocity
            if abs(cur_vel) < self.vel_threshold: 
                # if the velocity is < threshold, consider our velocity 0
                rospy.logdebug("-D- %s below threshold cur_vel=%0.3f vel=0" % (self.nodename, cur_vel))
                self.appendVel(0)
                self.calcRollingVel()
            else:
                rospy.logdebug("-D- %s above threshold cur_vel=%0.3f" % (self.nodename, cur_vel))
                if abs(cur_vel) < self.vel:
                    rospy.logdebug("-D- %s cur_vel < self.vel" % self.nodename)
                    # we know we're slower than what we're currently publishing as a velocity
                    self.appendVel(cur_vel)
                    self.calcRollingVel()
            
        else:
            # we received a new wheel value
            cur_vel = (self.wheel_latest - self.wheel_prev) / self.dt
            self.appendVel(cur_vel)
            self.calcRollingVel()
            rospy.logdebug("-D- %s **** wheel updated vel=%0.3f **** " % (self.nodename, self.vel))
            self.wheel_prev = self.wheel_latest
            self.then = rospy.Time.now()
            
        self.pub_vel.publish(self.vel)
        
    #####################################################
Example #24
Source File: launchpad_node (copy).py    From ROS-Programming-Building-Powerful-Robots with MIT License 5 votes vote down vote up 
def Stop(self):
		rospy.logdebug("Stopping")
		self._SerialDataGateway.Stop()
		

		
#######################################################################################################################
Example #25
Source File: launchpad_node (copy).py    From ROS-Programming-Building-Powerful-Robots with MIT License 5 votes vote down vote up 
def Start(self):
		rospy.logdebug("Starting")
		self._SerialDataGateway.Start()

#######################################################################################################################
Example #26
Source File: pid_velocity_sim.py    From ROS-Programming-Building-Powerful-Robots with MIT License 5 votes vote down vote up 
def targetCallback(self, msg):
    ######################################################
        self.target = msg.data
        self.ticks_since_target = 0
        # rospy.logdebug("-D- %s targetCallback " % (self.nodename))
Example #27
Source File: arduino (copy).py    From ROS-Programming-Building-Powerful-Robots with MIT License 5 votes vote down vote up 
def Start(self):
		rospy.logdebug("Starting")
		self._SerialDataGateway.Start()
Example #28
Source File: pid_velocity_sim.py    From ROS-Programming-Building-Powerful-Robots with MIT License 5 votes vote down vote up 
def doPid(self):
    #####################################################
#	self.motor = 200

#        pid_dt_duration = rospy.Time.now() - self.prev_pid_time
#        pid_dt = pid_dt_duration.to_sec()
#        self.prev_pid_time = rospy.Time.now()
        
#        self.error = self.target - self.vel
#        self.integral = self.integral + (self.error * pid_dt)
        # rospy.loginfo("i = i + (e * dt):  %0.3f = %0.3f + (%0.3f * %0.3f)" % (self.integral, self.integral, self.error, pid_dt))
#        self.derivative = (self.error - self.previous_error) / pid_dt
#        self.previous_error = self.error
    
#        self.motor = (self.Kp * self.error) + (self.Ki * self.integral) + (self.Kd * self.derivative)
    
        if self.motor > self.out_max:
            self.motor = 200
#            self.integral = self.integral - (self.error * pid_dt)
        if self.motor < self.out_min:
            self.motor = -200
#            self.integral = self.integral - (self.error * pid_dt)
      
        if (self.target == 0):
            self.motor = 0
    
#        rospy.logdebug("vel:%0.2f tar:%0.2f err:%0.2f int:%0.2f der:%0.2f ## motor:%d " % 
#                      (self.vel, self.target, self.error, self.integral, self.derivative, self.motor))
    
    


    #####################################################
Example #29
Source File: arduino (copy).py    From ROS-Programming-Building-Powerful-Robots with MIT License 5 votes vote down vote up 
def Stop(self):
		rospy.logdebug("Stopping")
		self._SerialDataGateway.Stop()
Example #30
Source File: pid_velocity.py    From ROS-Programming-Building-Powerful-Robots with MIT License 5 votes vote down vote up 
def targetCallback(self, msg):
    ######################################################
        self.target = msg.data
        self.ticks_since_target = 0
        # rospy.logdebug("-D- %s targetCallback " % (self.nodename))