def car_rotate(x_error,x_range):
pass
print('旋转')
if x_error<-10:
car.run(car_speed,-car_speed)
elif x_error>x_range:
car.run(-car_speed,car_speed)
def car_move(y_error,y_range):
pass
print('移动')
if y_error<0:
car.run(car_speed,car_speed)
elif y_error>y_range:
car.run(-car_speed,car_speed)
def main():
car.run()
car.exploration_plots()
car.iteration_plots()
forest.run()
forest.iteration_plots()
def car_rotate(x_error, car_speed):
pass
print('旋转')
if x_error < -5:
car.run(car_speed, -car_speed)
elif x_error > 5:
car.run(-car_speed, car_speed)
def car_back():
while(True):
while(True):
clock.tick()
img = sensor.snapshot()
blobs = img.find_blobs([ball_threshold])
if blobs:
max_blob = find_max(blobs)
x_error = max_blob[5]-80
y_error = max_blob[3]-ball_high
img.draw_rectangle(max_blob[0:4]) # rect
img.draw_cross(max_blob[5], max_blob[6]) # cx, cy
print(x_error,y_error)
if x_error<-x_range or x_error>x_range:
car_rotate(x_error,x_range)
else:
if 0<y_error<y_range:
break
else:
car_move(y_error,y_range)
else:
car.run(-car_turn_speed,car_turn_speed)
while(True):
pass
clock.tick()
img = sensor.snapshot()
blobs = img.find_blobs([home_threshold])
if blobs:
max_blob = find_max(blobs)
x_error = max_blob[5]-80
y_error = max_blob[3]-home_high
if x_error<-x_range_home or x_error>x_range_home:
car_rotate(x_error,x_range_home)
else:
if 0<y_error<y_range_home:
break
else:
car_move(y_error,y_range_home)
else:
car.run(car_turn_speed,-car_turn_speed)
def car_back():
global rand
rand = random.randint(0, 1)
car.run(-car_speed, -car_speed)
hand.hand_two_reset()
if rand == 0:
car.run(car_turn_speed, -car_turn_speed)
else:
car.run(-car_turn_speed, car_turn_speed)
hand.hand_down()
#turn_time=random.randint(100,400)
#time.sleep(turn_time)
car.run(0, 0)
theta_err = line.theta()
img.draw_line(line.line(), color = 127)
print(rho_err,line.magnitude(),rho_err)
if line.magnitude()>8:
#if -40<b_err<40 and -30<t_err<30:
rho_output = rho_pid.get_pid(rho_err,1)
theta_output = theta_pid.get_pid(theta_err,1)
output = rho_output+theta_output
car.run(50+output, 50-output)
else:
car.run(0,0)
else:
car.run(50,-50)
pass
#print(clock.fps())
import car
car = car.TowCar()
car.current_status()
while True:
car.run()
#!/usr/bin/env python3
import sys
import car
import race
import server
if __name__ == "__main__":
if str(sys.argv[1]) == "car":
sys.exit(car.run(sys.stdin.read().splitlines()))
elif str(sys.argv[1]) == "race":
sys.exit(race.run(sys.stdin.read().splitlines()))
elif str(sys.argv[1]) == "server":
sys.exit(server.run(sys.stdin.read().splitlines()))
else:
sys.stderr.write("You choose poorly.")
sys.exit(1)
def car_adjust():
car.run(-car_speed, -car_speed)
time.sleep(1000)
car.run(car_speed, -car_speed)
time.sleep(1000)
car.run(0, 0)
x_error = max_blob[5] - 80
y_error = max_blob[3] - ball_high
x_output = x_pid.get_pid(x_error, 1)
y_output = y_pid.get_pid(y_error, 1)
img.draw_rectangle(max_blob[0:4]) # rect
img.draw_cross(max_blob[5], max_blob[6]) # cx, cy
print('x_error,y_error,max_blob[3]', x_error, y_error, max_blob[3])
if 30 > max_blob[3] > 15:
x_range = 30
elif max_blob[3] >= 30:
x_range = 20
else:
x_range = 40
pass
if x_error < -x_range or x_error > x_range: #先调方向
car.run(-x_output, x_output)
else: #方向正确后if x_error<-x_range or x_error>x_range:#先调方向
if -y_range < y_error < y_range:
p = 2
car_stop(car_speed)
break
else:
if max_blob[3] > 20:
car_speed = 70
elif 25 > max_blob[3] >= 20:
car_speed = 60
elif 30 > max_blob[3] >= 25:
car_speed = 55
elif 40 > max_blob[3] >= 30:
car_speed = 48
elif max_blob[3] >= 40:
def find_max(blobs):
max_size=0
for blob in blobs:
if blob[2]*blob[3] > max_size:
max_blob=blob
max_size = blob[2]*blob[3]
return max_blob
while(True):
clock.tick()
img = sensor.snapshot()
blobs = img.find_blobs([green_threshold])
if blobs:
max_blob = find_max(blobs)
x_error = max_blob[5]-img.width()/2
h_error = max_blob[2]*max_blob[3]-size_threshold
print("x error: ", x_error)
'''
for b in blobs:
img.draw_rectangle(b[0:4])
img.draw_cross(b[5], b[6])
'''
img.draw_rectangle(max_blob[0:4])
img.draw_cross(max_blob[5], max_blob[6])
x_output=x_pid.get_pid(x_error,1)
h_output=h_pid.get_pid(h_error,1)
print("h_output",h_output)
car.run(-h_output-x_output,-h_output+x_output)
else:
#car.run(18,-18)
car.run(0,0)
if blob[2] * blob[3] > max_size:
max_blob = blob
max_size = blob[2] * blob[3]
return max_blob
while (True):
clock.tick() # Track elapsed milliseconds between snapshots().
img = sensor.snapshot() # Take a picture and return the image.
blobs = img.find_blobs([green_threshold])
if blobs:
max_blob = find_max(blobs)
x_error = max_blob[5] - img.width() / 2
h_error = max_blob[2] * max_blob[3] - size_threshold
print("x error: ", x_error)
'''
for b in blobs:
# Draw a rect around the blob.
img.draw_rectangle(b[0:4]) # rect
img.draw_cross(b[5], b[6]) # cx, cy
'''
img.draw_rectangle(max_blob[0:4]) # rect
img.draw_cross(max_blob[5], max_blob[6]) # cx, cy
x_output = x_pid.get_pid(x_error, 1)
h_output = h_pid.get_pid(h_error, 1)
print("h_output", h_output)
car.run(-h_output - x_output, -h_output + x_output)
else:
car.run(100, 100)
if blob[2] * blob[3] > max_size:
max_blob = blob
max_size = blob[2] * blob[3]
return max_blob
while (True):
clock.tick() # Track elapsed milliseconds between snapshots().
img = sensor.snapshot() # Take a picture and return the image.
blobs = img.find_apriltags()
if blobs:
max_blob = find_max(blobs)
x_error = max_blob[6] - img.width() / 2
h_error = max_blob[2] * max_blob[3] - size_threshold
print("x error: ", x_error)
'''
for b in blobs:
# Draw a rect around the blob.
img.draw_rectangle(b[0:4]) # rect
img.draw_cross(b[5], b[6]) # cx, cy
'''
img.draw_rectangle(max_blob[0:4]) # rect
img.draw_cross(max_blob[6], max_blob[7]) # cx, cy
x_output = x_pid.get_pid(x_error, 1)
h_output = h_pid.get_pid(h_error, 1)
print("h_output", h_output)
car.run(-h_output - x_output, -h_output + x_output)
else:
car.run(18, -18)
def car_stop(speed):
car.run(0, 0)
for i in range(speed):
car.run(-i, -i)
time.sleep(speed)
for i in range(speed):
car.run(i, i)
time.sleep(speed)
for i in range(speed - 30):
car.run(-i, -i)
time.sleep(speed)
for i in range(speed - 30):
car.run(i, i)
time.sleep(speed)
car.run(0, 0)
while (True):
clock.tick() # Track elapsed milliseconds between snapshots().
img = sensor.snapshot() # Take a picture and return the image.
blobs = img.find_apriltags()
if blobs:
max_blob = find_max(blobs)
x_error = max_blob[6] - img.width() / 2
h_error = max_blob[2] * max_blob[3] - size_threshold
print("x error: ", x_error)
'''
for b in blobs:
# Draw a rect around the blob.
img.draw_rectangle(b[0:4]) # rect
img.draw_cross(b[5], b[6]) # cx, cy
'''
img.draw_rectangle(max_blob[0:4]) # rect
img.draw_cross(max_blob[6], max_blob[7]) # cx, cy
x_output = x_pid.get_pid(x_error, 1)
h_output = h_pid.get_pid(h_error, 1)
print("h_output", h_output)
if -h_output - x_output < 0:
car.run(35, -h_output + x_output)
if -h_output + x_output < 0:
car.run(-h_output - x_output, 35)
#car.run(-h_output-x_output,-h_output+x_output)
else:
car.run(100, 100)
#img = sensor.snapshot()
line = img.get_regression([(-100, -100, 0, 0, 0, 0)], robust=True)
if (line):
rho_err = abs(line.rho()) - img.width() / 2
if line.theta() > 90:
theta_err = line.theta() - 180
else:
theta_err = line.theta()
img.draw_line(line.line(), color=127)
print(rho_err, line.magnitude(), rho_err, theta_err)
if line.magnitude() > 8:
if -15 <= theta_err <= 15:
rho_output = rho_pid.get_pid(rho_err, 1)
theta_output = theta_pid.get_pid(theta_err, 1)
output = rho_output + theta_output
car.run(1000 + (10 * output), 1000 - (10 * output))
old_L = 1000 + (10 * output)
old_R = 1000 - (10 * output)
elif -15 <= theta_err <= 15 and pin3.value():
rho_output = rho_pid.get_pid(rho_err, 1)
theta_output = theta_pid.get_pid(theta_err, 1)
output = rho_output + theta_output
car.run(750 + (10 * output), 750 - (10 * output))
old_L = 750 + (10 * output)
old_R = 750 - (10 * output)
elif 70 >= theta_err >= 15 or -15 >= theta_err >= -70:
rho_output = rho_pid.get_pid(rho_err, 1)
theta_output = theta_pid.get_pid(theta_err, 1)
output = rho_output + theta_output
car.run(700 + (10 * output), 750 - (10 * output))