class Servo(object):
'''Servo driver class'''
_MIN_PULSE_WIDTH = 600
_MAX_PULSE_WIDTH = 2400
_DEFAULT_PULSE_WIDTH = 1500
_FREQUENCY = 60
_DEBUG = False
_DEBUG_INFO = 'DEBUG "Servo.py":'
def __init__(self,
channel,
offset=-19,
lock=True,
bus_number=None,
address=0x40):
''' Init a servo on specific channel, this offset '''
if channel < 0 or channel > 16:
raise ValueError(
"Servo channel \"{0}\" is not in (0, 15).".format(channel))
if self._DEBUG:
print(self._DEBUG_INFO, "Debug on")
self.channel = channel
self.offset = offset
self.lock = lock
self.pwm = PWM(bus_number=bus_number, address=address)
self.frequency = self._FREQUENCY
self.write(90)
def setup(self):
self.pwm.setup()
def _angle_to_analog(self, angle):
''' Calculate 12-bit analog value from giving angle '''
pulse_wide = self.pwm.map(angle, 0, 180, self._MIN_PULSE_WIDTH,
self._MAX_PULSE_WIDTH)
analog_value = int(float(pulse_wide) / 1000000 * self.frequency * 4096)
if self._DEBUG:
print(self._DEBUG_INFO,
'Angle %d equals Analog_value %d' % (angle, analog_value))
return analog_value
@property
def frequency(self):
return self._frequency
@frequency.setter
def frequency(self, value):
self._frequency = value
self.pwm.frequency = value
@property
def offset(self):
return self._offset
@offset.setter
def offset(self, value):
''' Set offset for much user-friendly '''
self._offset = value
if self._DEBUG:
print(self._DEBUG_INFO, 'Set offset to %d' % self.offset)
def write(self, angle):
''' Turn the servo with giving angle. '''
if self.lock:
if angle > 180:
angle = 180
if angle < 0:
angle = 0
else:
if angle < 0 or angle > 180:
raise ValueError(
"Servo \"{0}\" turn angle \"{1}\" is not in (0, 180).".
format(self.channel, angle))
val = self._angle_to_analog(angle)
val += self.offset
self.pwm.write(self.channel, 0, val)
if self._DEBUG:
print(self._DEBUG_INFO, 'Turn angle = %d' % angle)
@property
def debug(self):
return self._DEBUG
@debug.setter
def debug(self, debug):
''' Set if debug information shows '''
if debug in (True, False):
self._DEBUG = debug
else:
raise ValueError(
'debug must be "True" (Set debug on) or "False" (Set debug off), not "{0}"'
.format(debug))
if self._DEBUG:
print(self._DEBUG_INFO, "Set debug on")
else:
print(self._DEBUG_INFO, "Set debug off")
class Back_Wheels(object):
''' Back wheels control class '''
Motor_A = 17
Motor_B = 27
PWM_A = 4
PWM_B = 5
_DEBUG = False
_DEBUG_INFO = 'DEBUG "back_wheels.py":'
def __init__(self, debug=False, bus_number=1, db="config"):
''' Init the direction channel and pwm channel '''
self.forward_A = True
self.forward_B = True
self.db = fileDB(db=db)
self.forward_A = int(self.db.get('forward_A', default_value=1))
self.forward_B = int(self.db.get('forward_B', default_value=1))
self.left_wheel = Motor(self.Motor_A, offset=self.forward_A)
self.right_wheel = Motor(self.Motor_B, offset=self.forward_B)
self.pwm = PWM(bus_number=bus_number)
def _set_a_pwm(value):
pulse_wide = self.pwm.map(value, 0, 100, 0, 4095)
self.pwm.write(self.PWM_A, 0, pulse_wide)
def _set_b_pwm(value):
pulse_wide = self.pwm.map(value, 0, 100, 0, 4095)
self.pwm.write(self.PWM_B, 0, pulse_wide)
self.left_wheel.pwm = _set_a_pwm
self.right_wheel.pwm = _set_b_pwm
self._speed = 0
self.debug = debug
if self._DEBUG:
print(
self._DEBUG_INFO, 'Set left wheel to #%d, PWM channel to %d' %
(self.Motor_A, self.PWM_A))
print(
self._DEBUG_INFO, 'Set right wheel to #%d, PWM channel to %d' %
(self.Motor_B, self.PWM_B))
def forward(self):
''' Move both wheels forward '''
self.left_wheel.forward()
self.right_wheel.forward()
if self._DEBUG:
print(self._DEBUG_INFO, 'Running forward')
def backward(self):
''' Move both wheels backward '''
self.left_wheel.backward()
self.right_wheel.backward()
if self._DEBUG:
print(self._DEBUG_INFO, 'Running backward')
def stop(self):
''' Stop both wheels '''
self.left_wheel.stop()
self.right_wheel.stop()
if self._DEBUG:
print(self._DEBUG_INFO, 'Stop')
@property
def speed(self, speed):
return self._speed
@speed.setter
def speed(self, speed):
self._speed = speed
''' Set moving speeds '''
self.left_wheel.speed = self._speed
self.right_wheel.speed = self._speed
if self._DEBUG:
print(self._DEBUG_INFO, 'Set speed to', self._speed)
@property
def debug(self):
return self._DEBUG
@debug.setter
def debug(self, debug):
''' Set if debug information shows '''
if debug in (True, False):
self._DEBUG = debug
else:
raise ValueError(
'debug must be "True" (Set debug on) or "False" (Set debug off), not "{0}"'
.format(debug))
if self._DEBUG:
print(self._DEBUG_INFO, "Set debug on")
self.left_wheel.debug = True
self.right_wheel.debug = True
self.pwm.debug = True
else:
print(self._DEBUG_INFO, "Set debug off")
self.left_wheel.debug = False
self.right_wheel.debug = False
self.pwm.debug = False
def ready(self):
''' Get the back wheels to the ready position. (stop) '''
if self._DEBUG:
print(self._DEBUG_INFO, 'Turn to "Ready" position')
self.left_wheel.offset = self.forward_A
self.right_wheel.offset = self.forward_B
self.stop()
def calibration(self):
''' Get the front wheels to the calibration position. '''
if self._DEBUG:
print(self._DEBUG_INFO, 'Turn to "Calibration" position')
self.speed = 50
self.forward()
self.cali_forward_A = self.forward_A
self.cali_forward_B = self.forward_B
def cali_left(self):
''' Reverse the left wheels forward direction in calibration '''
self.cali_forward_A = (1 + self.cali_forward_A) & 1
self.left_wheel.offset = self.cali_forward_A
self.forward()
def cali_right(self):
''' Reverse the right wheels forward direction in calibration '''
self.cali_forward_B = (1 + self.cali_forward_B) & 1
self.right_wheel.offset = self.cali_forward_B
self.forward()
def cali_ok(self):
''' Save the calibration value '''
self.forward_A = self.cali_forward_A
self.forward_B = self.cali_forward_B
self.db.set('forward_A', self.forward_A)
self.db.set('forward_B', self.forward_B)
self.stop()
def test(self):
import time
back_wheels = Back_Wheels()
DELAY = 0.01
try:
back_wheels.forward()
for i in range(0, 100):
back_wheels.speed = i
print("Forward, speed =", i)
time.sleep(DELAY)
for i in range(100, 0, -1):
back_wheels.speed = i
print("Forward, speed =", i)
time.sleep(DELAY)
back_wheels.backward()
for i in range(0, 100):
back_wheels.speed = i
print("Backward, speed =", i)
time.sleep(DELAY)
for i in range(100, 0, -1):
back_wheels.speed = i
print("Backward, speed =", i)
time.sleep(DELAY)
except KeyboardInterrupt:
print("KeyboardInterrupt, motor stop")
back_wheels.stop()
finally:
print("Finished, motor stop")
back_wheels.stop()