117 lines
2.7 KiB
Python
Executable file
117 lines
2.7 KiB
Python
Executable file
# SPDX-FileCopyrightText: 2021 jedgarpark for Adafruit Industries
|
|
# SPDX-License-Identifier: MIT
|
|
|
|
# Pico servo demo
|
|
# Hardware setup:
|
|
# Servo on GP0 with external 5V power supply
|
|
# Button on GP3 and ground
|
|
|
|
import time
|
|
import board
|
|
from digitalio import DigitalInOut, Direction, Pull
|
|
import pwmio
|
|
from adafruit_motor import servo
|
|
|
|
print("Servo test")
|
|
mode = -1 # track state of button mode
|
|
|
|
def setupLED():
|
|
led = DigitalInOut(board.LED)
|
|
led.direction = Direction.OUTPUT
|
|
led.value = True
|
|
return led
|
|
|
|
def setupButton():
|
|
# Mode button setup
|
|
button = DigitalInOut(board.GP3)
|
|
button.direction = Direction.INPUT
|
|
button.pull = Pull.UP
|
|
return button
|
|
|
|
|
|
|
|
def blink(led,count):
|
|
for _ in range(count):
|
|
led.value = False
|
|
time.sleep(0.1)
|
|
led.value = True
|
|
time.sleep(0.1)
|
|
|
|
def setupServo():
|
|
# Servo setup
|
|
pwm_servo = pwmio.PWMOut(board.GP0, duty_cycle=2 ** 15, frequency=50)
|
|
servo1 = servo.Servo(pwm_servo, min_pulse=500, max_pulse=2200) # tune pulse for specific servo
|
|
|
|
return servo1
|
|
|
|
def setServoAngle(servo1,angle):
|
|
# Servo setup
|
|
print("servo set angle =",angle)
|
|
servo1.angle = angle
|
|
|
|
return servo1
|
|
|
|
# Servo test
|
|
def servo_direct_test1():
|
|
print("Servo Direct Test 1")
|
|
setServoAngle(servo1,90)
|
|
time.sleep(2)
|
|
setServoAngle(servo1,0)
|
|
time.sleep(2)
|
|
setServoAngle(servo1,90)
|
|
time.sleep(2)
|
|
setServoAngle(servo1,180)
|
|
time.sleep(2)
|
|
|
|
|
|
# Servo test
|
|
def servo_direct_test():
|
|
print("Servo Direct Test")
|
|
print("servo test: 90")
|
|
servo1.angle = 90
|
|
time.sleep(2)
|
|
print("servo test: 0")
|
|
servo1.angle = 0
|
|
time.sleep(2)
|
|
print("servo test: 90")
|
|
servo1.angle = 90
|
|
time.sleep(2)
|
|
print("servo test: 180")
|
|
servo1.angle = 180
|
|
time.sleep(2)
|
|
|
|
|
|
# Servo smooth test
|
|
def servo_smooth_test():
|
|
print("servo smooth test: 180 - 0, -1º steps")
|
|
for angle in range(180, 0, -1): # 180 - 0 degrees, -1º at a time.
|
|
servo1.angle = angle
|
|
time.sleep(0.01)
|
|
time.sleep(1)
|
|
print("servo smooth test: 0 - 180, 1º steps")
|
|
for angle in range(0, 180, 1): # 0 - 180 degrees, 1º at a time.
|
|
servo1.angle = angle
|
|
time.sleep(0.01)
|
|
time.sleep(1)
|
|
|
|
|
|
def run_test(testnum):
|
|
if testnum is 0:
|
|
print("servo direct test")
|
|
servo_direct_test1()
|
|
elif testnum is 1:
|
|
print("servo smooth test")
|
|
servo_smooth_test()
|
|
|
|
led = setupLED()
|
|
button = setupButton()
|
|
servo1 = setupServo()
|
|
print("Type of servo = ",type(servo))
|
|
|
|
while True:
|
|
if not button.value:
|
|
blink(led,2)
|
|
mode = (mode + 1) % 2
|
|
print("switch to mode %d" % (mode))
|
|
time.sleep(0.8) # big debounce
|
|
run_test(mode)
|