def vehicle_control(data): global prev_error_steer global prev_error_vel global error_steer global error_vel global angle global velocity global velocity_number global cnt global count #variavel de teste de numero de iteracoes x = data.X #posicao X da linha detectada y = data.Y #posicao Y da linha detectada #print "x: ", x #posicao X da linha detectada error_steer = (x-400)/400 #posicao X da linha em relacao ao centro da imagem 800 pixels #print "Error: ",error_steer #sistema de controle de angulacao control_error_steer = kp_steer*error_steer + kd_steer*(prev_error_steer - error_steer)#+ ki*integral #print "Control Error Steer:", control_error_steer #angle = angle - control_error_steer#*np.pi/180 angle = 0 prev_error_steer = error_steer #prev_error_vel = error_vel #angle = 30/float(-320) * x + 30 #angle = -angle if angle > 30: angle = 30 if angle < -30: angle = -30 print "angle:",angle #sistema de controle de velocidade print "Count: ",count if (count <250): velocity = 10.0 count = count+1 elif(count>250 and count<10000): velocity =10.0 count = count+1 else: velocity =10.0 print "vel:",velocity print "-----------" #prepara msg para publicacao msg = drive_param() msg.velocity = velocity msg.angle = angle pub_2.publish(msg)
def vehicle_control(data): global prev_error_steer global prev_error_vel global error_steer global error_vel global angle global velocity global velocity_number global prev_theta global count #numero de iteracoes global time1 #tempo entre execucoes global steer_integral #acumulador do erro global steer_deriv global range_LF #leitura do sonar global range_LM #leitura do sonar global range_RM #leitura do sonar global range_RF #leitura do sonar var_print = 0 if var_print: print "-----------Vehicle CONTR0L ----------" print "count", count obstacle, desvio = trata_sonar() #calcula se ha obstaculo a frente #print 'Obstacle: ', obstacle #define o dt-------------------------------------------------------------------------------------------------------------------------------------- time_now = tp.time() #armazena o tempo atual if (count == 0): dt = 0.0 #inicializa a variavel else: dt = time_now - time1 #tempo entre iteracoes time1 = time_now #armazena o tempo atual para a proxima iteracao #armazenando os dados lidos do topico-------------------------------------------------------------------------------------------------------------------------------------- x = data.X2 #posicao X2 da linha detectada y = data.Y2 #posicao Y2 da linha detectada x1 = data.X1 slope = data.slope #definindo o erro de posicao do veiculo em relacao a linha error_steer = ( x - 400 ) / 400 #posicao X da linha em relacao ao centro da imagem 800 pixels if var_print: print "Error: ", error_steer #sistema de controle de velocidade-------------------------------------------------------------------------------------------------------------------------------------- #print "Count: ",count velocity = VEL_CRUZEIRO #atualiza velocidade do veiculo count = count + 1 #conta numero de iteracoes if var_print: print "vel:", velocity #Controle de tempo de resposta-------------------------------------------------------------------------------------------------------------------------------------- if abs(slope) <= MIN_SLOPE and x - x1 > 10: dist_corr = velocity * KMH_TO_MS * 0.5 else: dist_corr = velocity * KMH_TO_MS * 1 #distancia a ser percorrida em metros em 1 s #define o erro em termos de angulo-------------------------------------------------------------------------------------------------------------------------------------- if (velocity != ZERO): dist_ang = error_steer / dist_corr #angulo do erro em relacao a reta detectada else: dist_ang = ZERO #print "cat op sobre hip: ", error_steer/dist_corr if (abs(dist_ang) <= 1 and velocity != ZERO): theta_error = np.arcsin( error_steer / dist_corr) * RAD_TO_ANGLE #correcao normal elif (velocity != ZERO): theta_error = np.arcsin( (error_steer / abs(error_steer)) / (dist_corr / abs(dist_corr)) ) * RAD_TO_ANGLE #se a distancia inicial for pequena para o erro else: theta_error = ZERO #caso nao exista uma linha detectada if var_print: print "theta_error: ", theta_error #Calculando o PID-------------------------------------------------------------------------------------------------------------------------------------- if abs(steer_integral) >= MAX_ERRO_INTEGRADOR or abs( theta_error ) <= 0.1: #se o integrador comacar a crescer demais, zera steer_integral = ZERO else: steer_integral = steer_integral + theta_error * dt #incrementa o integrador if var_print: print "steer_integral: ", steer_integral steer_deriv = prev_theta - theta_error #calcula a diferenca do erro #print "previous error:", (prev_theta - theta_error) #sistema de controle de angulacao if abs(slope) <= MIN_SLOPE and x - x1 > 10: control_error_steer = -(kp_steer_c * theta_error + ki_steer_c * steer_integral + kd_steer_c * steer_deriv) if var_print: print 'CURVA: ', slope else: control_error_steer = -(kp_steer_l * theta_error + ki_steer_l * steer_integral + kd_steer_l * steer_deriv) if var_print: print 'RETA: ', slope if var_print: print 'X2 - X1', x - x1 #control_error_steer = calc_PID( theta_error, dt, slope, x, x1) #print 'Teste: ',control_error_steer prev_theta = theta_error #atualiza o valor do erro if (obstacle == LIVRE): # se nao houver obstaculo a frente angle = control_error_steer #*ANG_TO_RAD #atualiza o angulo de controle #print 'LIVRE' else: #atuacao em caso de obstaculo a frente angle = desvio prev_theta = 0 steer_integral = ZERO velocity = VEL_FRENAGEM #print 'OBSTACLE' if angle > MAX_STEER: #limita o angulo maximo angle = MAX_STEER if angle < -MAX_STEER: angle = -MAX_STEER #print "angle:",angle #-------------------------- if var_print: print "-----------END Vehicle CONTROL ----------" #prepara msg para publicacao------------------------------------------------------------------------------------------------------------------------------ msg = drive_param() msg.velocity = velocity msg.angle = angle pub_2.publish(msg)
def vehicle_control(data): global prev_error_steer global prev_error_vel global error_steer global error_vel global angle global velocity global velocity_number global prev_theta global cnt global count #numero de iteracoes global time1 #tempo entre execucoes global steer_integral #acumulador do erro print "-----------Vehicle CONTROL ----------" print "count", count time_now = tp.time() #armazena o tempo atual if (count == 0): dt = 0.0 #inicializa a variavel else: dt = time_now - time1 #tempo entre iteracoes time1 = time_now #armazena o tempo atual para a proxima iteracao print "dt: ", dt x = data.X #posicao X da linha detectada y = data.Y #posicao Y da linha detectada #print "x: ", x #posicao X da linha detectada #print "y: ", y #posicao Y da linha detectada error_steer = ( x - 400 ) / 400 #posicao X da linha em relacao ao centro da imagem 800 pixels #print "Error: ",error_steer #sistema de controle de velocidade #print "Count: ",count if (count < 1): velocity = VEL_CRUZEIRO #elif abs(prev_error_steer) > 1: # velocity = VEL_FRENAGEM elif (abs(error_steer) > 0.075) and y > 450: #elif (abs(error_steer) > 0.05): # velocity = VEL_FRENAGEM #elif x < 380 or x > 420: velocity = VEL_FRENAGEM print "CORRECAO" else: velocity = VEL_CRUZEIRO print "NORMAL" count = count + 1 print "vel:", velocity #-------------------------- dist_corr = velocity * KMH_TO_MS #dist_corr = y/500 #print "dist_corr: ", dist_corr dist_ang = error_steer / dist_corr #print "cat op sobre hip: ", error_steer/dist_corr if (abs(dist_ang) <= 1): theta_error = -np.arcsin( error_steer / dist_corr) * RAD_TO_ANGLE #correcao normal else: theta_error = -np.arcsin( (error_steer / abs(error_steer)) / (dist_corr / abs(dist_corr)) ) * RAD_TO_ANGLE #se a distancia inicial for pequena para o erro #print "theta_error: ", theta_error steer_integral = steer_integral + theta_error * dt #print "steer_integral: ", steer_integral #print "previous error:", (prev_theta - theta_error) #sistema de controle de angulacao control_error_steer_2 = kp_steer * theta_error + kd_steer * ( prev_theta - theta_error) + ki_steer * steer_integral control_error_steer_1 = kp_steer * theta_error + ki_steer * steer_integral control_error_steer_0 = kp_steer * theta_error control_error_steer = control_error_steer_2 prev_error_steer = control_error_steer prev_theta = theta_error #print "Control Error Steer:", control_error_steer #print "Control Error Steer_0:", control_error_steer_0 #print "Control Error Steer_1:", control_error_steer_1 #print "Control Error Steer_2:", control_error_steer_2 angle = control_error_steer #*ANG_TO_RAD #print "angle:", angle #print "angle:", angle * ANG_TO_RAD prev_error_steer = error_steer #prev_error_vel = error_vel #angle = 30/float(-320) * x + 30 #angle = -angle if angle > 30: angle = 30 if angle < -30: angle = -30 #print "angle:",angle #-------------------------- print "-----------END Vehicle CONTROL ----------" #prepara msg para publicacao msg = drive_param() msg.velocity = velocity msg.angle = angle pub_2.publish(msg)
def callback(data): global prev_error_steer global prev_error_vel global error_steer global error_vel global angle global velocity global velocity_number global cnt global count x = data.X #posicao X da linha detectada y = data.Y #posicao Y da linha detectada print "x: ", x #slope from image_processing #slope = data.Dist #if slope!=0: error_steer = ( x - 400 ) / 400 #posicao X da linha em relacao ao centro da imagem 800 pixels print "Error: ", error_steer """ if np.isnan(distance)==False: error_vel = (distance-1)/float(1) else: error_vel = -4.0 """ control_error_steer = kp_steer * error_steer + kd_steer * ( prev_error_steer - error_steer) #+ ki*integral print "Control Error Steer:", control_error_steer #angle = angle - control_error_steer#*np.pi/180 angle = 0 print "Count: ", count if (count < 1000): velocity = 0.5 count = count + 1 else: #count = count+1 velocity = 0.0 """ if (control_error_steer>19 or control_error_steer<-19): velocity = 0 else: velocity = 0 """ """ control_error_vel = kp_vel*error_vel #+ kd_vel*(prev_error_vel - error_vel) print "Control Error Vel:", control_error_vel velocity = velocity + (control_error_vel) nan_verify = velocity if np.isnan(velocity)==False: velocity_number = velocity if np.isnan(velocity)==True: if nan_verify == velocity: cnt = cnt+1 if cnt==5: velocity = 0 print "RESET" cnt=0 else: velocity = velocity_number """ prev_error_steer = error_steer #prev_error_vel = error_vel msg = drive_param() if velocity < 0: velocity = 0 #angle = 30/float(-320) * x + 30 #angle = -angle if angle > 30: angle = 30 if angle < -30: angle = -30 print "angle:", angle #if distance <= 1: # velocity = 0 #if distance > 1: # velocity = distance * 10 print "vel:", velocity print "-----------" #if velocity > 30: # velocity=30 msg.velocity = velocity msg.angle = angle pub.publish(msg)
def callback(data): global prev_error_steer global prev_error_vel global error_steer global error_vel global angle global velocity global velocity_number global cnt x = data.X y = data.Y #slope from image_processing #slope = data.Dist #if slope!=0: error_steer = (x - 400) / 400 """ if np.isnan(distance)==False: error_vel = (distance-1)/float(1) else: error_vel = -4.0 """ control_error_steer = kp_steer * error_steer + kd_steer * ( prev_error_steer - error_steer) #+ ki*integral print "Control Error Steer:", control_error_steer angle = angle - control_error_steer #*np.pi/180 velocity = 25 """ control_error_vel = kp_vel*error_vel #+ kd_vel*(prev_error_vel - error_vel) print "Control Error Vel:", control_error_vel velocity = velocity + (control_error_vel) nan_verify = velocity if np.isnan(velocity)==False: velocity_number = velocity if np.isnan(velocity)==True: if nan_verify == velocity: cnt = cnt+1 if cnt==5: velocity = 0 print "RESET" cnt=0 else: velocity = velocity_number """ prev_error_steer = error_steer #prev_error_vel = error_vel msg = drive_param() if velocity < 0: velocity = 0 #angle = 30/float(-320) * x + 30 #angle = -angle if angle > 30: angle = 30 if angle < -30: angle = -30 print "angle:", angle #if distance <= 1: # velocity = 0 #if distance > 1: # velocity = distance * 10 print "vel:", velocity #if velocity > 30: # velocity=30 msg.velocity = velocity msg.angle = angle pub.publish(msg)
def vehicle_control(data): global prev_error_steer global prev_error_vel global error_steer global error_vel global angle global velocity global velocity_number global prev_theta global cnt global count #numero de iteracoes global time1 #tempo entre execucoes global steer_integral #acumulador do erro var_print = 1 if var_print: print "-----------Vehicle CONTROL ----------" print "count", count time_now = tp.time() #armazena o tempo atual if (count == 0): dt = 0.0 #inicializa a variavel else: dt = time_now - time1 #tempo entre iteracoes time1 = time_now #armazena o tempo atual para a proxima iteracao #if var_print: # print "dt: ", dt x = data.X2 #posicao X2 da linha detectada y = data.Y2 #posicao Y2 da linha detectada x1 = data.X1 slope = data.slope #print "x: ", x #posicao X2 da linha detectada #print "y: ", y #posicao Y2 da linha detectada error_steer = ( x - 400 ) / 400 #posicao X da linha em relacao ao centro da imagem 800 pixels #print "Error: ",error_steer #sistema de controle de velocidade #print "Count: ",count if (count < 1): velocity = VEL_CRUZEIRO status = NORMAL elif (abs(error_steer) > 0.075) and y > 450: velocity = VEL_FRENAGEM status = CORRECAO if var_print: print "CORRECAO" else: velocity = VEL_CRUZEIRO status = NORMAL if var_print: print "NORMAL" count = count + 1 if var_print: print "vel:", velocity #-------------------------- if abs(slope) <= 1.0 and x - x1 > 10: dist_corr = velocity * KMH_TO_MS * 0.5 else: dist_corr = velocity * KMH_TO_MS * 1 #distancia a ser percorrida em metros em 1 s #print "dist_corr: ", dist_corr dist_ang = error_steer / dist_corr #angulo do erro #print "cat op sobre hip: ", error_steer/dist_corr if (abs(dist_ang) <= 1): theta_error = np.arcsin( error_steer / dist_corr) * RAD_TO_ANGLE #correcao normal else: theta_error = np.arcsin( (error_steer / abs(error_steer)) / (dist_corr / abs(dist_corr)) ) * RAD_TO_ANGLE #se a distancia inicial for pequena para o erro #print "theta_error: ", theta_error if steer_integral <= 50.0: steer_integral = steer_integral + theta_error * dt else: steer_integral = 0 #print "steer_integral: ", steer_integral steer_deriv = prev_theta - theta_error #print "previous error:", (prev_theta - theta_error) #sistema de controle de angulacao if abs(slope) <= 1.0 and x - x1 > 10: control_error_steer_2 = kp_steer_c * theta_error + ki_steer_c * steer_integral + kd_steer_c * steer_deriv control_error_steer_1 = kp_steer_c * theta_error + ki_steer_c * steer_integral control_error_steer_0 = kp_steer_c * theta_error print 'CURVA: ', slope cnt = cnt + 1 else: control_error_steer_2 = kp_steer_l * theta_error + ki_steer_l * steer_integral + kd_steer_l * steer_deriv control_error_steer_1 = kp_steer_l * theta_error + ki_steer_l * steer_integral control_error_steer_0 = kp_steer_l * theta_error print 'RETA: ', slope cnt = 0 print 'X2 - X1', x - x1 control_error_steer = -control_error_steer_2 prev_theta = theta_error #print "Control Error Steer:", control_error_steer #print "Control Error Steer_0:", control_error_steer_0 #print "Control Error Steer_1:", control_error_steer_1 #print "Control Error Steer_2:", control_error_steer_2 angle = control_error_steer #*ANG_TO_RAD #print "angle:", angle #print "angle:", angle * ANG_TO_RAD #prev_error_vel = error_vel #angle = 30/float(-320) * x + 30 #angle = -angle if angle > 30: angle = 30 if angle < -30: angle = -30 #print "angle:",angle #-------------------------- if var_print: print "-----------END Vehicle CONTROL ----------" #prepara msg para publicacao msg = drive_param() msg.velocity = velocity msg.angle = angle pub_2.publish(msg) msg_e = error_control() msg_e.error_steer = error_steer msg_e.control_error_steer = control_error_steer msg_e.steer_integral = steer_integral msg_e.steer_deriv = steer_deriv msg_e.status = status pub_3.publish(msg_e)