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)