I have been having problems with controlling the MPU6050 IMU and two Servos alongside it to create a thrust vector controlling rocket.
Basically when I use the MPU6050 the Servos will not move. They will only make a buzzing sound and slightly vibrate. Not sure if this is a power problem or something with the code.
Thanks.
Post your code and explain your problems
Me neither.
/*
Description: Model Rocket motor gimbal using 2 servo. This is yet again another attempt to fly a model rocket without fins using long burn motors.
It is using the Arduino PID controler to move a rocket motor.
Angle changes can be monitored using a USB cable or a bluetooth interface
Inspired by various camera gimbal projects
Sensor used is an MPU6050 board
Servo Connection
BROWN - gnd
red - 5v either on extension prototyping board or on original arduino board for UNO.
yellow - D10 (pwm for Sero X)
- D11 (servo Y)
MPU6050 board Connections
VCC - 5v
GND - GND
SCL - A5 or pin SCL on the stm32
SDA - A4 or pin SDA on the stm32
INT - D2 (not used)
LINE 80 = PID tuning for both x perameters and y parameters
*/
#include <Servo.h> //servo library
#include <I2Cdev.h>
#include <MPU6050_6Axis_MotionApps20.h> // Gyroscope and axcelerometer libraries
#include <PID_v1.h> // Arduino PID library
#include <Wire.h>
#define LED_PIN 13 //pin 13 for the arduino Uno and PC13 for the stm32
bool blinkState = true;
Servo ServoX; // X axis Servo
Servo ServoY; // Y axis Servo
float mpuPitch = 0;
float mpuRoll = 0;
float mpuYaw = 0;
// Create MPU object
// class default I2C address is 0x68; specific I2C addresses may be passed as a parameter here
// for exemple MPU6050 mpu(0x69);
MPU6050 mpu;
// MPU control/status vars
uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount; // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer
// orientation/motion vars
Quaternion q; // [w, x, y, z] quaternion container
VectorFloat gravity; // [x, y, z] gravity vector
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
// relative ypr[x] usage based on sensor orientation when mounted, e.g. ypr[PITCH]
#define PITCH 1 // defines the position within ypr[x] variable for PITCH; may vary due to sensor orientation when mounted
#define ROLL 2 // defines the position within ypr[x] variable for ROLL; may vary due to sensor orientation when mounted
#define YAW 0 // defines the position within ypr[x] variable for YAW; may vary due to sensor orientation when mounted
// PID's
//Define Variables we'll be connecting to
double SetpointX, InputX, OutputX;
double SetpointY, InputY, OutputY;
// Those initial tuning parameters need to be tuned
// this a bit like when you tune your copter apart from the fact that the rocket motor last only few seconds
// PID TUNING HERE!!!
//double KpX = 2, KiX = 5, KdX = 1;
double KpX = 3.55, KiX = 0.005, KdX = 2.05;
//Specify the links and initial tuning parameters
//double KpY = 2, KiY = 5, KdY = 1;
double KpY = 3.55, KiY = 0.005, KdY = 2.05;
PID myPIDX(&InputX, &OutputX, &SetpointX, KpX, KiX, KdX, DIRECT);
PID myPIDY(&InputY, &OutputY, &SetpointY, KpY, KiY, KdY, DIRECT);
//calibration stuff
//Change those 3 variables if you want to fine tune the skecth to your needs.
int buffersize = 200; //Amount of readings used to average, make it higher to get more precision but sketch will be slower (default:1000)
int acel_deadzone = 8; //Acelerometer error allowed, make it lower to get more precision, but sketch may not converge (default:8)
int giro_deadzone = 1; //Giro error allowed, make it lower to get more precision, but sketch may not converge (default:1)
int16_t ax, ay, az, gx, gy, gz;
int mean_ax, mean_ay, mean_az, mean_gx, mean_gy, mean_gz, state = 0;
int ax_offset, ay_offset, az_offset, gx_offset, gy_offset, gz_offset;
/*
Initial setup
do the board calibration
if you use the calibration function do not move the board until the calibration is complete
*/
void setup()
{
ServoX.attach(10); // attaches the X servo on PA1 for stm32 or D10 for the Arduino Uno
ServoY.attach(11); // attaches the Y servo on PA2 for stm32 or D11 for the Arduino Uno
// set both servo's to 90 degree
ServoX.write(90);
ServoY.write(90);
delay(500);
Wire.begin();
Serial.begin(230400);
while (!Serial); // wait for Leonardo enumeration, others continue immediately
// initialize device
Serial.println(F("Initializing MPU 6050 device..."));
mpu.initialize();
// verify connection
Serial.println(F("Testing device connections..."));
Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
// load and configure the DMP
Serial.println(F("Initializing DMP"));
devStatus = mpu.dmpInitialize();
// INPUT CALIBRATED OFFSETS HERE; SPECIFIC FOR EACH UNIT AND EACH MOUNTING CONFIGURATION!!!!
// use the calibrate function for yours
// you can also write down your offset ans use them so that you do not have to re-run the calibration
ax_offset = 1118;
ay_offset = 513;
az_offset = 1289;
gx_offset = 64;
gy_offset = -1;
gz_offset = -33;
// configure LED for output
pinMode(LED_PIN, OUTPUT);
calibrate();
initialize();
}
void initialize() {
// initialize device
Serial.println(F("Initializing MPU 6050 device..."));
mpu.initialize();
// verify connection
Serial.println(F("Testing device connections..."));
Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
// load and configure the DMP
Serial.println(F("Initializing DMP"));
devStatus = mpu.dmpInitialize();
mpu.setXAccelOffset(ax_offset);
mpu.setYAccelOffset(ay_offset);
mpu.setZAccelOffset(az_offset);
mpu.setXGyroOffset(gx_offset);
mpu.setYGyroOffset(gy_offset);
mpu.setZGyroOffset(gz_offset);
//turn the PID on
//servo's can go from 60 degree's to 120 degree so set the angle correction to + - 30 degrees max
myPIDX.SetOutputLimits(-30, 30);
myPIDY.SetOutputLimits(-30, 30);
myPIDX.SetMode(AUTOMATIC);
myPIDY.SetMode(AUTOMATIC);
SetpointX = 0;
SetpointY = 0;
// make sure it worked (returns 0 if so)
if (devStatus == 0)
{
// turn on the DMP, now that it's ready
Serial.println(F("Enabling DMP"));
mpu.setDMPEnabled(true);
// get expected DMP packet size for later comparison
packetSize = mpu.dmpGetFIFOPacketSize();
}
else
{
// ERROR!
// 1 = initial memory load failed, 2 = DMP configuration updates failed (if it's going to break, usually the code will be 1)
Serial.print(F("DMP Initialization failed code = "));
Serial.println(devStatus);
}
}
/*
MAIN PROGRAM LOOP
*/
void loop(void)
{
MainMenu();
//myloop();
}
void myloop(void)
{
// get current FIFO count
fifoCount = mpu.getFIFOCount();
// check for overflow (this should never happen unless our code is too inefficient)
if ( fifoCount == 1024)
{
// reset so we can continue cleanly
mpu.resetFIFO();
Serial.println(F("FIFO overflow!"));
return;
}
// check for correct available data length
if (fifoCount < packetSize)
return;
// read a packet from FIFO
mpu.getFIFOBytes(fifoBuffer, packetSize);
// track FIFO count here in case there is > 1 packet available
fifoCount -= packetSize;
// flush buffer to prevent overflow
mpu.resetFIFO();
// display Euler angles in degrees
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
mpuPitch = ypr[PITCH] * 180 / M_PI;
mpuRoll = ypr[ROLL] * 180 / M_PI;
mpuYaw = ypr[YAW] * 180 / M_PI;
// flush buffer to prevent overflow
mpu.resetFIFO();
// blink LED to indicate activity
blinkState = !blinkState;
digitalWrite(LED_PIN, blinkState);
// flush buffer to prevent overflow
mpu.resetFIFO();
InputX = mpuPitch;
myPIDX.Compute();
InputY = mpuRoll;
myPIDY.Compute();
//if using PID do those
ServoX.write(-OutputX + 90);
ServoY.write(OutputY + 90);
// if you do not want to use the PID
// ServoX.write(-mpuPitch + 90);
// ServoY.write(mpuRoll + 90);
float q1[4];
mpu.dmpGetQuaternion(&q, fifoBuffer);
q1[0] = q.w;
q1[1] = q.x;
q1[2] = q.y;
q1[3] = q.z;
serialPrintFloatArr(q1, 4);
Serial.print(mpuPitch);
Serial.print(" ");
Serial.print(mpuRoll);
Serial.print(" ");
Serial.print(OutputX);
Serial.print(" ");
Serial.println(OutputY);
delay(10);
// flush buffer to prevent overflow
mpu.resetFIFO();
}
/* ================================================================
=== Those 2 functions will format the data ===
================================================================ */
void serialPrintFloatArr(float * arr, int length) {
for (int i = 0; i < length; i++) {
serialFloatPrint(arr[i]);
Serial.print(",");
}
}
void serialFloatPrint(float f) {
byte * b = (byte *) &f;
for (int i = 0; i < 4; i++) {
byte b1 = (b[i] >> 4) & 0x0f;
byte b2 = (b[i] & 0x0f);
char c1 = (b1 < 10) ? ('0' + b1) : 'A' + b1 - 10;
char c2 = (b2 < 10) ? ('0' + b2) : 'A' + b2 - 10;
Serial.print(c1);
Serial.print(c2);
}
}
/*
calibration routines those will be executed each time the board is powered up.
we might want to calibrate it for good on a flat table and save it to the microcontroler eeprom
*/
void calibrate() {
// start message
Serial.println("\nMPU6050 Calibration Sketch");
//delay(1000);
Serial.println("\nYour MPU6050 should be placed in horizontal position, with package letters facing up. \nDon't touch it until you see a finish message.\n");
//delay(1000);
// verify connection
Serial.println(mpu.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
//delay(1000);
// reset offsets
mpu.setXAccelOffset(0);
mpu.setYAccelOffset(0);
mpu.setZAccelOffset(0);
mpu.setXGyroOffset(0);
mpu.setYGyroOffset(0);
mpu.setZGyroOffset(0);
while (1) {
if (state == 0) {
Serial.println("\nReading sensors for first time...");
meansensors();
state++;
delay(100);
}
if (state == 1) {
Serial.println("\nCalculating offsets...");
calibration();
state++;
delay(100);
}
if (state == 2) {
meansensors();
Serial.println("\nFINISHED!");
Serial.print("\nSensor readings with offsets:\t");
Serial.print(mean_ax);
Serial.print("\t");
Serial.print(mean_ay);
Serial.print("\t");
Serial.print(mean_az);
Serial.print("\t");
Serial.print(mean_gx);
Serial.print("\t");
Serial.print(mean_gy);
Serial.print("\t");
Serial.println(mean_gz);
Serial.print("Your offsets:\t");
Serial.print(ax_offset);
Serial.print("\t");
Serial.print(ay_offset);
Serial.print("\t");
Serial.print(az_offset);
Serial.print("\t");
Serial.print(gx_offset);
Serial.print("\t");
Serial.print(gy_offset);
Serial.print("\t");
Serial.println(gz_offset);
Serial.println("\nData is printed as: acelX acelY acelZ giroX giroY giroZ");
Serial.println("Check that your sensor readings are close to 0 0 16384 0 0 0");
Serial.println("If calibration was succesful write down offsets so you can set them in your projects using something similar to mpu.setXAccelOffset(youroffset)");
//while (1);
break;
}
}
}
void meansensors() {
long i = 0, buff_ax = 0, buff_ay = 0, buff_az = 0, buff_gx = 0, buff_gy = 0, buff_gz = 0;
while (i < (buffersize + 101)) {
// read raw accel/gyro measurements from device
mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
if (i > 100 && i <= (buffersize + 100)) { //First 100 measures are discarded
buff_ax = buff_ax + ax;
buff_ay = buff_ay + ay;
buff_az = buff_az + az;
buff_gx = buff_gx + gx;
buff_gy = buff_gy + gy;
buff_gz = buff_gz + gz;
}
if (i == (buffersize + 100)) {
mean_ax = buff_ax / buffersize;
mean_ay = buff_ay / buffersize;
mean_az = buff_az / buffersize;
mean_gx = buff_gx / buffersize;
mean_gy = buff_gy / buffersize;
mean_gz = buff_gz / buffersize;
}
i++;
delay(2); //Needed so we don't get repeated measures
}
}
void calibration() {
ax_offset = -mean_ax / 8;
ay_offset = -mean_ay / 8;
az_offset = (16384 - mean_az) / 8;
gx_offset = -mean_gx / 4;
gy_offset = -mean_gy / 4;
gz_offset = -mean_gz / 4;
while (1) {
int ready = 0;
mpu.setXAccelOffset(ax_offset);
mpu.setYAccelOffset(ay_offset);
mpu.setZAccelOffset(az_offset);
mpu.setXGyroOffset(gx_offset);
mpu.setYGyroOffset(gy_offset);
mpu.setZGyroOffset(gz_offset);
meansensors();
Serial.println("...");
blinkState = !blinkState;
digitalWrite(LED_PIN, blinkState);
if (abs(mean_ax) <= acel_deadzone) ready++;
else ax_offset = ax_offset - mean_ax / acel_deadzone;
if (abs(mean_ay) <= acel_deadzone) ready++;
else ay_offset = ay_offset - mean_ay / acel_deadzone;
if (abs(16384 - mean_az) <= acel_deadzone) ready++;
else az_offset = az_offset + (16384 - mean_az) / acel_deadzone;
if (abs(mean_gx) <= giro_deadzone) ready++;
else gx_offset = gx_offset - mean_gx / (giro_deadzone + 1);
if (abs(mean_gy) <= giro_deadzone) ready++;
else gy_offset = gy_offset - mean_gy / (giro_deadzone + 1);
if (abs(mean_gz) <= giro_deadzone) ready++;
else gz_offset = gz_offset - mean_gz / (giro_deadzone + 1);
if (ready == 6) break;
}
}
//================================================================
// Main menu to interpret all the commands sent by the altimeter console
//================================================================
void MainMenu()
{
char readVal = ' ';
int i = 0;
char commandbuffer[200];
while (Serial.available())
{
readVal = Serial.read();
if (readVal != ';' )
{
if (readVal != '\n')
commandbuffer[i++] = readVal;
}
else
{
commandbuffer[i++] = '\0';
break;
}
}
if (commandbuffer[0] != '\0') {
interpretCommandBuffer(commandbuffer);
commandbuffer[0] = '\0';
}
myloop();
}
void interpretCommandBuffer(char *commandbuffer) {
//Serial1.println((char*)commandbuffer);
//this will erase all flight
if (commandbuffer[0] == 'c')
{
Serial.println(F("calibration\n"));
// Do calibration suff
state = 0;
calibrate();
mpu.setXAccelOffset(ax_offset);
mpu.setYAccelOffset(ay_offset);
mpu.setZAccelOffset(az_offset);
mpu.setXGyroOffset(gx_offset);
mpu.setYGyroOffset(gy_offset);
mpu.setZGyroOffset(gz_offset);
}
}
How did I know that you would ignore the advice on posting code in How to get the best out of this forum
I edited it, it's there in the right format I think..
Thanks. That is so much easier to read and copy for examination
Thanks for your help, I am completely new to these forum things so still figuring it out.
post a pic of your project so we can see that you are trying to power servos from a breadboard.
The issue reads like a power supply thingy.
Are you really powering the servos from the Arduino?
That thing with 9V written on it in the lower right hand of the image you posted is a BIG problem.
Running power through a breadboard to power servo motors is a problem.
Get a new power supply and do not power motors through a breadboard.
Sorry, I forgot to mention that I am not actually directly running the power into the breadboard but instead am running it through a power supply module and then into the breadboard. Was gonna add but I got caught up in something else sorry for that inconvenience.
I also wanted to add that the readings on the Serial Monitor from the MPU6050 were looking good but when I start moving it around; the Servo's were supposed to be moving however there was no movement in them or any kind of indication that there is power or signal running through.
If the servos are those cheap plastic geared ones, they have a stall current of 800-1000ma. How much current can your power supply module supply?
I am using the SG90, 9G micro servos that I believe have a stall current of 650 ± 80 mA and the power supply unit can supply 700mA of current. It is the MB102 Breadboard Power Supply Module Board.
700ma - 650ma - 650ma is equal to how many ma?
OK I'm going to get a higher current power supply unit. Thank you so much for your help, I really appreciate it!
This topic was automatically closed 120 days after the last reply. New replies are no longer allowed.
