class Servos:
"""PCA 9865 Servo motors"""
def __init__(self, addr=0x6f, deflect_90_in_ms=0.5):
"""addr: The i2c address of the PWM chip.
deflect_90_in_ms: set this to calibrate the servo motors.
it is what a deflection of 90 degrees is
in terms of a pulse length in milliseconds."""
self._pwm = PWM(addr)
# This sets the timebase for it all
pwm_frequency = 100
self._pwm.setPWMFreq(pwm_frequency)
# Mid-point of the servo pulse length in milliseconds.
servo_mid_point_ms = 1.5
# Frequency is 1/period, but working ms, we can use 1000
period_in_ms = 1000 / pwm_frequency
# The chip has 4096 steps in each period.
pulse_steps = 4096
# Steps for every millisecond.
steps_per_ms = pulse_steps / period_in_ms
# Steps for a degree
self.steps_per_degree = (deflect_90_in_ms * steps_per_ms) / 90
# Mid-point of the servo in steps
self.servo_mid_point_steps = servo_mid_point_ms * steps_per_ms
# Map for channels
self.channels = [0, 1, 14, 15]
def stop_all(self):
# 0 in start is nothing
off_bit = 4096 # bit 12 is the OFF bit.
self._pwm.setPWM(self.channels[0], 0, off_bit)
self._pwm.setPWM(self.channels[1], 0, off_bit)
self._pwm.setPWM(self.channels[2], 0, off_bit)
self._pwm.setPWM(self.channels[3], 0, off_bit)
def _convert_degrees_to_steps(self, position):
return int(self.servo_mid_point_steps +
(position * self.steps_per_degree))
def set_servo_angle(self, channel, angle):
"""position: The position in degrees from the center. -90 to 90"""
# Validate
if angle > 90 or angle < -90:
raise ValueError("Angle outside of range")
# Then set the position
off_step = self._convert_degrees_to_steps(angle)
self._pwm.setPWM(self.channels[channel], 0, off_step)
class Servos:
## Constructor
# @param[in] addr: the I2C address of the PWM chip.
# @param[in] deflect_90_in_ms: set this to calibrate the seros
# it is the deflection of 90 degrees
# in terms of pulse length in ms
def __init__(self, addr=0x6f, deflect_90_in_ms=0.9):
self._pwm = PWM(addr)
pwm_freqeuncy = 60 # this sets the timebase for the motorhat
self._pwm.setPWMFreq(pwm_freqeuncy)
#Frequency is 1/period, but working in ms, we can use 1000
period_in_ms = 1000.0 / pwm_freqeuncy
#the chip has 4096 steps in each period.
pulse_steps = 4096.0
# mid point of the servo pulse length in milliseconds
servo_mid_point_ms = 1.6
# steps for every milisecond
steps_per_ms = pulse_steps / period_in_ms
# steps for a degree
self.steps_per_degree = (deflect_90_in_ms * steps_per_ms) / 90.0
# Mid point of the Servo in steps
self.servo_mid_point_steps = servo_mid_point_ms * steps_per_ms
def stop_all(self):
# 0 in start is nothing, 4096 sets the OFF bit.
self._pwm.setPWM(0, 0, 4096)
self._pwm.setPWM(1, 0, 4096)
self._pwm.setPWM(14, 0, 4096)
self._pwm.setPWM(15, 0, 4096)
def _convert_degrees_to_pwm(self, position):
return int(self.servo_mid_point_steps +
(position * self.steps_per_degree))
## Function to set the angle of a servo
# @param[in] angle: the angle in degrees from the center. -90 to 90
# @param[in] channel: channel of servo you wish to move
def set_servo_angle(self, channel, angle):
# validate angle
if angle > 90 or angle < -90:
raise ValueError("Angle outside of range")
# Then set the position
off_step = self._convert_degrees_to_pwm(angle)
self._pwm.setPWM(channel, 0, off_step)
class Servos(object):
def __init__(self, addr=0x6f, deflect_90_in_ms=0.9):
"""addr: The i2c address of the PWM chip.
deflect_90_in_ms: set this to calibrate the servo motors.
it is what a deflection of 90 degrees is
in terms of a pulse length in milliseconds."""
self._pwm = PWM(addr)
# This sets the timebase for it all
pwm_frequency = 60
self._pwm.setPWMFreq(pwm_frequency)
# Frequency is 1/period, but working ms, we can use 1000
period_in_ms = 1000.0 / pwm_frequency
# The chip has 4096 steps in each period.
pulse_steps = 4096.0
# Mid point of the servo pulse length in milliseconds.
servo_mid_point_ms = 1.5
# Steps for every millisecond.
steps_per_ms = pulse_steps / period_in_ms
# Steps for a degree
self.steps_per_degree = (deflect_90_in_ms * steps_per_ms) / 90.0
# Mid point of the servo in steps
self.servo_mid_point_steps = servo_mid_point_ms * steps_per_ms
# Prepare servo's turned off
self.stop_all()
def stop_all(self):
# 0 in start is nothing, 4096 sets the OFF bit.
self._pwm.setPWM(0, 0, 4096)
self._pwm.setPWM(1, 0, 4096)
self._pwm.setPWM(14, 0, 4096)
self._pwm.setPWM(15, 0, 4096)
def _convert_degrees_to_pwm(self, position):
return int(self.servo_mid_point_steps +
(position * self.steps_per_degree))
def set_servo_angle(self, channel, angle):
"""position: The position in degrees from the center. -90 to 90"""
# Validate
if angle > 90 or angle < -90:
raise ValueError("Angle outside of range")
# Then set the position
off_step = self._convert_degrees_to_pwm(angle)
self._pwm.setPWM(channel, 0, off_step)
class Raspi_MotorHAT:
FORWARD = 1
BACKWARD = 2
BRAKE = 3
RELEASE = 4
SINGLE = 1
DOUBLE = 2
INTERLEAVE = 3
MICROSTEP = 4
def __init__(self, addr=0x60, freq=1600):
self._i2caddr = addr # default addr on HAT
self._frequency = freq # default @1600Hz PWM freq
self.motors = [Raspi_DCMotor(self, m) for m in range(4)]
self.steppers = [
Raspi_StepperMotor(self, 1),
Raspi_StepperMotor(self, 2)
]
self._pwm = PWM(addr, debug=False)
self._pwm.setPWMFreq(self._frequency)
def setPin(self, pin, value):
if (pin < 0) or (pin > 15):
raise NameError('PWM pin must be between 0 and 15 inclusive')
if (value != 0) and (value != 1):
raise NameError('Pin value must be 0 or 1!')
if (value == 0):
self._pwm.setPWM(pin, 0, 4096)
if (value == 1):
self._pwm.setPWM(pin, 4096, 0)
def getStepper(self, steps, num):
if (num < 1) or (num > 2):
raise NameError(
'MotorHAT Stepper must be between 1 and 2 inclusive')
return self.steppers[num - 1]
def getMotor(self, num):
if (num < 1) or (num > 4):
raise NameError('MotorHAT Motor must be between 1 and 4 inclusive')
return self.motors[num - 1]
class Servos(object):
def __init__(self, addr=0x6f, deflect_90_in_ms = 1.15):
"""addr: The i2c address of the PWM chip.
deflect_90_in_ms: set this to calibrate the servo motors. It is what a deflection of 90 degrees is in turns of a pulse length in milliseconds.
"""
# deflect_90_in_ms - set this to calibrate the servo motors.
self._pwm = PWM(addr)
pwm_frequency = 60
self._pwm.setPWMFreq(pwm_frequency)
period_in_ms = 1000 / pwm_frequency
pulse_steps = 4096.0
servo_mid_point_ms = 1.5
# Steps for every millisecond
steps_per_ms = pulse_steps / period_in_ms
self.steps_per_degree = (deflect_90_in_ms * steps_per_ms) / 90.0
# Mid point of the servo in steps
self.servo_mid_point_steps = servo_mid_point_ms * steps_per_ms
def stop_all(self):
#0 in start is nothing, 4096 sets the off bit.
self._pwm(0, 0, 4096)
self._pwm(1, 0, 4096)
self._pwm(14, 0, 4096)
self._pwm(15, 0, 4096)
def convert_degrees_to_pwm(self, position):
return int(self.servo_mid_point_steps + (position * self.steps_per_degree))
def set_servo_angle(self, channel, angle):
"""position: The position in degrees from the center. -90 to 90"""
if angle > 90 or angle < -90:
raise ValueError("Angle outside of range")
off_step = self.convert_degrees_to_pwm(angle)
self._pwm.setPWM(channel, 0, off_step)
from Raspi_MotorHAT.Raspi_PWM_Servo_Driver import PWM
import atexit
pwm = PWM(0x6f)
pwm_frequency = 60
pwm.setPWMFreq(pwm_frequency)
#Frequency is 1/period, but working ms, we can use 1000
period_in_ms = 1000 / pwm_frequency
# The chip has 4096 steps in each period
pulse_steps = 4096.0
# Mid point of the servo pulse length in milliseconds
servo_mid_point_ms = 1.5
# What a deflection of 90 degrees is in pulse length in milliseconds
deflect_90_in_ms = 1.15
# Steps for every millisecond
steps_per_ms = pulse_steps / period_in_ms
# Steps for a degree
steps_per_degree = (deflect_90_in_ms * steps_per_ms) / 90.0
# Mid point of the servo in steps
servo_mid_point_steps = servo_mid_point_ms * steps_per_ms
def convert_degrees_to_pwm(position):
return int(servo_mid_point_steps + (position * steps_per_degree))
import atexit
import time
r = Robot()
pwm = PWM(0x6f)
servo_min = 150 # Servo minimum position.
servo_max = 600 # Maximum position
servo_mid = 225 + 150 # Middle position
def stop():
pwm.setPWM(0, 0, 0)
atexit.register(stop)
pwm.setPWMFreq(60) # Set frequency to 60 Hz
while (True):
r.set_left(255)
r.set_right(255)
print("min")
pwm.setPWM(0, 0, servo_min)
time.sleep(1)
print("max")
pwm.setPWM(0, 0, servo_max)
time.sleep(1)
r.stop_motors()
print("mid")
pwm.setPWM(0, 0, servo_mid)
time.sleep(1)