rpi-gpio-robot/pan_tilt_with_speed_control.py at main · jaredclaypoole/rpi-gpio-robot · 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
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
import time
from adafruit_servokit import ServoKit

kit = ServoKit(channels=16)

PAN = 0
TILT = 1

PAN_CENTER = 60
TILT_CENTER = 30

def clamp(a: float, lo: float = 0, hi: float = 180) -> float:
    return max(lo, min(hi, a))

def move_servo_deg_per_sec(channel: int, start: float, target: float, deg_per_sec: float = 90.0, hz: float = 50.0):
    """
    Move a servo from start -> target at approximately deg_per_sec.
    hz controls update rate; 50 Hz is plenty (20ms steps).
    """
    start = clamp(start)
    target = clamp(target)
    if deg_per_sec <= 0:
        kit.servo[channel].angle = target
        return

    dt = 1.0 / hz
    step = deg_per_sec * dt  # degrees per update

    pos = start
    kit.servo[channel].angle = pos

    direction = 1.0 if target > start else -1.0
    while (target - pos) * direction > 0:
        pos += direction * step
        # don't overshoot
        if (target - pos) * direction < 0:
            pos = target
        kit.servo[channel].angle = pos
        time.sleep(dt)

def move_pan_tilt(pan_start, tilt_start, pan_target, tilt_target, deg_per_sec=90.0, hz=50.0):
    # Move both in lockstep by updating both every tick
    pan_start = clamp(pan_start); pan_target = clamp(pan_target)
    tilt_start = clamp(tilt_start); tilt_target = clamp(tilt_target)

    dt = 1.0 / hz
    step = deg_per_sec * dt

    pan = pan_start
    tilt = tilt_start
    kit.servo[PAN].angle = pan
    kit.servo[TILT].angle = tilt

    pan_dir = 1.0 if pan_target > pan_start else -1.0
    tilt_dir = 1.0 if tilt_target > tilt_start else -1.0

    done_pan = False
    done_tilt = False

    while not (done_pan and done_tilt):
        if not done_pan:
            pan += pan_dir * step
            if (pan_target - pan) * pan_dir <= 0:
                pan = pan_target
                done_pan = True

        if not done_tilt:
            tilt += tilt_dir * step
            if (tilt_target - tilt) * tilt_dir <= 0:
                tilt = tilt_target
                done_tilt = True

        kit.servo[PAN].angle = pan
        kit.servo[TILT].angle = tilt
        time.sleep(dt)

if __name__ == "__main__":
    # Example usage:
    # Start centered

    kit.servo[PAN].angle = PAN_CENTER
    # kit.servo[TILT].angle = 90
    time.sleep(0.5)

    move_servo_deg_per_sec(PAN, start=PAN_CENTER, target=PAN_CENTER+60, deg_per_sec=30)

    time.sleep(0.5)
    move_servo_deg_per_sec(PAN, start=PAN_CENTER+60, target=PAN_CENTER-60, deg_per_sec=30)

    time.sleep(0.5)
    move_servo_deg_per_sec(PAN, start=PAN_CENTER-60, target=PAN_CENTER, deg_per_sec=30)

    # move_servo_deg_per_sec(TILT, start=90, target=60, deg_per_sec=120)
    move_servo_deg_per_sec(TILT, start=TILT_CENTER, target=0, deg_per_sec=30)
    move_servo_deg_per_sec(TILT, start=0, target=TILT_CENTER, deg_per_sec=30)

    # # Move both together at 60 deg/sec
    # move_pan_tilt(140, 60, 90, 90, deg_per_sec=60)