compAIR/compLIB
compAIR/compLIB
Archived
4
0
Fork 0
Code Issues Pull requests Projects Releases 1 Packages Wiki Activity Actions

Add servos

Browse source
This commit is contained in:
koka 2021-01-28 22:12:33 +00:00
parent acc91cdd74
commit 7ec6854ac6
2 changed files with 18 additions and 13 deletions
Download patch file Download diff file Expand all files Collapse all files

5
compLib/PCA9685.py
View file

@ -47,12 +47,11 @@ class PCA9685:
prescaleval /= 4096.0 # 12-bit
prescaleval /= float(freq)
prescaleval -= 1.0
prescale = math.floor(prescaleval + 0.5)
oldmode = self.read(self.__MODE1)
newmode = (oldmode & 0x7F) | 0x10 # sleep
self.write(self.__MODE1, newmode) # go to sleep
self.write(self.__PRESCALE, int(math.floor(prescale)))
self.write(self.__PRESCALE, int(round(prescaleval)))
self.write(self.__MODE1, oldmode)
time.sleep(0.005)
self.write(self.__MODE1, oldmode | 0x80)
@ -69,7 +68,7 @@ class PCA9685:
def setServoPulse(self, channel, pulse):
"Sets the Servo Pulse,The PWM frequency must be 50HZ"
pulse = pulse * 4096 / 20000 # PWM frequency is 50HZ,the period is 20000us
pulse = float(pulse) * (4096.0 / 20000.0) # PWM frequency is 50HZ,the period is 20000us
self.setPWM(channel, 0, int(pulse))

26
compLib/Servo.py
View file

@ -1,30 +1,36 @@
from compLib.PCA9685 import PCA9685
from PCA9685 import PCA9685
SERVO_COUNT = 10
pwm = PCA9685(0x40, debug=True)
pwm.setPWMFreq(50)
MIN_ANGLE = -90.0
MAX_ANGLE = 90.0
class Servo:
"""Control the servo ports on the robot
"""
@staticmethod
def set_position(channel: int, angle: int):
def set_position(channel: int, angle: int, offset: float =90):
"""Set position of servo connected to port
:param channel: channel between 0 and 7
:param angle: Angle of servo
"""
angle = abs(angle)
if channel == 0:
pwm.setServoPulse(8 + channel, 2500 - int(angle / 0.09))
elif channel < 8:
pwm.setServoPulse(8 + channel, 500 - int(angle / 0.09))
if channel < 0 or channel >= SERVO_COUNT:
raise IndexError("Invalid Servo channel specified!")
angle = max(min(angle, MAX_ANGLE), MIN_ANGLE)
pwm.setServoPulse(channel + 8, 500+int((angle+offset)/0.09))
@staticmethod
def setup_position():
"""Set position of servos to the position used during the setup process
"""
pwm.setServoPulse(8, 1500)
pwm.setServoPulse(9, 1500)
Servo.set_position(0, 0)
Servo.set_position(1, 0)