Balboa 32U4 Balancing Robot Kit for Raspberry Pi

/*
This code demonstrates how to use the Balboa 32U4 Balancing Robot Kit for Raspberry Pi to balance the robot using a combination of a compass sensor and a gyro sensor. The Balboa32U4, LSM303, and L3G libraries are used to control the motors and read sensor data.

To use this code, assemble the Balboa 32U4 Balancing Robot Kit according to the instructions provided in the product documentation, and connect it to your Raspberry Pi using the provided USB cable. The compass and gyro sensors should be connected to the I2C bus of the Balboa 32U4 board according to the wiring diagram provided in the product documentation. The code reads the sensor data using the read() function of the LSM303 and L3G libraries, and calculates the heading angle and angular velocity. It then adjusts the motor speeds using the setSpeeds() function of the Balboa32U4Motors library based on the orientation of the robot and the angular velocity. The LED pin is also toggled every second to indicate the program is running.

Note that balancing a robot is a complex task that requires careful tuning of the motor control algorithm and calibration of the sensors. The code provided here is a basic example that may need to be modified and optimized depending on the characteristics of your robot and the environment it operates in.
*/

#include <Wire.h>
#include <LSM303.h>
#include <L3G.h>
#include <Balboa32U4.h>

Balboa32U4Motors motors;
LSM303 compass;
L3G gyro;

const int led_pin = 13; // Pin connected to the onboard LED

void setup() {
  Serial.begin(9600); // Initialize the serial communication
  motors.setSpeeds(0, 0); // Set both motors to 0 speed
  pinMode(led_pin, OUTPUT); // Set the LED pin as an output
  compass.init(); // Initialize the compass sensor
  gyro.init(); // Initialize the gyro sensor
  gyro.enableDefault(); // Enable the gyro sensor with default settings
}

void loop() {
  // Read the sensor data
  compass.read();
  gyro.read();

  // Calculate the heading angle
  float heading = compass.heading();
  heading = heading < 0 ? heading + 360 : heading; // Convert negative heading to positive

  // Calculate the angular velocity
  float angular_velocity = gyro.g.z;

  // Print the sensor data to the serial monitor
  Serial.print("Heading: ");
  Serial.print(heading);
  Serial.print(" degrees, Angular Velocity: ");
  Serial.print(angular_velocity);
  Serial.println(" dps");

  // Check the orientation of the robot and adjust the motor speeds accordingly
  if (heading >= 45 && heading <= 315) {
    // Robot is tilted forwards or backwards, adjust motor speeds to balance
    int speed = constrain(map(angular_velocity, -1000, 1000, -400, 400), -400, 400);
    motors.setSpeeds(speed, speed);
  } else {
    // Robot is upright, stop the motors
    motors.setSpeeds(0, 0);
  }

  // Toggle the LED every second
  digitalWrite(led_pin, millis() % 1000 < 500 ? HIGH : LOW);
}