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

Add servo

Browse source
This commit is contained in:
Konstantin Lampalzer 2021-09-23 20:43:41 +02:00
parent 1c0310b9c7
commit 31eb91d51b
1 changed files with 57 additions and 29 deletions
Download patch file Download diff file Expand all files Collapse all files

86
compLib/Servo.py
View file

@ -1,36 +1,64 @@
# from compLib.PCA9685 import PCA9685
# SERVO_COUNT = 10
# pwm = PCA9685(0x40, debug=True)
# pwm.setPWMFreq(50)
# MIN_ANGLE = -90.0
# MAX_ANGLE = 90.0
from compLib.Spi import Spi, Register
# class Servo:
# """Control the servo ports on the robot
# """
SERVO_COUNT = 8
# @staticmethod
# def set_position(channel: int, angle: int, offset: float =90):
# """Set position of servo connected to port
MIN_ANGLE = -90.0
MAX_ANGLE = 90.0
# :param channel: channel between 0 and 7
# :param angle: Angle of servo
# """
# if channel < 0 or channel >= SERVO_COUNT:
# raise IndexError("Invalid Servo channel specified!")
def map(x, in_min, in_max, out_min, out_max):
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min
class Servo:
"""Control the servo ports on the robot
"""
@staticmethod
def set_position(port: int, angle: float, offset: float = 0.0):
"""Set position of servo connected to port
:param port: port between 1 and 8
:param angle: Angle of servo
"""
if port <= 0 or port > SERVO_COUNT:
raise IndexError("Invalid Servo port specified!")
# angle = max(min(angle, MAX_ANGLE), MIN_ANGLE)
# pwm.setServoPulse(channel + 8, 500+int((angle+offset)/0.09))
angle = max(min(angle, MAX_ANGLE), MIN_ANGLE)
# @staticmethod
# def setup_position():
# """Set position of servos to the position used during the setup process
# """
mapped_angle = int(map(angle, MIN_ANGLE, MAX_ANGLE, 0, 65535))
print(mapped_angle)
if port == 1:
Spi.write(Register.SERVO_1_PWM_H, 2, mapped_angle)
Spi.write(Register.PWM_1_CTRL, 1, 4)
elif port == 2:
Spi.write(Register.SERVO_2_PWM_H, 2, mapped_angle)
Spi.write(Register.PWM_1_CTRL, 1, 4)
elif port == 3:
Spi.write(Register.SERVO_3_PWM_H, 2, mapped_angle)
Spi.write(Register.PWM_2_CTRL, 1, 4)
elif port == 4:
Spi.write(Register.SERVO_4_PWM_H, 2, mapped_angle)
Spi.write(Register.PWM_2_CTRL, 1, 4)
elif port == 5:
Spi.write(Register.SERVO_5_PWM_H, 2, mapped_angle)
Spi.write(Register.PWM_3_CTRL, 1, 4)
elif port == 6:
Spi.write(Register.SERVO_6_PWM_H, 2, mapped_angle)
Spi.write(Register.PWM_3_CTRL, 1, 4)
elif port == 7:
Spi.write(Register.SERVO_7_PWM_H, 2, mapped_angle)
Spi.write(Register.PWM_4_CTRL, 1, 4)
elif port == 8:
Spi.write(Register.SERVO_8_PWM_H, 2, mapped_angle)
Spi.write(Register.PWM_4_CTRL, 1, 4)
@staticmethod
def setup_position():
"""Set position of servos to the position used during the setup process
"""
# Servo.set_position(0, 0)
# Servo.set_position(1, 0)
for i in range(1, SERVO_COUNT + 1):
Servo.set_position(i, 0)