class Servo(object):
def __init__(self, servo_number, start_pos=300, used_i2c_addresses=[0x40]):
self.servo_number = servo_number
self.start_pos = start_pos
self.servo_pin, self.servo_hat_address = self._get_servo_connection(
servo_number)
self.used_i2c_addresses = used_i2c_addresses
if self.servo_hat_address in self.used_i2c_addresses:
self.servo_obj = PWM(self.servo_hat_address)
self.pwm_freq = 50
self.servo_obj.setPWMFreq(
self.pwm_freq) # 60hz = 16.666ms, 50hz = 20ms, 40hz = 25ms
def turn_off(self):
self.servo_obj.setPWM(self.servo_pin, 0, 4096)
def set_pos(self, new_pos):
logger.debug('Servo {}\tgoing to {}'.format(self.servo_pin, new_pos))
# new generation servos check only total length of duty cycle
self.servo_obj.setPWM(channel=self.servo_pin, on=0, off=new_pos)
sleep(1.0 / self.pwm_freq)
@staticmethod
def _get_servo_connection(servo_pin):
"""
Returns servo pins as int and servo hat board address as hex.
"""
hat_address = servo_pin / 16
servo_pin %= 16
hat_address = int(hex(64 + hat_address), 16)
return servo_pin, hat_address
class Adafruit_MotorHAT:
FORWARD = 1
BACKWARD = 2
BRAKE = 3
RELEASE = 4
SINGLE = 1
DOUBLE = 2
INTERLEAVE = 3
MICROSTEP = 4
def __init__(self, addr = 0x60, freq = 1600, i2c=None, i2c_bus=None):
self._frequency = freq
self.motors = [ Adafruit_DCMotor(self, m) for m in range(4) ]
self.steppers = [ Adafruit_StepperMotor(self, 1), Adafruit_StepperMotor(self, 2) ]
self.servos= [ Adafruit_Servo(self,0),Adafruit_Servo(self,1),Adafruit_Servo(self,14),Adafruit_Servo(self,15)]
self._pwm = PWM(addr, debug=False, i2c=i2c, i2c_bus=i2c_bus)
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]
def getServo(self,num):
if(num!=0) and (num!=1) and (num!=14) and (num!=15):
raise NameError('MotorHAT Servo must be [0,1,14,15]')
if(num == 14) or (num == 15):
num=num-12
return self.servos[num]
class Adafruit_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 = [Adafruit_DCMotor(self, m) for m in range(4)]
self.steppers = [
Adafruit_StepperMotor(self, 1),
Adafruit_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 RGB_LED:
OFFSET_RED = 0
OFFSET_GREEN = 1
OFFSET_BLUE = 2
def __init__(self, debug=False):
from Adafruit_MotorHAT.Adafruit_PWM_Servo_Driver import PWM # @UnresolvedImport
self.pwm = PWM(address=0x40, debug=debug)
for i in range(15):
self.pwm.setPWM(i, 0, 4095)
def setLEDBrightness(self, led, offset, brightness):
self.pwm.setPWM(3 * led + offset, brightness << 4, 4095)
def setRGBint24(self, led, color):
r = color >> 16 & 0xFF
g = color >> 8 & 0xFF
b = color >> 0 & 0xFF
self.setRGBvint8(led, [r, g, b])
def setRGBvint8(self, led, color):
self.setLEDBrightness(led, self.OFFSET_RED, color[0])
self.setLEDBrightness(led, self.OFFSET_GREEN, color[1])
self.setLEDBrightness(led, self.OFFSET_BLUE, color[2])
def setRGB(self, led, color):
assert len(color) == 3
assert min(color) >= 0
assert max(color) <= 1
self.setRGBvint8(led, [int(c * 255) for c in color])
def __del__(self):
for i in range(15):
self.pwm.setPWM(i, 0, 4095)
del self.pwm
#!/usr/bin/python
#This example is using the GPIO pin directly from Raspberry Pi 3
import sys
import tty, termios
from Adafruit_MotorHAT.Adafruit_PWM_Servo_Driver import PWM
from time import sleep
import argparse
ticks = 4096.0
periodTime = 20000.0 #50Hz a period time is 20000us
midTime = 1500.0 #Servo Middle: 1.5ms=1500us
minVal = int(1000 / periodTime * ticks) #Minimum PWM length is 1ms=1000us
maxVal = int(2000 / periodTime * ticks) #Maxium PWM length is 2ms=2000us
initVal = int(midTime * ticks / periodTime)
pulseDiv = 1
pulseVal = initVal
pwm = PWM() #Initialize pwm
pwm.setPWMFreq(50) #Setting Freq as 50Hz
reset_order = [3, 2, 1, 4, 5, 6]
for channelPin in reset_order:
pwm.setPWM(channelPin, 0, initVal)
sleep(1)
pwm.setPWM(channelPin, 0, 0)
class LRCameraServo():
pwm_pin = 1
last_angle = -1
def __init__(self):
self.setup()
def setup(self):
GPIO.setwarnings(False)
GPIO.setmode(GPIO.BOARD)
self.pwm = PWM(0x40, debug=False)
self.pwm.setPWMFreq(50) # 50 full 'pulses' per second
self.front() # calibration
def setServoPulse(self, pulse_width):
# pulse: 1ms, range:[0.5, 2.5]
pulseLength = 1000000.0 # 1,000,000 us per second
pulseLength /= 50.0 # 50 Hz
#print("%d us per period" % pulseLength)
pulseLength /= 4096.0 # 12 bits of resolution
#print("%d us per bit" % pulseLength)
pulse_width *= 1000.0
pulse_width /= (pulseLength * 1.0)
#print("pluse: %f " % (pulse_width))
self.pwm.setPWM(self.pwm_pin, 0, int(pulse_width))
# turn servo
# angle to pulse width
def turn_servo(self, angle):
pulse_width = angle / 90.0 + 0.5
pulse_width = max(pulse_width, 0.5)
pulse_width = min(pulse_width, 2.5)
self.setServoPulse(pulse_width)
# turn left (angle is based on front)
def turn_right_servo_test(self, angle):
angle = 90 - angle
self.turn_servo(angle)
# turn right ((angle is based on front))
def turn_left_servo_test(self, angle):
angle = 90 + angle
self.turn_servo(angle)
# front
def front(self):
self.last_angle = 90
self.turn_servo(90)
# turn left based on last angle
def turn_right_servo(self, angle):
real_angle = self.last_angle - angle
self.turn_servo(real_angle)
self.last_angle = real_angle
# turn right based on last angle
def turn_left_servo(self, angle):
real_angle = self.last_angle + angle
self.turn_servo(real_angle)
self.last_angle = real_angle
def destroy(self):
self.pwm = None
GPIO.cleanup()
pwm = PWM(0x60)
pwm_frequency = 60
pwm.setPWMFreq(pwm_frequency)
period_in_ms = 1000 / pwm_frequency
pulse_steps = 4096
servo_midPoint_ms = 1.5
deflect_90_in_ms = 0.5
flect_90_in_ms = 2
steps_per_ms = pulse_steps / period_in_ms
steps_per_degree = (deflect_90_in_ms * steps_per_ms) / 90
sero_mid_point_steps = servo_midPoint_ms * steps_per_ms
def convert_degrees_to_pwm(position):
return int(sero_mid_point_steps + (position * steps_per_degree))
def stop():
#set pin off flag
pwm.setPWM(0, 0, 4096)
atexit.register(stop)
while 1:
position = int(input("请输入角度（-90 ~ 90 0是中间）:"))
end_step = convert_degrees_to_pwm(position)
PWM.setPWM(0, 0, 0, end_step)
