Last active
December 17, 2015 23:49
-
-
Save nealtodd/5691726 to your computer and use it in GitHub Desktop.
Subclass of WiimoteConnect for controlling a real Pi-driven Lego-based forklift truck.
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| import piface.pfio as pfio | |
| from wiimote import WiimoteConnect | |
| class WiiBot(WiimoteConnect): | |
| """ | |
| Use Wiimote to control robot | |
| Basic usage: | |
| from wiibot import WiiBot | |
| bot = WiiBot() | |
| bot.connect() | |
| bot.start() | |
| ... | |
| bot.stop() | |
| bot.disconnect() | |
| """ | |
| LEFT_WHEEL = 0 | |
| RIGHT_WHEEL = 1 | |
| LEFT_WHEEL_POLARITY = 2 | |
| RIGHT_WHEEL_POLARITY = 3 | |
| FORKLIFT = 4 | |
| FORKLIFT_POLARITY = 5 | |
| FORKLIFT_SWITCH_UPPER = 0 | |
| FORKLIFT_SWITCH_LOWER = 1 | |
| FORKS_MOVING = False | |
| ON = 1 | |
| OFF = 0 | |
| def write(self, target, action): | |
| pfio.digital_write(target, action) | |
| def __init__(self): | |
| super(WiiBot, self).__init__() | |
| pfio.init() | |
| self.freq = 10 | |
| def forward(self): | |
| self.write(self.LEFT_WHEEL_POLARITY, self.OFF) | |
| self.write(self.RIGHT_WHEEL_POLARITY, self.OFF) | |
| self.write(self.LEFT_WHEEL, self.ON) | |
| self.write(self.RIGHT_WHEEL, self.ON) | |
| def reverse(self): | |
| self.write(self.LEFT_WHEEL_POLARITY, self.ON) | |
| self.write(self.RIGHT_WHEEL_POLARITY, self.ON) | |
| self.write(self.LEFT_WHEEL, self.ON) | |
| self.write(self.RIGHT_WHEEL, self.ON) | |
| def left(self): | |
| self.write(self.LEFT_WHEEL_POLARITY, self.ON) | |
| self.write(self.RIGHT_WHEEL_POLARITY, self.OFF) | |
| self.write(self.LEFT_WHEEL, self.ON) | |
| self.write(self.RIGHT_WHEEL, self.ON) | |
| def right(self): | |
| self.write(self.LEFT_WHEEL_POLARITY, self.OFF) | |
| self.write(self.RIGHT_WHEEL_POLARITY, self.ON) | |
| self.write(self.LEFT_WHEEL, self.ON) | |
| self.write(self.RIGHT_WHEEL, self.ON) | |
| def forks_start(self): | |
| self.FORKS_MOVING = True | |
| self.write(self.FORKLIFT, self.ON) | |
| def forks_stop(self): | |
| self.FORKS_MOVING = False | |
| self.write(self.FORKLIFT, self.OFF) | |
| self.write(self.FORKLIFT_POLARITY, self.OFF) | |
| def forks_move(self, down): | |
| self.write(self.FORKLIFT_POLARITY, down) | |
| self.forks_start() | |
| while not self.forks_at_limit() and self.FORKS_MOVING: | |
| pass | |
| self.forks_stop() | |
| def forks_at_limit(self): | |
| at_limit = ( | |
| pfio.digital_read(self.FORKLIFT_SWITCH_LOWER) == 1 or | |
| pfio.digital_read(self.FORKLIFT_SWITCH_UPPER) == 1 | |
| ) | |
| at_limit and self._pulse_rumble(0.25) | |
| return at_limit | |
| def brake(self): | |
| self.write(self.LEFT_WHEEL, self.OFF) | |
| self.write(self.RIGHT_WHEEL, self.OFF) | |
| self.write(self.LEFT_WHEEL_POLARITY, self.OFF) | |
| self.write(self.RIGHT_WHEEL_POLARITY, self.OFF) | |
| def _btn_left(self, is_down): | |
| if is_down: | |
| self.left() | |
| self.leds(led1=True, led4=False) | |
| else: | |
| self.brake() | |
| self.leds(led1=False) | |
| def _btn_right(self, is_down): | |
| if is_down: | |
| self.right() | |
| self.leds(led1=False, led4=True) | |
| else: | |
| self.brake() | |
| self.leds(led4=False) | |
| def _btn_up(self, is_down): | |
| if is_down: | |
| if not (self.is_pressed(self.BTN_LEFT) or self.is_pressed(self.BTN_RIGHT)): | |
| self.forward() | |
| else: | |
| self.brake() | |
| def _btn_down(self, is_down): | |
| if is_down: | |
| if not (self.is_pressed(self.BTN_LEFT) or self.is_pressed(self.BTN_RIGHT)): | |
| self.reverse() | |
| else: | |
| self.brake() | |
| def _btn_plus(self, is_down): | |
| if is_down: | |
| self.forks_move(down=False) | |
| def _btn_minus(self, is_down): | |
| if is_down and not (self.is_pressed(self.BTN_PLUS)): | |
| self.forks_move(down=True) | |
| def _btn_a(self, is_down): | |
| if is_down: | |
| self.forks_move(down=False) | |
| else: | |
| self.FORKS_MOVING = False | |
| def _btn_b(self, is_down): | |
| if is_down: | |
| if not self.is_pressed(self.BTN_A): | |
| self.forks_move(down=True) | |
| else: | |
| self.FORKS_MOVING = False | |
| def _btn_home(self, is_down): | |
| pass | |
| def _btn_2(self, is_down): | |
| pass | |
| def _btn_1(self, is_down): | |
| pass |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Haven't tested this version with the physical bot yet. The previous revision works fine in case this is buggy.