RADU: Controlling Robot Movements with a Gamepad Controller using Python
Tags: robot, python, raspberrypi
Published: 2022-05-16 05:31:14

My robot RADU is a two wheeled robot that combines custom hardware for the sensor and motor, a Raspberry Pico and L293D motor shield, and a purchasable robotic arm that is controlled via a Raspberry Pi shield. For both parts of the robot, I want to use the same control mechanism and device: A game controller. To reach this goal, the last article gave an overview about the different libraries. This article is a tutorial about writing the controller software. You will learn how to use the Python library [Piborg Gamepad](https://github.com/piborg/Gamepad), and how to bridge pressed buttons or joystick to control the robots wheel movements and its arm.

_This article originally appeared at my blog [admantium.com](https://admantium.com/blog/robo16_controlling_wheel_and_arm_movements/)_.

## The Gamepad Library

The Gamepad Library of my choice is the [Piborg Gamepad](https://github.com/piborg/Gamepad). This versatile Python library provides simple abstraction to connect custom mapped gamepads, with a slight focus on video game consoles. It can detect the state of all buttons and the axis, e.g. on a PlayStation pad including the d-pad and the analog stick. And it provides different programming models how to access the gamepad state, e.g. with event listeners that trigger callbacks, or a continuous loop that gets the state of all buttons.

After a short time period only to get a simple button press into moving the robot, I decided to use this library to fully program my robot. Let’s start with the simple outline.

## Button Configuration

The first part is to import the library and setup variables for the buttons of the chosen gamepad. For this, checkout the [gamepad configurations](https://github.com/piborg/Gamepad/blob/master/Controllers.py) and take the defined button names from this file for your specific gamepad. Then, translate these to meaningful names. Here is my mapping for accessing the joystick, the digital pad and some buttons:

```python
import lib.Gamepad as Gamepad

# Gamepad settings
gamepadType = Gamepad.PS4

# Axis
joystickSpeed = 'LEFT-Y'
joystickSteering = 'RIGHT-X'

digital_x = 'DPAD-X' 
digital_y = 'DPAD-Y' 

# Button
button_ps = 'PS'
button_triangle = "TRIANGLE"
```

## Controller Connection

With the button setup completed, we will now wait write a method that waits for a gamepad to get connected. Once this condition is true, we will create a gamepad object that is instantiated with the configuration. This code looks as follows:

```python
def wait_for_gamepad():

  # Wait for a connection
  if not Gamepad.available():
    log_to_stdout_and_mqtt('Please connect your gamepad...')
    while not Gamepad.available():
      time.sleep(1.0)

  gamepad = gamepadType()
  return gamepad
```

## Detecting Gamepad Button Presses

The next step is to start the main loop of the program that will continually check the button state. I decided to use a simple loop that continually polls the buttons and executes some commands. Let’s start easy and define a listener for the buttons.

```python
def main_loop(gamepad)
  pollInterval = 0.01
  gamepad.startBackgroundUpdates()
  try:
    while gamepad.isConnected():
      if gamepad.beenPressed(button_triangle):
        log_to_stdout("Triangle Button is Pressed")

        # Sleep for our polling interval
        time.sleep(pollInterval)
  finally:
    # Ensure the background thread is always terminated when we are done
    gamepad.disconnect()
```

This method does a couple of things:

- The `pollInterval` determines how often per second the button states will be checked
- The complete button detection logic is enclosed in a `try...catch` block, mainly as a safeguard to release the gamepad if something goes wrong - alternatively you can define a custom context manager to handle the setup and teardown phases
- A gamepads button state are checked with the `beenPressed()` method, which gets an argument of the earlier defined button layout. As shown here, you can use `if` statements to check their state, and then execute some command
- At the end of the loop, a `sleep` command is issued with the defined `pollInterval`

Ok, now let’s see how to check for the d-pad and steering axes.

## Detecting the Digital and Analog Axis

During the button setup, we already defined custom names for the analog joysticks and the d-pad. Then, inside the main loop, we can detect their state as shown in the next code snippet.

```python
def main_loop(gamepad)
  #...
  try:
    while gamepad.isConnected():
      # ...
      speed = - gamepad.axis(joystickSpeed)
      steering = - gamepad.axis(joystickSteering)

      command = gamepad.axis(digital_x)

      log_to_stdout(f'Speed {speed}')
      log_to_stdout(f'Steering {steering}')

      engine_move(speed, steering, command)   
            
      time.sleep(pollInterval)
  finally:
    # Ensure the background thread is always terminated when we are done
    gamepad.disconnect()
```

To access the gamepad values, you use the `axis()` method and pass the button name. The values are floats of different granularly: For the d-pad buttons, its either `0.0` or `1.0`, but for the analog sticks, it’s a positive or negative 16-bit number with a maximum absolute value of `32.756`. Handle these values as they are required by your robot.

## Fine Tuning the Controls

This step is heavily dependent on your particular robots’ behavior. My robot continually listens for commands, processes them, and quits the command after 300ms of inactivity from the sender site. This means that the sender needs to continuously send the position of the joystick in order to let the robot only move when either is actually pressed.

This requirement is provided by the following code section:

```python
def main_loop(gamepad)
  #...
  try:
    while gamepad.isConnected():
      if (mode == MOVE_MODE):
        speed = -gamepad.axis(joystickSpeed)
        steering = -gamepad.axis(joystickSteering)

        if((speed != 0.0 or steering != 0.0) and not isStopped):
          last_speed = speed
          last_steeering = steering
          engine_move(speed, steering)   
        else:
          isStopped = True
                    
      time.sleep(pollInterval)
  finally:
    # Ensure the background thread is always terminated when we are done
    gamepad.disconnect()
```

## Controlling the Arm Movements

Once we figured out how to read and fine-tune the controller values to move the robots, lets create the additional feature to controls the robots arm as well. I will not detail the arm's microcontroller code that actually moves the arm, but focus on the gamepad code only.

The first feature that we need is to distinguish between different controller modes. This state is kept in the following variables:

```python
MOVE_MODE = 'MOVE_MODE'
ARM_MODE = 'ARM_MODE'
```

Inside the main loop, we need to make the following extension to change the current control mode:

```python
def main_loop(gamepad)
  #...
  while gamepad.isConnected():
    if gamepad.beenPressed(button_ps):
      if (mode == MOVE_MODE):
        mode = ARM_MODE
      else:
        mode = MOVE_MODE
```

Finally, the main loop distinguishes between the controls mode, and when the arm mode is active, it will parse the value of the joysticks for moving the arm, and of the digital pad for moving the gripper:

```python
def main_loop(gamepad)
  #...
  try:
    while gamepad.isConnected():
      #...
      if (mode == MOVE_MODE):
        # ...

      if (mode == ARM_MODE):
        gripper_x = gamepad.axis(digital_x)
        gripper_y = -gamepad.axis(digital_y)
        
        if (gripper_x != 0.0):
          arm.move_incr(SERVO_G, 4*int(gripper_x))
          log_to_stdout_and_mqtt("X {}, Y {}".format(gripper_x, gripper_y))
        if (gripper_y != -0.0):
          arm.move_incr(SERVO_Z, 4*int(gripper_y ))
          log_to_stdout_and_mqtt("X {}, Y {}".format(gripper_x, gripper_y ))

        arm_x = gamepad.axis(joystickSteering)
        arm_y = -gamepad.axis(joystickSpeed)
        
        if (arm_x != 0.0):
          arm.move_incr(SERVO_X, int(100*arm_x)/4)
          log_to_stdout_and_mqtt("SERVO_X {} => {}".format(arm_x, int(100*arm_x)/4))
        if (arm_y != -0.0):
          arm.move_incr(SERVO_Y, int(100*arm_y)/4)
          log_to_stdout_and_mqtt("SERVO_Y {} => {}".format(arm_y,int(100*arm_y)/4))

        if gamepad.beenPressed(button_triangle):
          arm.center()
                    
      time.sleep(pollInterval)
  finally:
    # Ensure the background thread is always terminated when we are done
    gamepad.disconnect()
```

## Conclusion

This article showed how to use the Python library [Piborg Gamepad](https://github.com/piborg/Gamepad). You learned how to define the button layout, how to start a program with waiting for a connected gamepad, and how to continuously check the state of button, the digital pad and joysticks. Putting all of this together is a small and readable script that you can customize to the requirements or your robot. The article showed concretely how to use this library for controlling the wheel movements and the arm movements of my robot RADU. In the next and final article, I will summarize my journey of developing a custom robot.