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)