Week 6
Sensors and Input Devices
Initially, I had the idea to create a snake game controlled by buttons connected to an Arduino board. I made a working sketch in C++ using SDL, but later I thought using buttons would be too simple. Getting familiar with a more complicated sensor seemed more interesting. So, I started adapting the usual snake game to be played with a gyroscope.
Using it just to translate movements up, down, right, and left would be too boring, so I wanted the snake to move in any direction. I thought making the board on which the snake moves tilt at the same angle as the gyro would be more interesting, but at that point, having a snake didn’t make sense. So, I decided to use a ball instead. The final idea was a ball moving through a labyrinth controlled via a gyroscope. I invited my friend from a different specialization to help me write the game in Unity, and I would be responsible for the data from the MPU-6050.
MPU-6050
I didn’t want to use a premade library at first, so I could understand how it works. I followed a video that used the same sensor for a drone application, utilizing the Kalman Filter to combine data from the accelerometer and gyroscope. Somehow, the exact code from the video didn’t work for me, so I rewrote some parts of it myself, especially the KF part. It did work, although it couldn’t measure roll, only yaw and pitch, and both of those only in the range of -90 to 90 degrees, with much worse accuracy near the ends of that interval. In the end, for the final program, I used a library by Jeff Rowberg called I2CDev, which used a processing unit on the sensor itself and returned quaternion data from the sensor. Unity, of course, has great support for quaternions, so it was an easy choice there.
Everything was done using ESP32. You can see the schematic for the connection here (the button is only needed for my version of the code):
Connection schematic for MPU-6050
MPU6050 Connections:
- (VCC) -> (3.3V)
- (GND) -> (GND)
- (SCL) -> GPIO 22
- (SDA) -> GPIO 21
- (INT) -> GPIO 2
Button Connections:
- One side of the button -> (3.3V)
- Other side of the button -> (GND) through a 10kΩ resistor and GPIO 13
#include "Wire.h"
const int I2C_ADDRESS = 0x68;
const int CONFIG_REGISTER = 0x1A;
const int DLPF_SETTING = 0x05; // 10Hz cutoff low pass filter
const int GYRO_CONFIG_REGISTER = 0x1B;
const int SENSITIVITY_SETTING = 0x08; // +- 500dps
const float GYRO_LSB_SENSITIVITY = 65.5; // for 500dps
const int ACCEL_CONFIG_REGISTER = 0x1C;
const int ACCEL_OUTPUT_SCALE = 0x00; // +- 2g
const int ACCEL_LSB_SENSITIVITY = 16384; // for 2g
const int GYRO_MEASUREMENT_REGISTER = 0x43;
const int ACCEL_MEASUREMENT_REGISTER = 0x3B;
const int PWR_MGMT_1 = 0x6B;
const int BAUD_RATE = 115200;
const int CALIBRATION_ITERATIONS = 1000;
const int BUTTON_PIN = 13;
float rateRoll, ratePitch, rateYaw;
float rateBiasRoll, rateBiasPitch, rateBiasYaw;
float accX, accY, accZ;
float angleRoll, anglePitch;
float angleBiasRoll, angleBiasPitch;
unsigned long lastMeasurementTime = 0;
float dt;
class KalmanFilter {
public:
KalmanFilter() : angle(0.0f), bias(0.0f), rate(0.0f) {
P[0][0] = 0.0f; P[0][1] = 0.0f;
P[1][0] = 0.0f; P[1][1] = 0.0f;
}
void setAngle(float newAngle) {
angle = newAngle;
}
void setBias(float newBias) {
bias = newBias;
}
float getAngle() const {
return angle;
}
float getBias() const {
return bias;
}
float getRate() const {
return rate;
}
void resetErrorCovariance() {
P[0][0] = 1.0f; // Estimate of error in the angle
P[0][1] = 0.0f;
P[1][0] = 0.0f;
P[1][1] = 1.0f; // Estimate of error in the bias
}
float process(float newAngle, float newRate, float dt) {
// Predict
rate = newRate - bias;
angle += dt * rate;
P[0][0] += dt * (dt*P[1][1] - P[0][1] - P[1][0] + Q_angle);
P[0][1] -= dt * P[1][1];
P[1][0] -= dt * P[1][1];
P[1][1] += Q_bias * dt;
// Measurement update
float S = P[0][0] + R_measure;
float K[2];
K[0] = P[0][0] / S;
K[1] = P[1][0] / S;
float y = newAngle - angle;
angle += K[0] * y;
bias += K[1] * y;
float P00_temp = P[0][0];
float P01_temp = P[0][1];
P[0][0] -= K[0] * P00_temp;
P[0][1] -= K[0] * P01_temp;
P[1][0] -= K[1] * P00_temp;
P[1][1] -= K[1] * P01_temp;
return angle;
}
private:
float Q_angle = 0.001f;
float Q_bias = 0.003f;
float R_measure = 0.03f;
float angle, bias, rate;
float P[2][2];
};
KalmanFilter rollKalman;
KalmanFilter pitchKalman;
void waitForButtonPress() {
while (digitalRead(BUTTON_PIN) == LOW) {
delay(20); // Short delay to debounce
}
while (digitalRead(BUTTON_PIN) == HIGH) {
delay(20); // Short delay to debounce
}
}
void gyro_signals(void) {
unsigned long currentMeasurementTime = micros();
dt = (currentMeasurementTime - lastMeasurementTime) / 1000000.0;
lastMeasurementTime = currentMeasurementTime;
// read accelerometer
Wire.beginTransmission(I2C_ADDRESS);
Wire.write(ACCEL_MEASUREMENT_REGISTER);
Wire.endTransmission();
Wire.requestFrom(I2C_ADDRESS, 6);
int16_t accXLSB = Wire.read() << 8 | Wire.read();
int16_t accYLSB = Wire.read() << 8 | Wire.read();
int16_t accZLSB = Wire.read() << 8 | Wire.read();
// read gyroscope
Wire.beginTransmission(I2C_ADDRESS);
Wire.write(GYRO_MEASUREMENT_REGISTER);
Wire.endTransmission();
Wire.requestFrom(I2C_ADDRESS, 6);
int16_t gyroX = Wire.read() << 8 | Wire.read();
int16_t gyroY = Wire.read() << 8 | Wire.read();
int16_t gyroZ = Wire.read() << 8 | Wire.read();
rateRoll = (float)gyroX / GYRO_LSB_SENSITIVITY;
ratePitch = (float)gyroY / GYRO_LSB_SENSITIVITY;
rateYaw = (float)gyroZ / GYRO_LSB_SENSITIVITY;
accX = (float)accXLSB / ACCEL_LSB_SENSITIVITY;
accY = (float)accYLSB / ACCEL_LSB_SENSITIVITY;
accZ = (float)accZLSB / ACCEL_LSB_SENSITIVITY;
float gravityMagnitude = sqrt(accX*accX + accY*accY + accZ*accZ);
accX /= gravityMagnitude;
accY /= gravityMagnitude;
accZ /= gravityMagnitude;
angleRoll = atan2(accY, sqrt(accX*accX + accZ*accZ)) * (180.0 / PI);
anglePitch = -atan2(accX, sqrt(accY*accY + accZ*accZ)) * (180.0 / PI);
}
void calibrateAngles(){
Serial.println("Place your hand into a starting position and
don't move until the progress bar is finished seconds");
Serial.println("Press the button to start calibration");
waitForButtonPress();
Serial.println("Calibrating...");
const int progressBarWidth = 10; // Width of the progress bar in characters
int lastProgress = -1;
for (int i = 0; i < CALIBRATION_ITERATIONS; i++ ){
gyro_signals();
angleBiasRoll += angleRoll;
angleBiasPitch += anglePitch;
rateBiasRoll += rateRoll;
rateBiasPitch += ratePitch;
rateBiasYaw += rateYaw;
int progressPercent = (i * 100) / CALIBRATION_ITERATIONS;
int currentProgress = (progressPercent * progressBarWidth) / 100;
if (currentProgress != lastProgress) {
Serial.print("[");
for (int j = 0; j < progressBarWidth; j++) {
if (j < currentProgress) {
Serial.print("#");
} else {
Serial.print(".");
}
}
Serial.print("] ");
Serial.print(progressPercent);
Serial.println("%");
lastProgress = currentProgress;
}
delay(1);
}
// Final print to ensure the progress bar shows complete at the end
Serial.print("[");
for (int j = 0; j < progressBarWidth; j++) Serial.print("#");
Serial.println("] 100%");
angleBiasRoll /= (float)CALIBRATION_ITERATIONS;
angleBiasPitch /= (float)CALIBRATION_ITERATIONS;
rateBiasRoll /= (float)CALIBRATION_ITERATIONS;
rateBiasPitch /= (float)CALIBRATION_ITERATIONS;
rateBiasYaw /= (float)CALIBRATION_ITERATIONS;
rollKalman.setAngle(angleBiasRoll);
rollKalman.setBias(rateBiasRoll);
rollKalman.resetErrorCovariance();
pitchKalman.setAngle(angleBiasPitch);
pitchKalman.setBias(rateBiasPitch);
pitchKalman.resetErrorCovariance();
Serial.println("Calibration Values:");
Serial.print("Roll Calibration Accel: ");
Serial.println(angleBiasRoll, 6);
Serial.print("Pitch Calibration Accel: ");
Serial.println(angleBiasPitch, 6);
Serial.print("Roll Calibration Gyro: ");
Serial.println(rateBiasRoll, 6);
Serial.print("Pitch Calibration Gyro: ");
Serial.println(rateBiasPitch, 6);
Serial.print("Yaw Calibration Gyro: ");
Serial.println(rateBiasYaw, 6);
Serial.println("Press a button to start transfering data");
waitForButtonPress();
}
void setup() {
Serial.begin(BAUD_RATE);
pinMode(BUTTON_PIN, INPUT);
Wire.setClock(400000);
Wire.begin();
delay(250);
Wire.beginTransmission(I2C_ADDRESS);
Wire.write(PWR_MGMT_1);
Wire.write(0x00);
Wire.endTransmission();
// configure dlpf
Wire.beginTransmission(I2C_ADDRESS);
Wire.write(CONFIG_REGISTER);
Wire.write(DLPF_SETTING);
Wire.endTransmission();
// configure accelerometer
Wire.beginTransmission(I2C_ADDRESS);
Wire.write(ACCEL_CONFIG_REGISTER);
Wire.write(ACCEL_OUTPUT_SCALE);
Wire.endTransmission();
// configure gyroscope
Wire.beginTransmission(I2C_ADDRESS);
Wire.write(GYRO_CONFIG_REGISTER);
Wire.write(SENSITIVITY_SETTING);
Wire.endTransmission();
calibrateAngles();
}
void loop() {
// Detect button press with debounce
if (digitalRead(BUTTON_PIN) == HIGH) {
delay(20); // Debounce delay
if (digitalRead(BUTTON_PIN) == HIGH) {
Serial.println("Button pressed. Recalibrating...");
calibrateAngles();
}
}
gyro_signals();
angleRoll = rollKalman.process(angleRoll, rateRoll, dt);
anglePitch = pitchKalman.process(anglePitch, ratePitch, dt);
Serial.print(angleRoll - angleBiasRoll, 6);
Serial.print(" ");
Serial.print(anglePitch - angleBiasPitch, 6);
Serial.println();
delay(5);
}
My code to read Euler angles from MPU-6050
And here’s a link to the library I used in the final version: GitHub jrowberg MPU6050
Unity
Creating an environment and adding a labyrinth model was very simple and easy. Unfortunately, my friend who wanted to help me didn’t have much time, but at least he found a way to connect a serial port to the Unity engine, which was really complicated, and I’m thankful I didn’t need to do that.
The worst part of all of it was actually making an intuitive translation of the motion of a gyro sewn to a glove to the motion of the in-game ball. We had a circular model of the labyrinth, and the ball moved around it. The camera followed the ball by spinning around the center of the labyrinth and tilting to adjust for the ball getting away from the center. That worked out great. The ball was to move toward or away from the center if the glove was tilted forwards or backwards and go around the center to the left or right if the glove was turned to the side.
That control model was the worst decision of this project. A human hand doesn’t have even angles it can move in symmetrical directions, so the speed of the ball should have been adjusted for that, or even worse, for each hand specifically. I decided to do that using calibration. So, there are instructions on the screen to turn your hand into certain positions and press the space bar after that. The game would record 5 quaternions from that calibration: CENTER, UP, DOWN, RIGHT, LEFT. Then it would somehow interpolate between them so the center would give 0 and any combination of others would give the max speed in that direction. Interpolating something like that, especially from a sphere, isn’t easy. Here, your hand doesn’t even make the gyro move around a perfect sphere; it moves very differently depending on the direction. In the end, some versions of that control worked okay but never really well.
Posting C# scripting files would take too much space, so here’s a link to download an archive with the compiled game.
And here’s video demo of playing it: