def SineSweep(opt, rate, t_i):
# timing maneuvers
oneSec = rate
dt = 15*oneSec
start_turning = 1*oneSec
t_0 = opt.t_0*oneSec
t_st = t_0 + start_turning
t_f = t_0 + dt +start_turning
T = 2*oneSec
# rest
if t_i < t_0:
servoCMD = opt.Z_turn
motorCMD = opt.neutral
# move forward
elif (t_i >= t_0) and (t_i < t_st):
servoCMD = opt.Z_turn
motorCMD = opt.speed
# move in sine wave motion
elif (t_i >= t_st) and (t_i < t_f):
servoCMD = angle_2_servo(15*sin(2*pi*(t_i-t_st)/float(T)))
motorCMD = opt.speed
# set straight and stop
else:
servoCMD = opt.Z_turn
motorCMD = opt.neutral
if not opt.stopped:
motorCMD = opt.brake
return (motorCMD, servoCMD)
def SineSweep(opt, rate, t_i):
# timing maneuvers
oneSec = rate
dt = 15*oneSec
start_turning = 1*oneSec
t_0 = opt.t_0*oneSec
t_st = t_0 + start_turning
t_f = t_0 + dt +start_turning
T = 2*oneSec
# rest
if t_i < t_0:
servoCMD = opt.Z_turn
motorCMD = opt.neutral
# move forward
elif (t_i >= t_0) and (t_i < t_st):
servoCMD = opt.Z_turn
motorCMD = opt.speed
# move in sine wave motion
elif (t_i >= t_st) and (t_i < t_f):
servoCMD = angle_2_servo(15*sin(2*pi*(t_i-t_st)/float(T)))
motorCMD = opt.speed
# set straight and stop
else:
servoCMD = opt.Z_turn
motorCMD = opt.neutral
if not opt.stopped:
motorCMD = opt.brake
return (motorCMD, servoCMD)
def main_auto():
#initialize ROS node
rospy.init_node('auto_mode', anonymous=True)
nh = rospy.Publisher('serv', SERV, queue_size=10)
rospy.Subscriber('imu', TimeData, imu_callback)
#set node rate
rateHz = 50
dt = 1.0 / rateHz
rate = rospy.Rate(rateHz)
#use simple pid control to keep steering straight
p = rospy.get_param("lateral/p")
i = rospy.get_param("lateral/i")
d = rospy.get_param("lateral/d")
pid = PID(P=p, I=i, D=d)
#main_loop
while not rospy.is_shutdown():
#get steering wheel command
u = pid.update(err, dt)
servoCMD = angle_2_servo(u)
#send command signal
# 위에서 PID 계산한 servoCMD 값을 serv_cmd로 publish한다
serv_cmd = SERV(servoCMD)
nh.publish(serv_cmd)
#wait
rate.sleep()
def main_auto():
# initialize ROS node
rospy.init_node('auto_mode', anonymous=True)
nh = rospy.Publisher('serv', SERV, queue_size = 10)
rospy.Subscriber('imu', TimeData, imu_callback)
# set node rate
rateHz = 50
dt = 1.0 / rateHz
rate = rospy.Rate(rateHz)
# use simple pid control to keep steering straight
p = rospy.get_param("lateral/p")
i = rospy.get_param("lateral/i")
d = rospy.get_param("lateral/d")
pid = PID(P=p, I=i, D=d)
# main loop
while not rospy.is_shutdown():
# get steering wheel command
u = pid.update(err, dt)
servoCMD = angle_2_servo(u)
# send command signal
serv_cmd = SERV(servoCMD)
nh.publish(serv_cmd)
# wait
rate.sleep()
def main_auto():
# initialize ROS node
rospy.init_node('auto_mode', anonymous=True)
#rospy.Subscriber('encoder', Vector3, enc_callback)
nh = rospy.Publisher('ecu', ECU, queue_size = 10)
# set node rate
rateHz = 50
dt = 1.0 / rateHz
rate = rospy.Rate(rateHz)
t_i = 0
t0 = time.time()
# read the control sequence from data file u_vec 2*60, dt=0.2 and u_vec[1,:] for acceleration and 2 for steering
u_vec=zeros(2,30)
# data 1
u_vec[0,:]=[1.0 1.0 1.0 1.0 0.0 -0.0 -0.0 -0.0 -0.0 -0.0 0.0 -0.0 -0.177 -1.0 -1.0 -1.0 -1.0 -0.823 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0]
u_vec[1,:]=[-0.239 -0.214 -0.155 -0.095 -0.062 -0.058 -0.064 0.524 -0.015 -0.01 -0.031 -0.015 -0.029
-0.025 0.155 0.524 0.524 0.524 0.017 0.018 0.018 0.019 0.019 0.019 0.02 0.02 0.02 0.02 0.02 0.02]
# data 2
u_vec[0,:]=[1.0 1.0 1.0 1.0 0.0 -0.0 -0.0 -0.0 -0.0 -0.0 0.0 -0.0 -0.027 -1.0 -1.0 -1.0 -1.0 -0.973 0.0 -0.0 -0.0 -0.0 -0.0 0.0 0.0 0.0 0.0 -0.0 0.0 -0.0]
u_vec[1,:]=[-0.477 -0.397 -0.235 -0.13 -0.01 -0.284 0.443 0.524 -0.159 0.054 0.339 -0.177 0.039
-0.171 0.454 0.524 0.524 0.524 0.032 0.034 0.034 0.035 0.036 0.036 0.037 0.037 0.037 0.037 0.037 0.037]
# main loop
while not rospy.is_shutdown():
# get steering wheel command
for i in range(60)
a_cmd=u_vec[0,i]
steer_ang=u_vec[1,i]
servoCMD=angle_2_servo(steer_ang)
# from lab8
a0 = rospy.get_param("ol_mpc/a0")
b0 = rospy.get_param("ol_mpc/b0")
c0 = rospy.get_param("ol_mpc/c0")
#lmd = rospy.get_param("ol_mpc/lmd")
# a0=0.1721
# b0=0.1174
# c0=0.6263
# lmd=1 # controller coeeficient, for bandwidth
#motorCMD=(-lmd*(V_cmd-v_x_enc)+c0*V_cmd^2+b0)/a0+90
motorCMD=(a_cmd+c0*v_x_enc**2+b0)/a0+90
motorCMD=96
ecu_cmd = ECU(motorCMD, servoCMD)
for j in range(10) # hold the command for 10*0.02=0.2, since the open loop sequence is generated with delta_t=0.2
nh.publish(ecu_cmd) # there is delay between publishing and the vehicle actually execute the cmd
t_i+=1
rate.sleep()
def main_auto():
# initialize ROS node
rospy.init_node('auto_mode', anonymous=True)
nh = rospy.Publisher('ecu', ECU, queue_size=10)
rospy.Subscriber('imu', TimeData, imu_callback)
# set node rate
rateHz = 50
dt = 1.0 / rateHz
rate = rospy.Rate(rateHz)
t_i = 0
# specify test and test options
experiment_opt = {
0: CircularTest,
1: Straight,
2: SineSweep,
3: DoubleLaneChange,
4: CoastDown
}
experiment_sel = rospy.get_param("controller/experiment_sel")
v_x_pwm = rospy.get_param("controller/v_x_pwm")
t_0 = rospy.get_param("controller/t_0")
t_exp = rospy.get_param("controller/t_exp")
str_ang = rospy.get_param("controller/steering_angle")
test_mode = experiment_opt.get(experiment_sel)
opt = TestSettings(SPD=v_x_pwm, turn=str_ang, dt=t_exp)
opt.t_0 = t_0
# use simple pid control to keep steering straight
p = rospy.get_param("controller/p")
i = rospy.get_param("controller/i")
d = rospy.get_param("controller/d")
pid = PID(P=p, I=i, D=d)
# main loop
while not rospy.is_shutdown():
# get steering wheel command
u = pid.update(err, dt)
servoCMD = angle_2_servo(u)
# get command signal
(motorCMD, _) = test_mode(opt, rateHz, t_i)
# send command signal
ecu_cmd = ECU(motorCMD, servoCMD)
nh.publish(ecu_cmd)
# wait
t_i += 1
rate.sleep()
def steering_command(rateHz, Kp, Kd, Ki):
global pixel_offset
global pixel_offset_prev
global ei
# Current Error, Derivative of Error, Intergral of Error
e = pixel_offset
ed = e - pixel_offset_prev
ei = ei + (e / float(rateHz))
# Calulate Turn Angle
turn = (e * Kp) + (ed * Kd) + (ei * Ki)
# Saturate Turn Value
if (turn < -30.0):
turn = -30.0
elif (turn > 30.0):
turn = 30.0
return angle_2_servo(-turn), -turn
def steering_command(rateHz, Kp, Kd, Ki):
global pixel_offset, lane_angle_offset_cur
global e_prev
global ei, d_f
e = pixel_offset
ed = e - e_prev
ei = ei + (e / float(rateHz))
# Calulate Turn Angle
turn = (e * Kp) + (ed * Kd) + (ei * Ki)
# Saturate Turn Value
if (turn < -30.0):
turn = -30.0
elif (turn > 30.0):
turn = 30.0
d_f = turn
e_prev = e
return angle_2_servo(-turn), -turn
def arduino_interface():
global motor_pwm, servo_pwm, ecu_pub
# launch node, subscribe to motorPWM and servoPWM, publish ecu
rospy.init_node('arduino_interface')
rospy.Subscriber('motor_pwm', ECU, motor_callback, queue_size = 10)
rospy.Subscriber('servo_pwm', ECU, servo_callback, queue_size = 10)
ecu_pub = rospy.Publisher('ecu', ECU, queue_size = 10)
# initialize motor and servo at neutral
motor_pwm = 90
steering_angle = 0
steering_offset = 2
servo_pwm = angle_2_servo(steering_angle - steering_offset)
# Set motor to neutral on shutdown
rospy.on_shutdown(neutralize)
# process callbacks and keep alive
rospy.spin()
def __init__(self, SPD = 95, turn = 0, dt = 10):
# PWN signal values for motor
self.speed = SPD
self.neutral = 90
self.stopped = False
self.brake = 50
self.dt_man = dt # length of time the motor is on
self.t_turn = 4 # length of time before first turn
self.t_0 = 2 # initial time at rest before experiment
self.turn_deg = turn
# check valid speed
if SPD < 90 or SPD > 130:
self.speed = 95
# check valid turns
# left is positive, right is negative
if turn < -30 or turn > 30:
turn = 0
# PWN signal values for servo
self.turn = angle_2_servo(turn) # right turn
self.Z_turn = 90 # zero (no) turn
def __init__(self, SPD = 95, turn = 0, dt = 10):
# PWN signal values for motor
self.speed = SPD
self.neutral = 90
self.stopped = False
self.brake = 50
self.dt_man = dt # length of time the motor is on
self.t_turn = 4 # length of time before first turn
self.t_0 = 2 # initial time at rest before experiment
self.turn_deg = turn
# check valid speed
if SPD < 90 or SPD > 130:
self.speed = 95
# check valid turns
# left is positive, right is negative
if turn < -30 or turn > 30:
turn = 0
# PWN signal values for servo
self.turn = angle_2_servo(turn) # right turn
self.Z_turn = 90 # zero (no) turn
def steering_command(rateHz):
global pixel_offset
global L, l
global steeringGain
steeringAngle = 0
# Calulate Turn Angle
if (pixel_offset > 0):
e = pixel_offset * steeringGain
steeringAngle = L / sqrt(sin(atan2(e, l)) / l / e)
elif (pixel_offset < 0):
e = -pixel_offset * steeringGain
steeringAngle = -L / sqrt(sin(atan2(e, l)) / l / e)
turn = degrees(steeringAngle)
# Saturate Turn Value
if (turn < -30.0):
turn = -30.0
elif (turn > 30.0):
turn = 30.0
return angle_2_servo(-turn), -turn
def shutdown_func():
global nh
ecu_cmd = ECU(0, angle_2_servo(0))
nh.publish(ecu_cmd)