146 lines
3.4 KiB
Python
146 lines
3.4 KiB
Python
# SPDX-FileCopyrightText: 2018 Dave Astels for Adafruit Industries
|
|
#
|
|
# SPDX-License-Identifier: MIT
|
|
|
|
"""
|
|
Continuous servo based walking/waddling/etc robot.
|
|
|
|
Adafruit invests time and resources providing this open source code.
|
|
Please support Adafruit and open source hardware by purchasing
|
|
products from Adafruit!
|
|
|
|
Written by Dave Astels for Adafruit Industries
|
|
Copyright (c) 2018 Adafruit Industries
|
|
Licensed under the MIT license.
|
|
|
|
All text above must be included in any redistribution.
|
|
"""
|
|
|
|
import time
|
|
from adafruit_crickit import crickit
|
|
|
|
tail = crickit.dc_motor_1
|
|
|
|
# Each servo corresponds to one of the legs
|
|
front_right = crickit.continuous_servo_1
|
|
front_left = crickit.continuous_servo_2
|
|
rear_right = crickit.continuous_servo_3
|
|
rear_left = crickit.continuous_servo_4
|
|
|
|
# Useful groups of legs
|
|
all_legs = [front_right, front_left, rear_right, rear_left]
|
|
front_legs = [front_right, front_left]
|
|
rear_legs = [rear_right, rear_left]
|
|
right_legs = [front_right, rear_right]
|
|
left_legs = [front_left, rear_left]
|
|
|
|
# The sign (+1/-1) for forward motion for each servo
|
|
direction_values = {front_right: +1,
|
|
front_left: -1,
|
|
rear_right: +1,
|
|
rear_left: -1}
|
|
|
|
# Tweak the pwn ranges for each servo so that throttle of 0 stops the motor
|
|
pwm_ranges = {front_right: (500, 2400),
|
|
front_left: (500, 2400),
|
|
rear_right: (500, 2400),
|
|
rear_left: (500, 2400)}
|
|
|
|
|
|
def init():
|
|
for leg in all_legs:
|
|
limits = pwm_ranges[leg]
|
|
leg.set_pulse_width_range(min_pulse=limits[0], max_pulse=limits[1])
|
|
leg.throttle = 0
|
|
|
|
|
|
def wag(speed):
|
|
tail.throttle = speed
|
|
time.sleep(0.1)
|
|
tail.throttle = 0.0
|
|
time.sleep(0.25)
|
|
|
|
|
|
def wag_for(seconds):
|
|
target_time = time.monotonic() + seconds
|
|
wag_throttle = 1.0
|
|
while time.monotonic() < target_time:
|
|
wag(wag_throttle)
|
|
wag_throttle *= -1
|
|
|
|
|
|
def forward(servo_or_servos, speed):
|
|
if isinstance(servo_or_servos, list):
|
|
for servo in servo_or_servos:
|
|
servo.throttle = speed * direction_values[servo]
|
|
else:
|
|
servo_or_servos.throttle = speed * direction_values[servo_or_servos]
|
|
|
|
|
|
def reverse(servo_or_servos, speed):
|
|
if isinstance(servo_or_servos, list):
|
|
for servo in servo_or_servos:
|
|
servo.throttle = speed * -1 * direction_values[servo]
|
|
else:
|
|
servo_or_servos.throttle = speed * -1 * direction_values[servo_or_servos]
|
|
|
|
|
|
def stop(servo_or_servos):
|
|
if isinstance(servo_or_servos, list):
|
|
for servo in servo_or_servos:
|
|
servo.throttle = 0
|
|
else:
|
|
servo_or_servos.throttle = 0
|
|
|
|
|
|
def rotate_clockwise(speed):
|
|
forward(left_legs, speed)
|
|
reverse(right_legs, speed)
|
|
|
|
|
|
def rotate_counterclockwise(speed):
|
|
forward(right_legs, speed)
|
|
reverse(left_legs, speed)
|
|
|
|
|
|
def crawl_forward(speed):
|
|
forward(all_legs, speed)
|
|
|
|
|
|
def crawl_backward(speed):
|
|
reverse(all_legs, speed)
|
|
|
|
|
|
def turtle():
|
|
stop([rear_right, rear_left])
|
|
stop(rear_left)
|
|
forward(front_right, 0.5)
|
|
forward(front_left, 0.5)
|
|
|
|
|
|
def snake_step():
|
|
stop(all_legs)
|
|
forward(right_legs, 0.5)
|
|
time.sleep(1.0)
|
|
stop(right_legs)
|
|
forward(left_legs, 0.5)
|
|
time.sleep(1.0)
|
|
stop(left_legs)
|
|
|
|
|
|
init()
|
|
|
|
def demo1():
|
|
crawl_forward(0.5)
|
|
wag_for(5.0)
|
|
rotate_clockwise(0.25)
|
|
wag_for(3.0)
|
|
crawl_backward(0.5)
|
|
wag_for(2.0)
|
|
rotate_counterclockwise(0.25)
|
|
wag_for(3.0)
|
|
crawl_forward(0.5)
|
|
wag_for(5.0)
|
|
stop(all_legs)
|
|
|
|
demo1()
|