def __init__(self, fwdpin, revpin, fwdcorr=0, revcorr=0):
self._pin_fwd = fwdpin
self._pin_rev = revpin
self._pwd_fwd = None
self._pwd_rev = None
self._fwdcorr = 1 - (float(validate_max(fwdcorr)) / 100)
self._revcorr = 1 - (float(validate_max(revcorr)) / 100)
self._initialized = False
def _calc_steps_from_angle(self, angle, spin=False):
angle = validate_max(angle, 360)
# For turning the robot width is radius, for spin diameter
if spin:
const = 1
else:
const = 2
return angle * math.pi * const * self._robot_width / 360 / self._step_dist
def set_angle(self, angle):
if self._initialized:
angle = validate_max(angle, self._max_angle)
self._curr_angle = angle
steps = int(self._min_steps + (self._curr_angle *
(self._max_steps - self._min_steps) / self._max_angle))
command = 'echo P1-%s=%s > /dev/servoblaster' % (self._pin, steps)
os.system(command)
def init(self, init_speed=20):
if not self._initialized:
init_speed = validate_max(init_speed)
GPIO.setup(self._pin_fwd, GPIO.OUT)
GPIO.setup(self._pin_rev, GPIO.OUT)
self._pwd_fwd = GPIO.PWM(self._pin_fwd, init_speed)
self._pwd_rev = GPIO.PWM(self._pin_rev, init_speed)
self._pwd_fwd.start(0)
self._pwd_rev.start(0)
self._initialized = True
def reverse(self, speed):
if self._initialized:
speed = validate_max(speed)
self._pwd_rev.ChangeFrequency(speed + 5)
self._pwd_rev.ChangeDutyCycle(speed * self._revcorr)
self._pwd_fwd.ChangeDutyCycle(0)
def set(self, intensity):
if self._initialized:
intensity = validate_max(100 - intensity)
self._pwm.ChangeDutyCycle(intensity)
def turn_rev_left(self, lf_pct=50):
lf_speed = float(validate_max(lf_pct)) / 100 * self._curr_speed
self._go_reverse(lf_speed, self._curr_speed)
self._last_action = self.turn_rev_left
self._last_action_arguments = {'lf_pct': lf_pct}
def turn_right(self, rg_pct=50):
rg_speed = float(validate_max(rg_pct)) / 100 * self._curr_speed
self._go_forward(self._curr_speed, rg_speed)
self._last_action = self.turn_right
self._last_action_arguments = {'rg_pct': rg_pct}
def set_speed(self, speed):
speed = validate_max(speed)
self._curr_speed = speed
self._exec_last_action()
return self._curr_speed