-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathsrv.py
57 lines (41 loc) · 1.45 KB
/
srv.py
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
import machine
import math
import utime
class Servo:
def __init__(self, pin_id, min_us=544.0, max_us=2400.0, min_deg=0.0, max_deg=180.0, freq=50):
self.pwm = machine.PWM(machine.Pin(pin_id))
self.pwm.freq(freq)
self.current_us = 0.0
self._slope = (min_us - max_us) / (math.radians(min_deg) - math.radians(max_deg))
self._offset = min_us
# Калибровка
self.calibrate(min_us, max_us, min_deg, max_deg)
def calibrate(self, min_us, max_us, min_deg, max_deg):
self._slope = (min_us - max_us) / (math.radians(min_deg) - math.radians(max_deg))
self._offset = min_us
def write(self, deg):
self.write_rad(math.radians(deg))
def read(self):
return math.degrees(self.read_rad())
def write_rad(self, rad):
self.write_us(rad * self._slope + self._offset)
def read_rad(self):
return (self.current_us - self._offset) / self._slope
def write_us(self, us):
self.current_us = us
self.pwm.duty_ns(int(self.current_us * 1000.0))
def read_us(self):
return self.current_us
def off(self):
self.write_us((self._offset + self._slope * math.radians(90)) / 2)
def set_pwm_freq(self, freq):
self.pwm.freq(freq)
'''
pin_id = 0
pwm = Servo(pin_id,min_us=544.0,max_us=2400.0,min_deg=0.0,max_deg=180.0,freq=50)
pwm.write(180)
time.sleep_ms(2000)
pwm.write(90)
time.sleep_ms(1500)
print("INIT DONE11111")
'''