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)