HSRobotControl/RobotCircuitPythonTrinketM0.py at master · programotter/HSRobotControl · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
import time
import board
import pulseio
import digitalio
from adafruit_motor import servo
import supervisor

led = digitalio.DigitalInOut(board.D13)
led.direction = digitalio.Direction.OUTPUT

switchDown = digitalio.DigitalInOut(board.D3)
switchDown.direction = digitalio.Direction.INPUT
switchDown.pull = digitalio.Pull.UP

switchUp = digitalio.DigitalInOut(board.D4)
switchUp.direction = digitalio.Direction.INPUT
switchUp.pull = digitalio.Pull.UP

pwm1 = pulseio.PWMOut(board.A2, duty_cycle=2 ** 15, frequency=50)
pwm2 = pulseio.PWMOut(board.A1, duty_cycle=2 ** 15, frequency=50)

left_servo = servo.Servo(pwm1, min_pulse=750, max_pulse=2250)
right_servo = servo.Servo(pwm2, min_pulse=750, max_pulse=2250)

left_servo.actuation_range = 180.0
right_servo.actuation_range = 180.0
angle_up = 25.0
angle_down = 25.0
interval = 0.01
updownchange = 0.05
updowncount = 500.0
updownsleep = 0.05
horizontal = 90.0
left_servo_offset = 0.0
right_servo_offset = -1.0
left_servo.angle = horizontal + left_servo_offset
right_servo.angle = horizontal + right_servo_offset

while True:
    if supervisor.runtime.serial_bytes_available:
            value = input()

            if value == "":
                continue
            else:
                print(value)
                values = value.split(" ")
                if len(values) == 3:
                    if value[0] >= value[1] and value[0] <= value[2]:
                        minangle = horizontal - angle_down
                        maxangle = horizontal + angle_up
                        differenceangle = maxangle - minangle
                        mindistance = float(values[1])
                        maxdistance = float(values[2])
                        differencedistance = maxdistance - mindistance
                        distance = float(values[0])
                        ratiodistance = (distance - mindistance) / differencedistance
                        angle = minangle + differenceangle * ratiodistance
                        print(angle)
                        if angle >= minangle and angle <= maxangle:
                            left_servo.angle = angle + left_servo_offset
                            right_servo.angle = 180 - angle + right_servo_offset
                            if not switchUp.value:
                                updowncount += updowncount * updownchange
                                time.sleep(updownsleep)
                            if not switchDown.value:
                                updowncount -= updowncount * updownchange
                                time.sleep(updownsleep)