|
7 | 7 | >>> servo.angle = 30 # Turn motor to 30 degrees position. |
8 | 8 | """ |
9 | 9 |
|
| 10 | +import time |
| 11 | +from typing import List |
10 | 12 | from typing import Union |
11 | 13 |
|
12 | 14 | from pslab.instrument.waveform_generator import PWMGenerator |
@@ -70,3 +72,40 @@ def _get_duty_cycle(self, angle): |
70 | 72 | angle *= self._max_angle_pulse - self._min_angle_pulse # Scale |
71 | 73 | angle += self._min_angle_pulse # Offset |
72 | 74 | return angle / (self._frequency**-1 * MICROSECONDS) |
| 75 | + |
| 76 | + |
| 77 | +class RoboticArm: |
| 78 | + """Robotic arm controller for up to 4 servos.""" |
| 79 | + |
| 80 | + MAX_SERVOS = 4 |
| 81 | + |
| 82 | + def __init__(self, servos: List[Servo]) -> None: |
| 83 | + if len(servos) > RoboticArm.MAX_SERVOS: |
| 84 | + raise ValueError( |
| 85 | + f"At most {RoboticArm.MAX_SERVOS} servos can be used, got {len(servos)}" |
| 86 | + ) |
| 87 | + self.servos = servos |
| 88 | + |
| 89 | + def run_schedule(self, timeline: List[List[int]], time_step: float = 1.0) -> None: |
| 90 | + """Run a time-based schedule to move servos. |
| 91 | +
|
| 92 | + Parameters |
| 93 | + ---------- |
| 94 | + timeline : List[List[int]] |
| 95 | + A list of timesteps,where each sublist represents one timestep, |
| 96 | + with angles corresponding to each servo. |
| 97 | +
|
| 98 | + time_step : float, optional |
| 99 | + Delay in seconds between each timestep. Default is 1.0. |
| 100 | + """ |
| 101 | + if len(timeline[0]) != len(self.servos): |
| 102 | + raise ValueError("Each timestep must specify an angle for every servo") |
| 103 | + |
| 104 | + tl_len = len(timeline[0]) |
| 105 | + if not all(len(tl) == tl_len for tl in timeline): |
| 106 | + raise ValueError("All timeline entries must have the same length") |
| 107 | + |
| 108 | + for tl in timeline: |
| 109 | + for i, s in enumerate(self.servos): |
| 110 | + s.angle = tl[i] |
| 111 | + time.sleep(time_step) |
0 commit comments