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 /= 4096.0 # 12-bit
prescaleval /= float(freq) prescaleval /= float(freq)
prescaleval -= 1.0 prescaleval -= 1.0
prescale = math.floor(prescaleval + 0.5)
oldmode = self.read(self.__MODE1) oldmode = self.read(self.__MODE1)
newmode = (oldmode & 0x7F) | 0x10 # sleep newmode = (oldmode & 0x7F) | 0x10 # sleep
self.write(self.__MODE1, newmode) # go to 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) self.write(self.__MODE1, oldmode)
time.sleep(0.005) time.sleep(0.005)
self.write(self.__MODE1, oldmode | 0x80) self.write(self.__MODE1, oldmode | 0x80)
@ -69,7 +68,7 @@ class PCA9685:
def setServoPulse(self, channel, pulse): def setServoPulse(self, channel, pulse):
"Sets the Servo Pulse,The PWM frequency must be 50HZ" "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)) self.setPWM(channel, 0, int(pulse))

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