I am in the process of building a correction wheel attached to a 6v dc motor. The purpose is to counteract/stabilize rotational movement of a box that is suspended by a string in the center. I have a gyro mpu6050 that I use the raw gyro data and a TB6612FNG stepper motor controller. There is an external power supply to the TB6612FNG in order to power the motor and not bog down the arduino. The mpu6050 is on 3.3V with two 4.7k pullups on the sda and scl. The issue I am having is that when I run this code, the serial monitor outputs and runs the motor/correction wheel for 40-50 seconds perfectly. This runtime is affected by the aggressiveness in which I rotate the gyro. By aggressively rotating the gyro, the serial monitor cuts out almost instantly and comes to a stop and the motor will either stop or chase the last output the monitor shows. After trying capacitors, steady power supply, different motors, I came to a possibility that it was the mpu6050 cutting out (getting interrupted) by a noisy motor and wires. So I went on a quest of covering the scl, sda, and motor wires in EMI tape and adding a ferrite bead to each. Additionally I added 0.1 µF capacitor across the motor terminals. I am still getting the same freezing of the monitor and mpu6050. The variables I have been playing with is the pwm of the motor, normally 255 at full speed, and the Wire.setClock. Changing these two variables has helped marginally. Additionally I have a few filters to smooth the gyroscope and reaction so its not so twitchy. This version does not have a PID controller due to the current issues. I am new to arduino so I am sure there are issues with my code. I am looking for any input to help and figure out what is causing this interference / freezing.
#include <Wire.h>
// TB6612FNG motor pins
const int PWMA = 6; // PWM control
const int AIN1 = 4; // Motor input 1
const int AIN2 = 5; // Motor input 2
// MPU6050 I2C address
const int MPU_ADDR = 0x68; // MPU6050 default
// Control parameters
const float alpha_gyro = 0.8; // Filter smoothing factor for gyro data (0 = raw, 1 = fully smoothed)
const float Krate = 7.0; // Gain that converts gyro rate to motor PWM output
const float gyro_deadband = 2.0; // Ignore tiny rotations (°/s).
const float motor_smooth = 0.7; // Smooths sudden PWM changes to prevent jerky motion (0–1)
const float MOTOR_MAX_RPM = 5000.0; // Motor rated speed (RPM). Rated speed of motor for display purposes only
// Gyro variables
float gyroZ_zero = 0; // Stores the gyro’s zero offset (calibration baseline)
float gyroZ_filtered = 0; // Stores the smoothed Z-axis rotation rate
float motorPWM_prev = 0; // Keeps previous PWM value for smoothing transitions
unsigned long prevTime = 0; // Used to calculate time difference between loops
// Setup
void setup() {
Serial.begin(115200);
Wire.begin(); // Initialize I2C bus
Wire.setClock(80000); // 100 kHz standard speed
// Motor pins
pinMode(PWMA, OUTPUT);
pinMode(AIN1, OUTPUT);
pinMode(AIN2, OUTPUT);
// Wake up MPU6050
Wire.beginTransmission(MPU_ADDR);
Wire.write(0x6B); // PWR_MGMT_1 Register: Power Management 1
Wire.write(0); // Set to 0 to wake up the sensor
Wire.endTransmission(true);
delay(100); // Give sensor time to stabilize
Serial.println("MPU6050 & Motor Controller Initialized");
// Gyro calibration
Serial.println("Calibrating gyro... Keep device still.");
long sum = 0;
const int samples = 1000; // Number of readings for averaging offset
for (int i = 0; i < samples; i++) {
sum += readRawGyroZ(); // Read raw Z-axis data from sensor
delay(2); // Small delay between samples
}
gyroZ_zero = sum / (float)samples; // Compute average offset
Serial.print("Gyro zero offset: "); Serial.println(gyroZ_zero, 2); // Print offset value
Serial.println("Calibration complete.\n");
prevTime = millis(); // Start timing for control loop
}
// Read raw gyro Z
int16_t readRawGyroZ() {
Wire.beginTransmission(MPU_ADDR);
Wire.write(0x47); // Address of GYRO_ZOUT_H register (high byte of Z-axis gyro data)
Wire.endTransmission(false); // Keep connection active
Wire.requestFrom(MPU_ADDR, 2, true); // Request 2 bytes (high + low)
int16_t gz = Wire.read() << 8 | Wire.read(); // Combine bytes into a single 16-bit value
return gz; // Return raw Z-axis gyro reading
}
// Main loop
void loop() { // Calculate time elapsed since last loop iteration
unsigned long currentTime = millis();
float dt = (currentTime - prevTime) / 1000.0; // Convert milliseconds to seconds
prevTime = currentTime;
// Read and process gyro
float gyroZ = (readRawGyroZ() - gyroZ_zero) / 131.0; // Convert raw data to °/s (131 LSB per °/s)
gyroZ_filtered = alpha_gyro * gyroZ_filtered + (1 - alpha_gyro) * gyroZ; // Apply exponential smoothing filter
// Motor control
float motorPWMf = Krate * gyroZ_filtered; // Scale gyro rate to motor command
// Deadband to ignore tiny movements (reduce noise and vibration)
if (abs(gyroZ_filtered) < gyro_deadband) motorPWMf = 0;
// Smooth motor PWM changes
motorPWMf = motor_smooth * motorPWM_prev + (1 - motor_smooth) * motorPWMf;
motorPWM_prev = motorPWMf;
// Limit PWM output to a safe range
int motorPWM = constrain((int)motorPWMf, -100, 100);
// Motor direction
String direction;
if (motorPWM > 0) {
direction = "CW"; // Clockwise rotation
digitalWrite(AIN1, HIGH); // Set motor direction pins
digitalWrite(AIN2, LOW);
analogWrite(PWMA, motorPWM); // Send PWM value for speed control
} else if (motorPWM < 0) {
direction = "CCW"; // Counter-clockwise rotation
digitalWrite(AIN1, LOW);
digitalWrite(AIN2, HIGH);
analogWrite(PWMA, -motorPWM); // Invert PWM for opposite direction
} else {
direction = "STOP"; // Stop motor
digitalWrite(AIN1, LOW);
digitalWrite(AIN2, LOW);
analogWrite(PWMA, 0);
}
// Motor RPM
float motorRPM = abs(motorPWM) / 255.0 * MOTOR_MAX_RPM; // Scale PWM to approximate RPM
// Telemetry
Serial.print("Gyro Z: "); Serial.print(gyroZ_filtered, 2);
Serial.print("°/s | Motor Dir: "); Serial.print(direction);
Serial.print(" | Motor PWM: "); Serial.print(abs(motorPWM));
Serial.print(" | Motor RPM: "); Serial.println(motorRPM, 0);
delay(50); // Run loop at ~20 Hz update rate
}
