from time import sleep
from utils.pinout import set_pinout
from components.servo import Servo
pinout = set_pinout()
s1 = Servo(pinout.PWM1_PIN)
# s2 = Servo(pinout.PWM2_PIN)
# s3 = Servo(pinout.PWM3_PIN)
import blesync_server
import blesync_uart.server
import utils.ble.bluefruit as bf
angle = 45
s1.set_degree(angle)
angle_min = 10
angle_max = 150
@blesync_uart.server.UARTService.on_message
def on_message(service, conn_handle, message):
global angle
print(str(message), angle)
if message == bf.LEFT:
angle = angle - 5
if angle < angle_min:
angle = angle_min
s1.set_degree(angle)
# servo test - example
from time import sleep
from utils.pinout import set_pinout
from components.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)
from time import sleep
from utils.pinout import set_pinout
from components.servo import Servo
pinout = set_pinout()
s1 = Servo(pinout.PWM1_PIN)
s2 = Servo(pinout.PWM2_PIN)
# s3 = Servo(pinout.PWM3_PIN)
import blesync_server
import blesync_uart.server
import utils.ble.bluefruit as bf
angle1 = 45
angle2 = 45
s1.set_degree(angle1)
s2.set_degree(angle2)
angle_min = 10
angle_max = 150
@blesync_uart.server.UARTService.on_message
def on_message(service, conn_handle, message):
global angle1, angle2
print(str(message), angle1, angle2)
if message == bf.LEFT:
angle1 = angle1 - 5
if angle1 < angle_min:
angle1 = angle_min
s1.set_degree(angle1)
LR_COMPENSATION = +10 # %, -10 slows L motor comared to R
ULTRASONIC_SAMPLING = 60 # ms, how often detect obstacle
moto_L1 = Pin(pinout.MOTOR_1A, Pin.OUT)
moto_L2 = Pin(pinout.MOTOR_2A, Pin.OUT)
moto_L = PWM(Pin(pinout.MOTOR_12EN, Pin.OUT), freq=500, duty = 0)
moto_R3 = Pin(pinout.MOTOR_3A, Pin.OUT)
moto_R4 = Pin(pinout.MOTOR_4A, Pin.OUT)
moto_R = PWM(Pin(pinout.MOTOR_34EN, Pin.OUT), freq=500, duty = 0)
echo = HCSR04(trigger_pin=pinout.PWM2_PIN, echo_pin=pinout.PWM1_PIN)
servo = Servo(pinout.PWM3_PIN)
servo.set_degree(50)
def read_distance(echo):
d = echo.distance_cm()
if d < 0:
sleep_ms(50)
d = echo.distance_cm()
print('distance', d)
return d
def compensate_speed_left(speed):
return speed + int(speed/100 * LR_COMPENSATION/2) * ENABLE_COMPENSATION
def compensate_speed_right(speed):
return speed - int(speed/100 * LR_COMPENSATION/2) * ENABLE_COMPENSATION
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 components.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)
sleep_ms(delay)
