class Lab3:
def __init__(self):
# publish to cmd
self.pub = rospy.Publisher("car_cmd_switch_node/cmd", Twist2DStamped, queue_size=10)
# tune pid values below. omega should have opposite sign from d -> use negative p,i
self.pid_1 = PID(p= -7.5, i= 0, d=0)
self.pid_2 = PID(p= -5.4, i= -0.1 , d=0)
self.my_msg = Twist2DStamped()
self.allow_follow = False
self.velocity = 0.2 # adjust velocity here
# receive the phi and d to update PID controller and movement
rospy.Subscriber("lane_filter_node/lane_pose", LanePose, self.follow_lane)
# receive command from joystick
rospy.Subscriber("fsm_node/mode", FSMState, self.callbackFSM)
# use the pose (d and phi) to calculate omega
def follow_lane(self, pose):
# if joystick key "a" is pressed, run lane follow
if (self.allow_follow):
rospy.logwarn("Paprawin B. Lab3: LANE FOLLOWING Code Running")
self.my_msg.v = self.velocity
# use first PID controller on d
omega_1 = self.pid_1.calc_control(pose.d, 0.0015)
# use second PID controller on phi
omega_2 = self.pid_2.calc_control(pose.phi, 0.0015)
# add up omega values
self.my_msg.omega = omega_1 + omega_2
# sends new controller message to system
self.pub.publish(self.my_msg)
# else it will stop running
else:
self.my_msg.v = 0
self.my_msg.omega = 0
self.pub.publish(self.my_msg)
# Determine which key was pressed by user
def callbackFSM(self, keypressed):
if (keypressed.state == "LANE_FOLLOWING"):
self.allow_follow = True # program will run once "a" is pressed
elif (keypressed.state == "NORMAL_JOYSTICK_CONTROL"):
self.allow_follow = False # disable lane following program when "s" is pressed
class Homework5:
def __init__(self):
# homework 5 publishes to the control input for vehicle_dynamics to read
self.pub = rospy.Publisher("/control_input", Float32, queue_size=10)
self.pid_obj = PID(p=0.03, i=0.01, d=0.317)
# set param controller_ready to true
rospy.set_param("controller_ready", "true")
# receives/responds-to messages from error topic published by vehicle_dynamics.py
rospy.Subscriber("/error", Float32, self.callback)
# call PID class' control calculation when receiving error message
def callback(self, error):
# calculates and updates the pid values using the received error
result = self.pid_obj.calc_control(error.data, 0.001)
# sends new controller message to system
self.pub.publish(result)
