#!/usr/bin/python import RPi.GPIO as GPIO from .PCA9685 import PCA9685 import time import math class dw_Motor: def __init__(self, controller, num, freq): _SERVO_MIN_MS = 1.250 #ms _SERVO_MAX_MS = 1.750 #ms self.speed = 0 self.MC = controller self.cnum = num self.pin = 0 self.freq = freq self.servo_min = math.trunc( ( _SERVO_MIN_MS * 4096 ) / (1000.0 / self.freq ) - 1 ) self.servo_max = math.trunc( ( _SERVO_MAX_MS * 4096 ) / (1000.0 / self.freq ) - 1 ) self.servo_zero = math.trunc( ( self.servo_min + self.servo_max ) / 2 ) # halfway = 0 degrees if (num == 0): self.pin = 0 elif (num == 1): self.pin = 1 elif (num == 2): self.pin = 2 elif (num == 3): self.pin = 3 elif (num == 4): self.pin = 4 elif (num == 5): self.pin = 5 else: raise NameError('Port must be between 1 and 12 inclusive') # switch off self.off() def off(self): self.MC.setPin(self.pin, 0) def setAngle(self, angle): pulse = self.servo_zero + ( (self.servo_zero - self.servo_min ) * angle / 80 ) print "angle=%s pulse=%s" % (angle, pulse) #self.setPWMmS( pulse ) def setPWM(self, value): if(value > 0): self.MC._pwm.set_pwm(self.pin, 0, int(value) ) if(value == 0): self.off() def setPWMmS(self, length_ms): self.setPWM( math.trunc( ( length_ms * 4096 ) / ( 1000.0 / self.freq ) ) - 1 ) def setPWMuS(self, length_us): self.setPWM( math.trunc( ( length_us * 4096 ) / ( 1000000.0 / self.freq ) ) -1 ) def setMotorSpeed(self, value): # Check for PWM values if(value > 1000) and (value < 2000): self.setPWMuS(value) # Translate for motor values if(value > 0) and (value <= 255): self.setPWMuS( round(translate(value, 0, 255, 1500, 2000))) if(value == 0): self.setPWMuS(1500) if(value < 0) and (value >= -255): self.setPWMuS(round(translate(abs(value), 0, 255, 1500, 1000))) class dw_Servo: def __init__(self, controller, num, freq): _SERVO_MIN_MS = 1.250 #ms _SERVO_MAX_MS = 1.750 #ms self.speed = 0 self.MC = controller self.cnum = num self.pin = 0 self.freq = freq self.servo_min = math.trunc( ( _SERVO_MIN_MS * 4096 ) / (1000.0 / self.freq ) - 1 ) self.servo_max = math.trunc( ( _SERVO_MAX_MS * 4096 ) / (1000.0 / self.freq ) - 1 ) self.servo_zero = math.trunc( ( self.servo_min + self.servo_max ) / 2 ) # halfway = 0 degrees if (num == 0): self.pin = 6 elif (num == 1): self.pin = 7 elif (num == 2): self.pin = 8 elif (num == 3): self.pin = 9 elif (num == 4): self.pin = 10 elif (num == 5): self.pin = 11 else: raise NameError('Motors must be between 1 and 12 inclusive') # switch off self.off() def off(self): self.MC.setPin(self.pin, 0) def setAngle(self, angle): pulse = self.servo_zero + ( (self.servo_zero - self.servo_min ) * angle / 80 ) print "angle=%s pulse=%s" % (angle, pulse) #self.setPWMmS( pulse ) def setPWM(self, value): if(value > 0): self.MC._pwm.set_pwm(self.pin, 0, int(value) ) if(value == 0): self.off() def setPWMmS(self, length_ms): self.setPWM( math.trunc( ( length_ms * 4096 ) / ( 1000.0 / self.freq ) ) - 1 ) def setPWMuS(self, length_us): self.setPWM( math.trunc( ( length_us * 4096 ) / ( 1000000.0 / self.freq ) ) -1 ) def setMotorSpeed(self, value): # Check for PWM values if(value > 1000) and (value < 2000): self.setPWMuS(value) # Translate for motor values if(value > 0) and (value <= 255): self.setPWMuS( round(translate(value, 0, 255, 1500, 2000))) if(value == 0): self.setPWMuS(1500) if(value < 0) and (value >= -255): self.setPWMuS(round(translate(abs(value), 0, 255, 1500, 1000))) class dw_Controller: def __init__(self, addr = 0x61, freq = 100, correctionFactor = 1.0): self._i2caddr = addr # default addr on HAT self._frequency = freq # default @60Hz PWM freq # self.steppers = [ Adafruit_StepperMotor(self, 1), Adafruit_StepperMotor(self, 2) ] self._pwm = PCA9685(addr) self._pwm.set_pwm_freq(self._frequency, correctionFactor) # Just gonna default to high for now self.motors = [ dw_Motor(self, m, freq) for m in range(6) ] self.servos = [ dw_Servo(self, m, freq) for m in range(6) ] 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.set_pwm(pin, 0, 4096) if (value == 1): self._pwm.set_pwm(pin, 4096, 0) def setAllPin(self, 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.set_all_pwm(0, 4096) if (value == 1): self._pwm.set_all_pwm(4096, 0) def getMotor(self, num): if (num < 1) or (num > 6): raise NameError('Motors must be between 1 and 6 inclusive') return self.motors[num-1] def getServo(self, num): if (num < 1) or (num > 6): raise NameError('Servos must be between 1 and 6 inclusive') return self.servos[num-1] def setAllPWM(self, value): if(value > 0): self._pwm.set_all_pwm(0, value) if(value == 0): self.allOff() def setAllPWMmS(self, value): if(value > 0): self._pwm.set_all_pwm(0, value) if(value == 0): self.allOff() def setAllPWMuS(self, value): if(value > 0): self._pwm.set_all_pwm(0, value) if(value == 0): self.allOff() def allOff(self): this.setAllPin( 0 ); def translate(value, leftMin, leftMax, rightMin, rightMax): # Figure out how 'wide' each range is leftSpan = leftMax - leftMin rightSpan = rightMax - rightMin # Convert the left range into a 0-1 range (float) valueScaled = float(value - leftMin) / float(leftSpan) # Convert the 0-1 range into a value in the right range. return rightMin + (valueScaled * rightSpan)