Self-Balancing Robot with MatriXDeck

Introduction

Turn your MatriXDeck into an impressive self-balancing robot! This project combines the built-in MPU6050 motion sensor with the Balance Robot Kit to create a robot that can stand upright on two wheels, respond to tilt commands for movement, and navigate its environment.

Self-balancing robots are an excellent way to learn about:

• Control theory and PID controllers
• Sensor fusion algorithms
• Real-time control systems
• Robotics mechanics and motor control

This project is perfect for those interested in robotics and control systems, providing a hands-on way to explore these concepts with a visually impressive result.

Materials Needed

• MatriXDeck V1
• MatriXDeck Balance Robot Kit
• Small Phillips screwdriver
• USB-C cable for programming and initial testing

Project Overview

We'll transform the MatriXDeck into a self-balancing robot with these capabilities:

• Maintaining balance on two wheels
• Responding to tilt for forward/backward/turning movement
• Displaying status and debug information on the LED matrix
• Optional: obstacle avoidance with the Sensor Pack (ultrasonic sensor)

Step-by-Step Instructions

1. Assembling the Robot Kit

Before programming, you'll need to assemble the Balance Robot Kit:

1. Attach the motor mounts to the chassis base
2. Install the DC motors into the mounts
3. Connect the motor controller board to the motors
4. Attach the wheels to the motor shafts
5. Mount the MatriXDeck bracket to the chassis
6. Secure your MatriXDeck to the bracket
7. Connect the motor controller to the MatriXDeck GPIO pins

Refer to the assembly guide included with the Balance Robot Kit for detailed instructions with diagrams.

2. Setting Up the Libraries and Constants

Now let's set up our code with the necessary libraries and constants:

#include <MatriXDeck.h>
#include <Wire.h>
#include "MPU6050_tockn.h"

MatriXDeck matrix;
MPU6050 mpu6050(Wire);

// Motor controller pins
#define MOTOR_A_IN1 25
#define MOTOR_A_IN2 26
#define MOTOR_B_IN1 27
#define MOTOR_B_IN2 14
#define MOTOR_A_PWM 16
#define MOTOR_B_PWM 17

// PWM properties
#define PWM_FREQ 1000
#define PWM_RES 8
#define PWM_A_CHANNEL 0
#define PWM_B_CHANNEL 1

// PID controller constants
#define SAMPLE_TIME 10  // milliseconds
float Kp = 30.0;   // Proportional gain
float Ki = 150.0;  // Integral gain
float Kd = 0.8;    // Derivative gain

// Variables for storing angle and PID control
float targetAngle = 0.0;
float currentAngle, lastAngle;
float error, lastError, cumError, rateError;
float pidOutput;

// Timing variables
unsigned long lastTime;

3. Setting Up the Motors

Let's set up the motor control functions:

void setupMotors() {
  // Configure motor control pins
  pinMode(MOTOR_A_IN1, OUTPUT);
  pinMode(MOTOR_A_IN2, OUTPUT);
  pinMode(MOTOR_B_IN1, OUTPUT);
  pinMode(MOTOR_B_IN2, OUTPUT);
  
  // Configure PWM channels
  ledcSetup(PWM_A_CHANNEL, PWM_FREQ, PWM_RES);
  ledcSetup(PWM_B_CHANNEL, PWM_FREQ, PWM_RES);
  ledcAttachPin(MOTOR_A_PWM, PWM_A_CHANNEL);
  ledcAttachPin(MOTOR_B_PWM, PWM_B_CHANNEL);
  
  // Initially stop motors
  stopMotors();
}

void setMotors(int speedA, int speedB) {
  // Constrain speeds to valid PWM range
  speedA = constrain(speedA, -255, 255);
  speedB = constrain(speedB, -255, 255);
  
  // Set motor A direction and speed
  if (speedA > 0) {
    digitalWrite(MOTOR_A_IN1, HIGH);
    digitalWrite(MOTOR_A_IN2, LOW);
    ledcWrite(PWM_A_CHANNEL, speedA);
  } else if (speedA < 0) {
    digitalWrite(MOTOR_A_IN1, LOW);
    digitalWrite(MOTOR_A_IN2, HIGH);
    ledcWrite(PWM_A_CHANNEL, -speedA);
  } else {
    digitalWrite(MOTOR_A_IN1, LOW);
    digitalWrite(MOTOR_A_IN2, LOW);
    ledcWrite(PWM_A_CHANNEL, 0);
  }
  
  // Set motor B direction and speed
  if (speedB > 0) {
    digitalWrite(MOTOR_B_IN1, HIGH);
    digitalWrite(MOTOR_B_IN2, LOW);
    ledcWrite(PWM_B_CHANNEL, speedB);
  } else if (speedB < 0) {
    digitalWrite(MOTOR_B_IN1, LOW);
    digitalWrite(MOTOR_B_IN2, HIGH);
    ledcWrite(PWM_B_CHANNEL, -speedB);
  } else {
    digitalWrite(MOTOR_B_IN1, LOW);
    digitalWrite(MOTOR_B_IN2, LOW);
    ledcWrite(PWM_B_CHANNEL, 0);
  }
}

void stopMotors() {
  digitalWrite(MOTOR_A_IN1, LOW);
  digitalWrite(MOTOR_A_IN2, LOW);
  digitalWrite(MOTOR_B_IN1, LOW);
  digitalWrite(MOTOR_B_IN2, LOW);
  ledcWrite(PWM_A_CHANNEL, 0);
  ledcWrite(PWM_B_CHANNEL, 0);
}

4. Implementing the PID Controller

Now let's implement the core PID controller:

float computePID() {
  // Calculate time elapsed since last computation
  unsigned long currentTime = millis();
  float elapsedTime = (currentTime - lastTime) / 1000.0;  // Convert to seconds
  
  // Only update at the specified sample rate
  if (elapsedTime < SAMPLE_TIME / 1000.0) {
    return pidOutput;  // Return last output if not time yet
  }
  
  // Get current angle from MPU6050
  mpu6050.update();
  currentAngle = mpu6050.getAngleX();  // Use X-axis angle for balance
  
  // Calculate error terms
  error = currentAngle - targetAngle;
  cumError += error * elapsedTime;  // Integral term
  rateError = (error - lastError) / elapsedTime;  // Derivative term
  
  // Apply some constraints to prevent integral windup
  cumError = constrain(cumError, -20, 20);
  
  // Compute PID output
  pidOutput = Kp * error + Ki * cumError + Kd * rateError;
  
  // Store values for next iteration
  lastError = error;
  lastTime = currentTime;
  
  return pidOutput;
}

5. Main Setup and Loop Functions

Now, let's implement the main setup and loop functions:

void setup() {
  // Initialize MatriXDeck
  matrix.begin();
  
  // Initialize I2C communication
  Wire.begin();
  
  // Initialize MPU6050
  mpu6050.begin();
  
  // Display startup message
  matrix.clear();
  matrix.print("MPU", 0, 0, GREEN);
  matrix.print("CALIB", 0, 8, GREEN);
  matrix.update();
  
  // Calibrate the MPU6050 (keep robot stationary)
  mpu6050.calcGyroOffsets(true);
  
  // Initialize motors
  setupMotors();
  
  // Display ready message
  matrix.clear();
  matrix.print("READY", 5, 4, GREEN);
  matrix.update();
  delay(1000);
  
  // Initialize PID variables
  lastTime = millis();
  lastError = 0;
  cumError = 0;
}

void loop() {
  // Compute PID output for balancing
  float output = computePID();
  
  // Apply motor control
  // If angle is too far from vertical, stop motors for safety
  if (abs(currentAngle) > 30) {
    stopMotors();
    
    // Display fallen message
    matrix.clear();
    matrix.print("FALLEN", 3, 4, RED);
    matrix.update();
  } else {
    // Apply PID output to motors
    // Adding turning component based on joystick input
    int joystickX = analogRead(36) - 2048;  // Center around zero
    int turnAmount = map(joystickX, -2048, 2048, -30, 30);
    
    // Only apply turning if joystick moved significantly
    if (abs(joystickX) < 300) {
      turnAmount = 0;
    }
    
    setMotors(output - turnAmount, output + turnAmount);
    
    // Display angle on LED matrix
    matrix.clear();
    
    // Draw horizon line based on tilt
    int horizonY = map(currentAngle, -30, 30, 0, 15);
    matrix.drawLine(0, horizonY, 31, horizonY, GREEN);
    
    // Draw angle value
    char angleText[6];
    sprintf(angleText, "%d.%d", (int)currentAngle, (int)(abs(currentAngle * 10) - abs((int)currentAngle) * 10));
    matrix.print(angleText, 0, 0, YELLOW);
    
    matrix.update();
  }
}

6. Tuning the PID Controller

PID tuning is critical for good balance. Start with these values and adjust based on performance:

// Start with conservative values
float Kp = 30.0;   // Proportional gain
float Ki = 150.0;  // Integral gain
float Kd = 0.8;    // Derivative gain

If the robot:

• Oscillates rapidly: Decrease Kp or increase Kd
• Falls too easily: Increase Kp
• Drifts slowly: Increase Ki
• Responds too aggressively: Decrease Kp and possibly Ki

Troubleshooting

Robot won't balance at all

• Check motor connections and direction
• Make sure the MPU6050 is mounted correctly (X-axis aligned with forward direction)
• Verify the center of gravity (may need weight adjustments)

Robot oscillates rapidly

• Decrease Kp or increase Kd
• Check for mechanical issues (loose connections, wheel alignment)

Robot drifts in one direction

• Recalibrate the MPU6050 on a level surface
• Adjust targetAngle slightly (±0.5°) to compensate for weight distribution

Motors not responding

• Check all wiring connections
• Verify GPIO pin assignments
• Check battery level

Extending the Project

Here are some ideas to enhance your self-balancing robot:

1. Obstacle Avoidance: Add the ultrasonic sensor from the Sensor Pack
2. Remote Control: Add Bluetooth control via a smartphone app
3. Path Following: Implement line-following with IR sensors
4. Data Logging: Record performance data to optimize PID values
5. Dynamic Displays: Create informative displays on the LED matrix

Conclusion

Congratulations! You've built a self-balancing robot using your MatriXDeck and the Balance Robot Kit. This project demonstrates advanced concepts in control systems and robotics, providing both an educational experience and an impressive demonstration piece.

The self-balancing robot is a complex project that brings together multiple disciplines—mechanics, electronics, and software. As you fine-tune the PID controller and experiment with additional features, you'll gain deeper insights into how these systems work together.

Remember that PID tuning is often an iterative process requiring patience and experimentation. Each adjustment brings you closer to optimal performance, and the skills you develop here apply to many other robotics and control system projects.

---

Share your balanced robot videos with us on our Discord community or tag us on social media with #MatriXDeckRobot!