def callback(self, data):
# signals
s1, s2 = data.delta_encoder1, data.delta_encoder2
# estimated velocities
r = 0.0352
b = 0.115
f = 10
tpr = 360
vw1, vw2 = 2 * np.pi * r * f * s1 / tpr, 2 * np.pi * r * f * s2 / tpr
# errors
e1, e2 = self.vw1 - vw1, self.vw2 - vw2
# integration error
self.int1 += e1 * 0.1
self.int2 += e2 * 0.1
# control signals
sig = PWM()
sig.PWM1 = self.kp * e1 + self.ki * self.int1
sig.PWM2 = self.kp * e2 + self.ki * self.int2
#print(e1, e2)
self.pub.publish(sig)
def cb_enc(self, data):
# signals
s1, s2 = data.delta_encoder1, data.delta_encoder2
# estimated velocities
vw1, vw2 = [2*np.pi*self.r*self.f*s/self.tpr for s in (s1, s2)]
# error from final translational speed
e1, e2 = [self.v - v for v in [vw1, vw2]]
# error from angle
ew = 10*(self.d1 - self.d2)
e1 -= ew
e2 += ew
# error from distance
d = (self.d1 + self.d2/2)
print("Distance to wall: " + str(d))
d = 0.5*(self.d - d)
e1 += d
e2 -= d
# integration error
self.int1 += e1*0.1
self.int2 += e2*0.1
# control signals
sig = PWM()
sig.PWM1 = self.kp*e1 + self.ki*self.int1
sig.PWM2 = self.kp*e2 + self.ki*self.int2
self.pub_pwm.publish(sig)
def publisher_pwm():
pub_pwm = rospy.Publisher('/kobuki/pwm', PWM, queue_size=10)
rospy.init_node('talker', anonymous=True)
# publishing rate is 10 Hz, or every 100 milli sec
rate = rospy.Rate(10)
pwm_msg = PWM()
pwm_msg.PWM1 = 110
pwm_msg.PWM2 = 100
while not rospy.is_shutdown():
rospy.loginfo(pwm_msg)
pub_pwm.publish(pwm_msg)
rate.sleep()
def __init__(self):
'''
[[[[[STEP -1]]]]]
set design variables of robot
'''
self.ticks_per_rev = 360
self.b = 0.23 # in meters
self.r = 0.0352 # in meters
self.control_freq = 10 # in Hz
self.dt = 1/self.control_freq
# set PI control variables
# NOTE: index 1 is Left and index 2 is Right
self.error = 0
self.int_error = 0
self.alpha1 = 15
self.alpha2 = 13.2
self.beta1 = 0.4
self.beta2 = 0.5
self.desired_w1 = 0
self.estimated_w1 = 0
self.desired_w2 = 0
self.estimated_w2 = 0
# formulate PWM message
self.pwm_msg = PWM()
# self.twist_desired = Twist()
# self.twist_desired.linear.x = 0.5
# self.twist_desired.linear.y = 0.5
# self.twist_desired.angular = 0.5
'''
def arbitrary(left, right):
# publisher
pub = rospy.Publisher('/kobuki/pwm', PWM)
# node
rospy.init_node('arbitrary')
# signal rate
rate = rospy.Rate(10)
# signal
sig = PWM()
# left and right wheel signals
sig.PWM1, sig.PWM2 = int(left), int(right)
while not rospy.is_shutdown():
pub.publish(sig)
rate.sleep()
#!/usr/bin/env python # license removed for brevity import rospy from ras_lab1_msgs.msg import PWM from geometry_msgs.msg import Twist from ras_lab1_msgs.msg import Encoders import math # robot model ticks = 360 b = 0.115 r = 0.0352 freq = 10 pwm = PWM() # desired velocity vw1d = 0 vw2d = 0 # error sum error_sum1 = 0.0 error_sum2 = 0.0 # controller Kp = 1.0 Ki = 0.005 Kd = 0.1 # last erros le1 = 0
def start():
# Node initalization
rospy.init_node("FeedbackController")
# Subscriber definitions
rospy.Subscriber("/kobuki/encoders", Encoders, enc_feedback)
rospy.Subscriber("/motor_controller/twist", Twist, twist_input)
# PWM Publisher definition
pwm_pub = rospy.Publisher("/kobuki/pwm", PWM, queue_size=1)
# Setting Publishing frequency
r = rospy.Rate(10)
# PWM message type object declaration
pwm_var = PWM()
# Global assignment
global error_integral_1
global error_integral_2
# Condition for the while loop
while not rospy.is_shutdown():
if ENC_VAR == None:
continue
# Assigning the values of current wheel angular velocities to a variable
omega_1 = ENC_VAR.delta_encoder1
omega_2 = ENC_VAR.delta_encoder2
# Calculating corresponding wheel linear velocities frim the formula V = W.r
velocity_1 = omega_1 * R
velocity_2 = omega_2 * R
#print "Velocity1:",velocity_1, "Velocity2:",velocity_2
# Calculating the current linear velocity of the Kobuki robot
current_velocity = 0.5 * (velocity_1 + velocity_2)
# Calculating the current angular velocity of the Kobuki robot
current_angular_velocity = (velocity_2 - velocity_1) / (2 * b)
# Defining the desired linear and angular velocity
desired_velocity = TW_VAR.linear.x
desired_angular_velocity = TW_VAR.angular.z
# Calculating the desired wheel velocities
desired_velocity_1 = desired_velocity - (b * desired_angular_velocity)
desired_velocity_2 = desired_velocity + (b * desired_angular_velocity)
# Calculating the linear velocity errors of both the wheels
error_velocity_1 = desired_velocity_1 - velocity_1
error_velocity_2 = desired_velocity_2 - velocity_2
# Reimann Sum approximation of the integral terms
error_integral_1 = error_integral_1 + (error_velocity_1 * dt)
error_integral_2 = error_integral_2 + (error_velocity_2 * dt)
# Control Outputs
U_wheel_1 = Kp1 * error_velocity_1 + Ki1 * error_integral_1
U_wheel_2 = Kp2 * error_velocity_2 + Ki2 * error_integral_2
# Assigning the pwm values to the pwm_var
pwm_var.PWM1 = U_wheel_1
pwm_var.PWM2 = U_wheel_2
# Publishing the message
pwm_pub.publish(pwm_var)
r.sleep()