SunFounder PiDog - Page 67

SunFounder PiDog Kit, Release 1.0
(continued from previous page)
from pidog import Pidog
from pidog.walk import Walk
import readchar
import threading
import os
my_dog = Pidog()
sleep(0.5)
usage = '''
Pidog Balance Ctrl + C to Exit
...
'''
stand_coords = [[[0, 80], [0, 80], [0, 80], [0, 80]]]
forward_coords = Walk(fb=Walk.FORWARD, lr=Walk.STRAIGHT).get_coords()
backward_coords = Walk(fb=Walk.BACKWARD, lr=Walk.STRAIGHT).get_coords()
turn_left_coords = Walk(fb=Walk.FORWARD, lr=Walk.LEFT).get_coords()
turn_right_coords = Walk(fb=Walk.FORWARD, lr=Walk.RIGHT).get_coords()
current_coords = stand_coords
current_pose = {'x': 0, 'y': 0, 'z': 80}
current_rpy = {'roll': 0, 'pitch': 0, 'yaw': 0}
thread_start = True
def move_thread():
while thread_start:
for coord in current_coords:
# print(coord)
my_dog.set_rpy(
**
current_rpy, pid=True)
my_dog.set_pose(
**
current_pose)
my_dog.set_legs(coord)
angles = my_dog.pose2legs_angle()
my_dog.legs.servo_move(angles, speed=98)
t = threading.Thread(target=move_thread)
def main():
global current_coords, current_pose, current_rpy, thread_start
my_dog.do_action('stand', speed=80)
my_dog.wait_legs_done()
t.start()
while True:
os.system('cls' if os.name == 'nt' else 'clear')
print(usage)
key = readchar.readkey()
if key == readchar.key.CTRL_C or key in readchar.key.ESCAPE_SEQUENCES:
thread_start = False
break
elif key == 'w':
current_coords = forward_coords
elif key == 's':
(continues on next page)
2.2. Funny Project 63