diff --git a/darkwater_escape/PCA9685.py b/darkwater_escape/PCA9685.py index 09ba060..379ffb0 100644 --- a/darkwater_escape/PCA9685.py +++ b/darkwater_escape/PCA9685.py @@ -83,7 +83,7 @@ class PCA9685(object): self._device.write8(MODE1, mode1) time.sleep(0.005) # wait for oscillator - def set_pwm_freq(self, freq_hz): + def set_pwm_freq(self, freq_hz, correctionFactor=1.0): """Set the PWM frequency to the provided value in hertz.""" prescaleval = 25000000.0 # 25MHz prescaleval /= 4096.0 # 12-bit @@ -91,7 +91,7 @@ class PCA9685(object): prescaleval -= 1.0 logger.debug('Setting PWM frequency to {0} Hz'.format(freq_hz)) logger.debug('Estimated pre-scale: {0}'.format(prescaleval)) - prescale = int(math.floor(prescaleval + 0.5)) + prescale = round(prescaleval * correctionFactor + 0.5) logger.debug('Final pre-scale: {0}'.format(prescale)) oldmode = self._device.readU8(MODE1); newmode = (oldmode & 0x7F) | 0x10 # sleep @@ -101,7 +101,7 @@ class PCA9685(object): time.sleep(0.005) self._device.write8(MODE1, oldmode | 0x80) - def set_pwm_freq_min(self, freq_hz, correctionFactor=1.0): + def set_pwm_freq_min(self, freq_hz, correctionFactor=1.0): "Sets the PWM frequency" prescaleval = 25000000.0 # 25MHz prescaleval /= 4096.0 # 12-bit @@ -122,7 +122,7 @@ class PCA9685(object): time.sleep(0.005) self.i2c.write8(self.__MODE1, oldmode | 0x80) - def set_pwm_freq_max(self, freq_hz, correctionFactor=1.0): + def set_pwm_freq_max(self, freq_hz, correctionFactor=1.0): "Sets the PWM frequency" prescaleval = 25000000.0 # 25MHz prescaleval /= 4096.0 # 12-bit @@ -143,7 +143,7 @@ class PCA9685(object): time.sleep(0.005) self.i2c.write8(self.__MODE1, oldmode | 0x80) - def get_pwm_freq(self): + def get_pwm_freq(self): prescale = self.i2c.readU8(self.__PRESCALE) calcfreq = 25000000.0 / 4096.0 / ( float(prescale) + 1 ) if (self.debug): diff --git a/darkwater_escape/darkwater_escape.py b/darkwater_escape/darkwater_escape.py index d4d6010..6370a6d 100644 --- a/darkwater_escape/darkwater_escape.py +++ b/darkwater_escape/darkwater_escape.py @@ -23,31 +23,18 @@ class dw_Motor: self.servo_zero = math.trunc( ( self.servo_min + self.servo_max ) / 2 ) # halfway = 0 degrees - if (num == 0): self.pin = 9 elif (num == 1): - self.pin = 8 + self.pin = 8 elif (num == 2): - self.pin = 10 + self.pin = 10 elif (num == 3): - self.pin = 11 + self.pin = 11 elif (num == 4): - self.pin = 12 + self.pin = 12 elif (num == 5): - self.pin = 13 - elif (num == 6): - self.pin = 0 - elif (num == 7): - self.pin = 1 - elif (num == 8): - self.pin = 2 - elif (num == 9): - self.pin = 3 - elif (num == 10): - self.pin = 5 - elif (num == 11): - self.pin = 4 + self.pin = 13 else: raise NameError('Motors must be between 1 and 12 inclusive') @@ -64,40 +51,28 @@ class dw_Motor: def setPWM(self, value): if(value > 0): - self.MC._pwm.setPWM(self.pin, 0, int(value) ) + self.MC._pwm.set_pwm(self.pin, 0, int(value) ) if(value == 0): self.off() def setPWMmS(self, length_ms): - self.setPWM( round( length_ms * 4096 ) / ( 1000 / self.freq ) ) + self.setPWM( math.trunc( ( length_ms * 4096 ) / ( 1000.0 / self.freq ) ) - 1 ) def setPWMuS(self, length_us): - self.setPWM( round( length_us * 4096 ) / ( 1000000 / self.freq ) ) + 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.setPWMmS(value) + self.setPWMuS(value) # Translate for motor values if(value > 0) and (value <= 255): - self.setPWMmS( round(translate(value, 0, 255, 1500, 2000))) + self.setPWMuS( round(translate(value, 0, 255, 1500, 2000))) if(value == 0): - self.setPWMmS(1500) + self.setPWMuS(1500) if(value < 0) and (value >= -255): - self.setPWMmS(round(translate(abs(value), 0, 255, 1500, 1000))) + self.setPWMuS(round(translate(abs(value), 0, 255, 1500, 1000))) - def run(self, command, speed = 0): - if not self.MC: - return - if (command == dw_Controller.FORWARD): - self.MC.setPin(self.PHpin, 0) - self.MC._pwm.set_pwm(self.ENpin, 0, speed*16) - if (command == dw_Controller.BACKWARD): - self.MC.setPin(self.PHpin, 1) - self.MC._pwm.set_pwm(self.ENpin, 0, speed*16) - if (command == dw_Controller.RELEASE): - self.MC.setPin(self.PHpin, 0) - self.MC.setPin(self.ENpin, 0) class dw_Servo: def __init__(self, controller, num, freq): @@ -119,28 +94,16 @@ class dw_Servo: if (num == 0): - self.pin = 9 - elif (num == 1): - self.pin = 8 - elif (num == 2): - self.pin = 10 - elif (num == 3): - self.pin = 11 - elif (num == 4): - self.pin = 12 - elif (num == 5): - self.pin = 13 - elif (num == 6): self.pin = 0 - elif (num == 7): + elif (num == 1): self.pin = 1 - elif (num == 8): + elif (num == 2): self.pin = 2 - elif (num == 9): + elif (num == 3): self.pin = 3 - elif (num == 10): + elif (num == 4): self.pin = 5 - elif (num == 11): + elif (num == 5): self.pin = 4 else: raise NameError('Port must be between 1 and 12 inclusive') @@ -158,19 +121,27 @@ class dw_Servo: def setPWM(self, value): if(value > 0): - self.MC._pwm.setPWM(self.pin, 0, int(value) ) + self.MC._pwm.set_pwm(self.pin, 0, int(value) ) if(value == 0): self.off() def setPWMmS(self, length_ms): - self.setPWM( round( length_ms * 4096 ) / ( 1000 / self.freq ) ) + self.setPWM( math.trunc( ( length_ms * 4096 ) / ( 1000.0 / self.freq ) ) - 1 ) def setPWMuS(self, length_us): - self.setPWM( round( length_us * 4096 ) / ( 1000000 / self.freq ) ) + self.setPWM( math.trunc( ( length_us * 4096 ) / ( 1000000.0 / self.freq ) ) -1 ) - def run(self, command, speed = 0): - if not self.MC: - return + 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: @@ -183,8 +154,8 @@ class dw_Controller: self._pwm.set_pwm_freq(self._frequency, correctionFactor) # Just gonna default to high for now - self.servo = [ dw_PWM(self, m, freq) for m in range(6) ] - self.esc = [ dw_PWM(self, m, freq) for m in range(6, 12) ] + 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):