def __init__(self):
self.motor_0_direction = Digital(33, 'out')
self.motor_1_direction = Digital(35, 'out')
self.motor_2_direction = Digital(37, 'out')
self.motor_0_speed = PWM(0)
self.motor_1_speed = PWM(1)
self.motor_2_speed = PWM(2)
def __init__(self):
self.motor_0_direction = Digital(33, 'out')
self.motor_1_direction = Digital(35, 'out')
self.motor_2_direction = Digital(37, 'out')
self.motor_0_speed = PWM(0)
self.motor_1_speed = PWM(1)
self.motor_2_speed = PWM(2)
class Pathfinder(object):
def __init__(self):
self.motor_0_direction = Digital(33, 'out')
self.motor_1_direction = Digital(35, 'out')
self.motor_2_direction = Digital(37, 'out')
self.motor_0_speed = PWM(0)
self.motor_1_speed = PWM(1)
self.motor_2_speed = PWM(2)
def move(self, x, y, w):
_motor_0 = -x * sin(pi/3) + (y * cos(pi/3)) + w
_motor_1 = -y + w
_motor_2 = x * sin(pi/3) + (y * cos(pi/3)) + w
_max = max([fabs(_motor_0), fabs(_motor_1), fabs(_motor_2)])
if _max > 1:
_motor_0 /= _max
_motor_1 /= _max
_motor_2 /= _max
_d, _spd = self._parse_speed(_motor_0)
self.motor_0_direction.write(_d)
self.motor_0_speed.duty_cycle = int(self.motor_0_speed.period * _spd)
_d, _spd = self._parse_speed(_motor_1)
print _d, _spd
self.motor_1_direction.write(_d)
self.motor_1_speed.duty_cycle = int(self.motor_1_speed.period * _spd)
_d, _spd = self._parse_speed(_motor_2)
print _d, _spd
self.motor_2_direction.write(_d)
self.motor_2_speed.duty_cycle = int(self.motor_2_speed.period * _spd)
def _parse_speed(self, speed):
if speed == 0:
return 0, 0
elif speed > 0:
return 0, speed
elif speed < 0:
return 1, 1 + speed
class Pathfinder(object):
def __init__(self):
self.motor_0_direction = Digital(33, 'out')
self.motor_1_direction = Digital(35, 'out')
self.motor_2_direction = Digital(37, 'out')
self.motor_0_speed = PWM(0)
self.motor_1_speed = PWM(1)
self.motor_2_speed = PWM(2)
def move(self, x, y, w):
_motor_0 = -x * sin(pi / 3) + (y * cos(pi / 3)) + w
_motor_1 = -y + w
_motor_2 = x * sin(pi / 3) + (y * cos(pi / 3)) + w
_max = max([fabs(_motor_0), fabs(_motor_1), fabs(_motor_2)])
if _max > 1:
_motor_0 /= _max
_motor_1 /= _max
_motor_2 /= _max
_d, _spd = self._parse_speed(_motor_0)
self.motor_0_direction.write(_d)
self.motor_0_speed.duty_cycle = int(self.motor_0_speed.period * _spd)
_d, _spd = self._parse_speed(_motor_1)
print _d, _spd
self.motor_1_direction.write(_d)
self.motor_1_speed.duty_cycle = int(self.motor_1_speed.period * _spd)
_d, _spd = self._parse_speed(_motor_2)
print _d, _spd
self.motor_2_direction.write(_d)
self.motor_2_speed.duty_cycle = int(self.motor_2_speed.period * _spd)
def _parse_speed(self, speed):
if speed == 0:
return 0, 0
elif speed > 0:
return 0, speed
elif speed < 0:
return 1, 1 + speed
