def servo_init(pin = PIN_SER):
servo = Servo(PIN_SER)
return servo
# servo test - example
from time import sleep
from util.pinout import set_pinout
from util.servo import Servo
# todo: PWM double setup error
pinout = set_pinout()
# s1 = Servo(pinout.PWM1_PIN)
# s2 = Servo(pinout.PWM2_PIN)
s3 = Servo(pinout.PWM3_PIN)
angles = [0, 20, 50, 70, 90]
while True:
for a in angles:
# s1.set_degree(a)
# s2.set_degree(a)
s3.set_degree(a)
sleep(1)
# invkin2(point2d, angleMode=DEGREES, l1=l1,l2=l2)
print("servos init")
from config import Config
setup = Config("draw2servos")
setup.print_all()
try:
L1 = int(setup.get("L1"))
L2 = int(setup.get("L2"))
except:
print("config.Err")
print("L1, L2: " + str(L1) + ", " + str(L2))
print("servos init")
from util.servo import Servo
s1 = Servo(17)
s2 = Servo(16)
s1.set_degree(90)
s2.set_degree(0)
def move_servo2(p1, p2, delay=5):
steps = move_2d_line(p1, p2)
for step in steps:
alfa, beta = invkin2(step, L1, L2)
print(step, alfa, beta)
s1.set_degree(alfa)
s2.set_degree(beta)
# servo laser pointer - example
from time import sleep_ms
from util.transform import *
from util.servo import Servo
# todo: PWM double setup error
s1 = Servo(17)
s2 = Servo(16)
dist = 100 # distance and size of board
delay = 5
def move_servo2(p1, p2, delay=delay):
steps = move_2d_line(p1, p2)
for step in steps:
alfa = cosangle(step[0], dist, dist)[0]
beta = cosangle(step[1], dist, dist)[0]
print(step, alfa, beta)
s1.set_degree(alfa)
s2.set_degree(beta)
sleep_ms(delay)
def run_test():
p1 = 0, 0 # strart point
p2 = 50, 50 # stop point
move_servo2(p1, p2)
from time import sleep
from util.analog import Analog
from util.servo import map
from util.servo import Servo
an2 = Analog(35)
s1 = Servo(16)
s1.set_degree(0)
# 0-150
amin = 0
amax = 150
while True:
data = an2.get_adc_aver(2)
sleep(0.05)
angle = map(data, 0, 4000, amin, amax)
print(data, angle)
s1.set_degree(angle)