def trace_line_calb():
while (True):
err = rocky.grey("left") - rocky.grey("right")
# print( "err%d"%err )
rocky.left(100)
rocky.right(100)
time.sleep(0.7)
def trace_line():
global icr_pid
record_cnt = 10
gray_1 = 0
gray_2 = 0
last_gray_1 = 0
last_gray_2 = 0
gray_diff_err_squ = []
gray_diff_err = []
for i in range(record_cnt):
gray_diff_err_squ.insert(i + 1, 0)
gray_diff_err.insert(i + 1, 0)
record_idx = 0
max_icr = 80
trace_speed = 80
icr_pid_init(icr_pid, 1.3, 0.1, 0.0, 100)
icr = 0
on_white_cnt = 0
while (True):
err = rocky.grey("left") - rocky.grey("right")
gray_1 = rocky.grey("left")
gray_2 = rocky.grey("right")
err = gray_1 - gray_2
gray_diff_err_squ[record_idx] = abs(gray_1 - last_gray_1) * abs(
gray_1 - last_gray_1) - abs(gray_2 - last_gray_2) * abs(
gray_2 - last_gray_2)
gray_diff_err[record_idx] = err
record_idx += 1
record_idx = record_idx % record_cnt
last_gray_1 = gray_1
last_gray_2 = gray_2
# print( "err %d"%err )
out = icr_pid_loop(icr_pid, err)
if (icr >= max_icr):
if (out < 0):
icr += out
elif (icr <= -max_icr):
if (out > 0):
icr += out
else:
icr += out
left_speed = trace_speed + icr
right_speed = trace_speed - icr
# print( "L: %d, R: %d"%(left_speed, right_speed) )
rocky.drive(left_speed, right_speed)
time.sleep(0.01)
def rocky_read():
print("rocky color is:", rocky.get_color()) # not a demand
print("red value is:", rocky.red())
print("green value is:", rocky.green())
print("blue value is:", rocky.blue())
print("rocky grey is:", rocky.grey())
print("rocky obstacle is:", rocky.is_obstacle_ahead())
print("rocky light is:", rocky.light_strength())
print("rocky light reflect is:", rocky.reflection_strength())
print("rocky ir reflect is:", rocky.ir_reflection_strength())
print("rocky left motor current is ", rocky.motor_current('left'))
rocky.forward(100)
time.sleep(1)
print("rocky left motor current is ", rocky.motor_current('left'))
rocky.stop()
print("")
def rocky_read():
print("rocky color is:", rocky.color())
print("rocky grey is:", rocky.grey('left'))
print("rocky obstacle is:", rocky.is_obstacle_ahead())
print("rocky light is:", rocky.light_intensity('left'))
codey.ir_receive()
print("***codey APIS all succeed***")
# about rocky
rocky.color("#334455")
rocky.forward(50, 1)
rocky.backward(50, 1)
rocky.turn_left(50, 1)
rocky.turn_right(50, )
rocky.forward(50)
rocky.backward(50)
rocky.turn_left(50)
rocky.turn_right(50)
rocky.drive(50, 50)
rocky.turn_left_angle(15)
rocky.turn_right_angle(15)
rocky.stop()
rocky.is_obstacle_ahead()
rocky.is_color('red')
rocky.red()
rocky.green()
rocky.blue()
rocky.reflection_strength()
rocky.light_strength()
rocky.ir_reflection_strength()
rocky.grey()
print("***rocky APIS all succeed***")
codey.on_button('A', call_back)