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)