def standstill_img(speed, cs_thresh): #保持静止
uart.startcs()
## time.sleep(0.01)
speed_p = -80 #encoder and speed 250
speed_d = 0
speed_i = 0 #4
max_i_erro = 10
speed_zero = 0
set_speed = speed
last_value = 0
speed_i_erro = 0
speed = 0
direct = 0
times = 0
while True:
## times+=1
cs, speedl, speedr = uart.askcs()
## print speedl
if speedr == -1:
speedl = 0
speedr = 0
## print (speedl,speedr)
c_speederro = speedr - set_speed
speed_i_erro += c_speederro * 0.01
speed_i_erro = motor.limiter(speed_i_erro, -max_i_erro, max_i_erro)
l_speederro = last_value - set_speed
out_speed = speed_p * c_speederro + speed_d * l_speederro + speed_i * speed_i_erro
last_value = speedr
speed = (out_speed + speed_zero)
ret, c_img = cap.read()
c_red = img.red_bgr(c_img)
c_red = img.change_binary(c_red)
c_center, red_num, dis_xy, dis_x = img.get_center_new(c_red)
direct = st3_k_img_direct * (c_center[0] - ((WIDTH / 2)))
direct = motor.limiter(direct, -st3_direct_limmiter,
st3_direct_limmiter)
## print 'direct:',direct
#time.sleep(0.01)
## if (abs(speedl - set_speed) >= 10):
speed = (out_speed + speed_zero)
## print("speedcontrol\n",SPEED)
## print('out_speed:',out_speed)
motor.set_car_run(1, speed, direct)
## else:
## motor.set_car_run(1,set_speed,0)
## else:
## times += 1
## if (times > 20):
if (cs != 0) and (cs < cs_thresh): ##改过
print 'cs1:', cs, c_center[1]
motor.set_car_run(1, 0, 0)
## print times
## motor.set_car_run(1,45,0)
break
def stop_now(status_now):
done_flag = 0
if(status_now == 'speed'):
#直接给向后的速度，让电机急停
out_speed = 80
out_direct = 0
motor.set_car_run(1,out_speed,out_direct)
time.sleep(1)
## while True:
## set_speed = 0
## speed_p = 40 #encoder and speed
## speed_d =0
## speed_i = 2
## max_i_erro = 10
## speed_zero = 0
## l_speederro = 0
## speed_i_erro = 0
##
## c_speederro = (2*encoder.array[0]-1) * encoder.array[1] - set_speed
## speed_i_erro += c_speederro*0.01
## speed_i_erro = motor.limiter(speed_i_erro,-max_i_erro,max_i_erro)
## #由pid设置输出速度
## out_speed = speed_p*c_speederro+speed_d*l_speederro+speed_i*speed_i_erro
## l_speederro = c_speederro
## print ('c_speederro:',c_speederro)
## #当速度为0时，跳出
## if(c_speederro == 0):
## done_flag = 1
## break
## motor.set_car_run(1,out_speed,out_direct)
elif(status_now == 'direct'):
#读方向
l_direct = encoder.array[0]
out_direct = (2*l_direct-1) * 130 #赋值为相反方向
out_speed = 0
motor.set_car_run(1,out_speed,out_direct)
time.sleep(0.5)
while True:
set_direct = 0
direct_p = 40 #encoder and speed
direct_d =0
direct_i = 2
max_i_erro = 10
direct_zero = 0
l_directerro = 0
direct_i_erro = 0
c_directerro = (2*encoder.array[0]-1) * encoder.array[1] - set_direct
direct_i_erro += c_directerro*0.01
direct_i_erro = motor.limiter(direct_i_erro,-max_i_erro,max_i_erro)
out_direct = direct_p*c_directerro + direct_d*l_directerro + direct_i*direct_i_erro
l_directerro = c_directerro
print ('c_directerro:' , c_directerro)
if(c_directerro == 0):
done_flag = 1
break
motor.set_car_run(1,out_speed,out_direct)
return done_flag
def standstill(): #保持静止
speed_p = 65 #encoder and speed
speed_d = 0
speed_i = 2
max_i_erro = 10
speed_zero = 0
set_speed = 0
last_value = 0
speed_i_erro = 0
global SPEED
global DIRECTION
global still_flag
still_flag = 1
SPEED = 0
DIRECTION = 0
while still_flag == 1:
cs, speedl, speedr = uart.asksignal()
c_speederro = speedl - set_speed
speed_i_erro += c_speederro * 0.01
speed_i_erro = motor.limiter(speed_i_erro, -max_i_erro, max_i_erro)
l_speederro = last_value - set_speed
out_speed = speed_p * c_speederro + speed_d * l_speederro + speed_i * speed_i_erro
last_value = speedl
#time.sleep(0.01)
if (speedl or speedr) != 0:
SPEED = -(out_speed + speed_zero)
#print("speedcontrol\n",SPEED)
#print('out_speed%d:' % out_speed)
motor.set_car_run(1, SPEED, DIRECTION)
elif (speedl or speedr) == 0:
motor.set_car_run(0, 0, 0)
#print(encoder.array[1],encoder.array[0])
if still_flag == 0:
set_car_run(1, 0, 0)
def pid(data_in, set_value, p, i, d, i_erro=0):
max_i_erro = 10
last_value = 0
c_erro = data_in - set_value
i_erro += c_erro * 0.01
i_erro = motor.limiter(i_erro, -max_i_erro, max_i_erro)
l_erro = last_value - set_value
data_out = p * c_erro + d * l_erro + i * i_erro
last_value = data_in
return data_out
def pid(data_in,data_out,p,i,d):
max_i_erro = 10
zero = 70
set_speed = 0
last_value = 0
while True:
c_speederro = encoder.array[2] - set_speed
speed_i_erro += c_speederro*0.01
speed_i_erro = motor.limiter(speed_i_erro,-max_i_erro,max_i_erro)
l_speederro = last_value - set_speed
out_speed = p*c_speederro+d*l_speederro
last_value = encoder.array[2]
def cs_stop(): #保持静止
## uart.startcs()
## time.sleep(0.01)
speed_p = -70 #encoder and speed
speed_d = 0
speed_i = 0
max_i_erro = 10
speed_zero = 0
set_speed = 0
last_value = 0
speed_i_erro = 0
SPEED = 0
DIRECTION = 0
times = 0
## while True:
## cs,speedl,speedr = uart.askcs()
#### print cs
## if ((cs != 0) and (cs <cs_thresh)):
## print cs
## break
## print 'stop now'
while True:
cs, speedl, speedr = uart.askcs()
## print speedl
if speedr == -1:
speedl = 0
speedr = 0
## print (speedl,speedr)
c_speederro = speedr - set_speed
speed_i_erro += c_speederro * 0.01
speed_i_erro = motor.limiter(speed_i_erro, -max_i_erro, max_i_erro)
l_speederro = last_value - set_speed
out_speed = speed_p * c_speederro + speed_d * l_speederro + speed_i * speed_i_erro
last_value = speedr
#time.sleep(0.01)
if (speedr != 0):
SPEED = (out_speed + speed_zero)
#print("speedcontrol\n",SPEED)
## print('out_speed:',out_speed)
motor.set_car_run(1, SPEED, DIRECTION)
else:
motor.set_car_run(1, 0, 0)
if ((cs != 0) and (cs < 1350)):
#### times += 1
#### if (times > 10000):
print 'cs2', cs
uart.endcs()
break
def standstill(speed, cs_thresh): #保持静止
uart.startcs()
## time.sleep(0.01)
speed_p = -80 #encoder and speed 250
speed_d = 0
speed_i = 0 #4
max_i_erro = 10
speed_zero = 0
set_speed = speed
last_value = 0
speed_i_erro = 0
SPEED = 0
DIRECTION = 0
times = 0
while True:
## times+=1
cs, speedl, speedr = uart.askcs()
## print speedl
if speedr == -1:
speedl = 0
speedr = 0
## print (speedl,speedr)
c_speederro = speedr - set_speed
speed_i_erro += c_speederro * 0.01
speed_i_erro = motor.limiter(speed_i_erro, -max_i_erro, max_i_erro)
l_speederro = last_value - set_speed
out_speed = speed_p * c_speederro + speed_d * l_speederro + speed_i * speed_i_erro
last_value = speedr
#time.sleep(0.01)
## if (abs(speedl - set_speed) >= 10):
SPEED = (out_speed + speed_zero)
## print("speedcontrol\n",SPEED)
## print('out_speed:',out_speed)
motor.set_car_run(1, SPEED, 0)
## else:
## motor.set_car_run(1,set_speed,0)
## else:
## times += 1
## if (times > 20):
if (cs != 0) and (cs < cs_thresh):
print 'cs1:', cs
motor.set_car_run(1, 0, 0)
## print times
## motor.set_car_run(1,45,0)
break
def keep_speed(set_speed): #保持速度，以编码器读数为准
k1 = 0.2375 #sum and pwm
speed_p = 0
speed_d = 0
speed_i = 0
max_i_erro = 10
last_value = 0
speed_i_erro = 0
while True:
real_speed = encoder.array[2] * k1
c_speederro = real_speed - set_speed
speed_i_erro = c_speederro * 0.01
speed_i_erro = motor.limiter(speed_i_erro, -max_i_erro, max_i_erro)
l_speederro = last_value - set_speed
out_speed = speed_p * c_speederro + speed_d * l_speederro + speed_i * speed_i_erro
last_value = encoder.array[2]
if real_speed >= set_speed:
SPEED = set_speed - out_speed + speed_zero
elif real_speed < set_speed:
SPEED = set_speed + out_speed + speed_zero
motor.set_car_run(1, SPEED, DIRECTION)
def wheelstop():
direct_p = -90
direct_d = 0
direct_i = 0
max_i_erro = 10
speed_zero = 0
set_speed = 0
speed = 0
last_value = 0
direct_i_erro = 0
## still_flag = 1
SPEED = 0
DIRECTION = 0
num = 0
while True:
rf, speedl, speedr = uart.asksignal()
if speedr == -1:
speedl = 0
speedr = 0
## print (speedl,speedr)
c_directerro = (speedl - speedr) - set_speed
direct_i_erro += c_directerro * 0.01
direct_i_erro = motor.limiter(direct_i_erro, -max_i_erro, max_i_erro)
l_directerro = last_value
out_direct = direct_p * c_directerro + direct_d * l_directerro + direct_i * direct_i_erro
last_value = speedl - speedr
#time.sleep(0.01)
if ((abs(speedl - speedr)) > 10):
direct = out_direct
## SPEED = (out_speed+speed_zero)
#print("speedcontrol\n",SPEED)
## print('out_speed:',out_speed)
motor.set_car_run(1, speed, out_direct)
else:
motor.set_car_run(1, 0, 0)
num += 1
if (num >= 3):
break
#yes
## if rfsgl == 1111: ####
## st5_flag = 1
## status = 5
## continue
#no
## else:
#加速是否完成
## rfi,fb_speed,speedr = uart.asksignal()
if fb_speed >= st2_speed:
speed = st2_speed
else:
speed = st2_speed + st2_accelerate
#图像做闭环的pid方向控制（需要改）
direct = k_img_direct * (c_center[0] - ((WIDTH / 2)))
direct = motor.limiter(direct, -st2_direct_limmiter,
st2_direct_limmiter)
## print 'direct:',direct
motor.set_car_run(1, speed, direct)
elif status == 3:
speed = st3_speed
## motor.set_car_run(1,speed,0)
standstill_img(speed, cs_thresh)
## rf,speedl,speedr = uart.asksignal()
## motor.set_car_run(1,speed,0)
## print 'slow'
#小速度滑行，等待制动距离
speedcontrol.cs_stop()
## print 'start cs'
## while True:
## cs,speedl,speedr = uart.askcs()
motor.set_car_run(1, -80, 0)
time.sleep(1)
status = 3
motor.set_car_run(0, 0, 0)
print("staues2 is done!")
break
else:
out_direct = speedcontrol.pid(c_center[0] + 1, WIDTH / 2, 2.2,
0, 0)
## if (last_direct > 0) and (out_direct < 0):
## out_direct = -out_direct
## if (out_direct != last_direct) and (out_direct < -100):
## last_direct = out_direct
## print("last_direct",last_direct)
out_direct = motor.limiter(out_direct, -50, 50)
motor.set_car_run(1, statues2_speed, out_direct)
print(c_center, out_direct, red_num)
status = 2
## if(status == 3):
## if(real_speed == 0):
## status = 4
## else:
## status = 3
##
## print('current status:%d' % status)
##
## if(status == 3):
## motor.stop()
## real_speed = encoder.array[1]
## print 'speed:',encoder.array[:]
## speedcontrol.standstill()
if(status == 2):
if(status2_flag >= 4):
speed = status2_speed
else:
speed = status2_speed+10
out_direct = speedcontrol.pid(c_center[0]+1,WIDTH/2,1,0,0)
## if (last_direct > 0) and (out_direct < 0):
## out_direct = -out_direct
## if (out_direct != last_direct) and (out_direct < -100):
## last_direct = out_direct
## print("last_direct",last_direct)
out_direct = motor.limiter(out_direct,-30,30)
motor.set_car_run(1,speed,out_direct)
status2_flag += 1
## print 'status2',c_center,out_direct,red_num
if(status == 3):
## helm.helm_out(helm_h)
## time.sleep(2)
helm.helm_in(helm_h)
time.sleep(0.1)
speedcontrol.retreat(-80)
status3_flag = 1
##
## print('current status:%d' % status)
##
