def control(data):
global old_error
global d
close_error = data.dl * (1 - lb) - data.dr * (lb)
far_error = data.dfl - data.dfr
#yref = 1 #fixed distance from right wall instead of relative
if far_error < 0:
far_error = 0
#e = close_error
e = 0.7 * close_error + 0.3 * far_error
#e = e * scaling_factor
d = da * (e - old_error)
u = kp * e + d + servo_offset
# Saturate control signal
if u > 100:
u = 100
if u < -100:
u = -100
old_error = e
#print e,
#print ",",
#print u,
#rospy.loginfo("ERROR: %f\tDERIVATE: %f\tANGLE:%f", steering_error, d, angle)
msg = drive_param()
msg.velocity = vel_input
msg.angle = u
pub.publish(msg)
def control(data):
global old_error
global d
width = data.wdl + data.wdr
yref = lb * width
#yref = 1 #fixed distance from right wall instead of relative
y = data.wdr
e = beta*yref - y
#e = e * scaling_factor
d = da * (e - old_error)
u = kp*e + d + servo_offset
# Saturate control signal
if u > 100:
u=100
if u < -100:
u=-100
old_error = e
#print e,
#print ",",
#print u,
#print ",",
#rospy.loginfo("ERROR: %f\tDERIVATE: %f\tANGLE:%f", steering_error, d, angle)
msg = drive_param()
msg.velocity = vel_input
msg.angle = u
pub.publish(msg)
def mode_switch(data):
global current_mode
current_mode = data.mode
# Turn off drive and center steering when mode is changed
msg = drive_param()
msg.velocity = 0
msg.angle = 0
pub.publish(msg)
def eStop(received_eStop):
global current_mode
if received_eStop:
# Set mode to off
current_mode = 0
# Turn off drive and center steering when eStop is received
msg = drive_param()
msg.velocity = 0
msg.angle = 0
pub.publish(msg)
def control(data):
scaling_factor = 50
# Steering PID
steering_error = data.pid_error * scaling_factor
angle = kp * steering_error - servo_offset
# Saturate control signal
angle = 100 if angle > 100 else angle
angle = -100 if angle < -100 else angle
msg = drive_param()
msg.velocity = vel_input
msg.angle = angle
pub.publish(msg)
def control(data):
global old_steering_error
global D
scaling_factor = 50
# Steering PID
steering_error = data.pid_error * scaling_factor
old_steering_error=data.pid_old_error
D = ad*D - bd*(steering_error-old_steering_error)
angle = kp*steering_error - servo_offset + D
# Saturate control signal
angle = 100 if angle > 100 else angle
angle = -100 if angle < -100 else angle
msg = drive_param()
msg.velocity = data.pid_vel
msg.angle = angle
pub.publish(msg)
old_steering_error = steering_error
pub = rospy.Publisher('f1tenth_controller_xbox360/output',
drive_param,
queue_size=10)
# Set rate which the loop is run with
rate = rospy.Rate(100)
# Initialize xbox 360 controller
joy = xbox.Joystick()
steering = 0
while not rospy.is_shutdown():
if joy.connected():
# Calculate velocity and steering angle from joystick and trigger values
forward = (joy.rightTrigger() - joy.leftTrigger()) * 20
left = joy.leftX() * -100
steering = steering * 0 + 1 * left
# Create message
msg = drive_param()
msg.velocity = forward
msg.angle = steering
# Publish message
pub.publish(msg)
rate.sleep()