def car_drive_continue_test():
print("car drive continue")
for speed in list(range(0, 101)):
print("drive speed: %d" % speed)
rocky.drive(speed, speed)
time.sleep(0.1)
for speed in list(range(0, 101)):
print("drive speed: %d" % -speed)
rocky.drive(-speed, -speed)
time.sleep(0.1)
rocky.stop()
def run(dir, duration):
if dir == 0:
rocky.drive(200, 200)
elif dir == 1:
rocky.drive(0, 200)
elif dir == 2:
rocky.drive(-200, -200)
elif dir == 3:
rocky.drive(200, 0)
elif dir == 4:
rocky.drive(0, 0)
time.sleep(duration)
def car_drive_test():
print("right left + power")
for speed in list(range(0, 101, 20)):
print("right speed: %d" % speed)
rocky.drive(speed, speed)
time.sleep(1)
print("right left - power")
for speed in list(range(0, 101, 20)):
print("right speed: %d" % speed)
rocky.drive(-speed, -speed)
time.sleep(1)
rocky.stop()
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)
codey.reset_time()
codey.ir_send("A")
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***")
cmdRaw = codey.get_variable('cmd')
if len(cmdRaw) == 0:
rocky.stop()
codey.set_variable('cmd', "0_0_0")
continue
cmdData = cmdRaw.split('_')
typeA = int(cmdData[0])
# for debug
# typeA = -1
if typeA == 0:
if len(cmdData) == 3:
lS = int(cmdData[1])
rS = int(cmdData[2])
rocky.drive(lS, rS)
else:
rocky.stop()
elif typeA == 1:
runCmd(['00003c1e0e0400000000040e1e3c0000', (0,3), 0])
elif typeA == 2:
runCmd(['1c224a524c201c00001c224a524c201c', (3,3), 0])
elif typeA == 3:
runCmd(['00363e1c3e3600000000363e1c3e3600', (1,0.5), (2,1), (3,0.5), 0])
elif typeA == 4:
runCmd(['000c18181c0c000000000c1c18180c00', (0,0.3), (2,0.3), (2,0.3), (0,0.3), 0])
elif typeA == 5:
runCmd(['00000c1e3e3c000000003c3e1e0c0000', (1,0.4), (2,0.3), (3,0.6), (2,0.3),(1,0.4), 0])
elif typeA == 6:
runCmd(['003c4242423c000000003c4242423c00', (2,0.3), 0])
elif typeA == -1:
def run(l, r, duration):
rocky.drive(l, r)
time.sleep(duration)
def run(l, r, duration):
print("l: %d r: %d, d: %f" % (l, r, duration))
rocky.drive(l, r)
# rocky.forward( l )
time.sleep(duration)
