CamJam Edukit 3 Robot

This example drives the CamJam EduKit 3 robot and is based on Tiny4WD Robot Drive with some modifications by Mike Horne to support the different motor driver board that kit uses.

The code below shows, with a few extra bits and pieces needed in case you’re running without the robot libraries, how to use this library to drive it around. The left analogue stick controls the robot, and the HOME button exits.

../_images/edukit3.jpg 
  1# Code for CamJam EduKit 3 robot
  2#
  3# By Mike Horne, based on code by Tom Oinn/Emma Norling code
  4
  5# Need floating point division of integers
  6from __future__ import division
  7
  8from time import sleep
  9
 10try:
 11    # Attempt to import the GPIO Zero library. If this fails, because we're running somewhere
 12    # that doesn't have the library, we create dummy functions for set_speeds and stop_motors which
 13    # just print out what they'd have done. This is a fairly common way to deal with hardware that
 14    # may or may not exist!
 15
 16    # Use GPIO Zero implementation of CamJam EduKit robot (thanks Ben Nuttall/Dave Jones!)
 17    from gpiozero import CamJamKitRobot
 18
 19    print('GPIO Zero found')
 20
 21    # Get the robot instance and the independent motor controllers
 22    robot = CamJamKitRobot()
 23    motor_left = robot.left_motor
 24    motor_right = robot.right_motor
 25
 26    # Motors are reversed. If you find your robot going backwards, set this to 1
 27    motor_multiplier = -1
 28
 29
 30    def set_speeds(power_left, power_right):
 31        """
 32        As we have an motor hat, we can use the motors
 33
 34        :param power_left: 
 35            Power to send to left motor
 36        :param power_right: 
 37            Power to send to right motor, will be inverted to reflect chassis layout
 38        """
 39
 40        # If one wants to see the 'raw' 0-100 values coming in
 41        # print("source left: {}".format(power_left))
 42        # print("source right: {}".format(power_right))
 43
 44        # Take the 0-100 inputs down to 0-1 and reverse them if necessary
 45        power_left = (motor_multiplier * power_left) / 100
 46        power_right = (motor_multiplier * power_right) / 100
 47
 48        # Print the converted values out for debug
 49        # print("left: {}".format(power_left))
 50        # print("right: {}".format(power_right))
 51
 52        # If power is less than 0, we want to turn the motor backwards, otherwise turn it forwards
 53        if power_left < 0:
 54            motor_left.backward(-power_left)
 55        else:
 56            motor_left.forward(power_left)
 57
 58        if power_right < 0:
 59            motor_right.backward(-power_right)
 60        else:
 61            motor_right.forward(power_right)
 62
 63
 64    def stop_motors():
 65        """
 66        As we have an motor hat, stop the motors using their motors call
 67        """
 68        # Turn both motors off
 69        motor_left.stop()
 70        motor_right.stop()
 71
 72except ImportError:
 73
 74    print('GPIO Zero not found, using dummy functions.')
 75
 76
 77    def set_speeds(power_left, power_right):
 78        """
 79        No motor hat - print what we would have sent to it if we'd had one.
 80        """
 81        print('DEBUG Left: {}, Right: {}'.format(power_left, power_right))
 82        sleep(0.3)
 83
 84
 85    def stop_motors():
 86        """
 87        No motor hat, so just print a message.
 88        """
 89        print('DEBUG Motors stopping')
 90
 91# All we need, as we don't care which controller we bind to, is the ControllerResource
 92from approxeng.input.selectbinder import ControllerResource
 93
 94
 95# Enable logging of debug messages, by default these aren't shown
 96# import logzero
 97# logzero.setup_logger(name='approxeng.input', level=logzero.logging.DEBUG)
 98
 99class RobotStopException(Exception):
100    """
101    The simplest possible subclass of Exception, we'll raise this if we want to stop the robot
102    for any reason. Creating a custom exception like this makes the code more readable later.
103    """
104    pass
105
106
107def mixer(yaw, throttle, max_power=100):
108    """
109    Mix a pair of joystick axes, returning a pair of wheel speeds. This is where the mapping from
110    joystick positions to wheel powers is defined, so any changes to how the robot drives should
111    be made here, everything else is really just plumbing.
112    
113    :param yaw: 
114        Yaw axis value, ranges from -1.0 to 1.0
115    :param throttle: 
116        Throttle axis value, ranges from -1.0 to 1.0
117    :param max_power: 
118        Maximum speed that should be returned from the mixer, defaults to 100
119    :return: 
120        A pair of power_left, power_right integer values to send to the motor driver
121    """
122    left = throttle + yaw
123    right = throttle - yaw
124    scale = float(max_power) / max(1, abs(left), abs(right))
125    return int(left * scale), int(right * scale)
126
127
128# Outer try / except catches the RobotStopException we just defined, which we'll raise when we want to
129# bail out of the loop cleanly, shutting the motors down. We can raise this in response to a button press
130try:
131    while True:
132        # Inner try / except is used to wait for a controller to become available, at which point we
133        # bind to it and enter a loop where we read axis values and send commands to the motors.
134        try:
135            # Bind to any available joystick, this will use whatever's connected as long as the library
136            # supports it.
137            with ControllerResource(dead_zone=0.1, hot_zone=0.2) as joystick:
138                print('Controller found, press HOME button to exit, use left stick to drive.')
139                print(joystick.controls)
140                # Loop until the joystick disconnects, or we deliberately stop by raising a
141                # RobotStopException
142                while joystick.connected:
143                    # Get joystick values from the left analogue stick
144                    x_axis, y_axis = joystick['lx', 'ly']
145                    # Get power from mixer function
146                    power_left, power_right = mixer(yaw=x_axis, throttle=y_axis)
147                    # Set motor speeds
148                    set_speeds(power_left, power_right)
149                    # Get a ButtonPresses object containing everything that was pressed since the last
150                    # time around this loop.
151                    joystick.check_presses()
152                    # Print out any buttons that were pressed, if we had any
153                    if joystick.has_presses:
154                        print(joystick.presses)
155                    # If home was pressed, raise a RobotStopException to bail out of the loop
156                    # Home is generally the PS button for playstation controllers, XBox for XBox etc
157                    if 'home' in joystick.presses:
158                        raise RobotStopException()
159        except IOError:
160            # We get an IOError when using the ControllerResource if we don't have a controller yet,
161            # so in this case we just wait a second and try again after printing a message.
162            print('No controller found yet')
163            sleep(1)
164except RobotStopException:
165    # This exception will be raised when the home button is pressed, at which point we should
166    # stop the motors.
167    stop_motors()