hi, im arduino beginner building a self stabilizing platform with an arduino pro mini and imu3000 combo.
i been researching the codes and manage gather the codes and get the servo motors to move according to the sensor values, however when i placed the sensor on top of the servo motor, it will become very unstable and motors start to swing back and forward and unable to stabilize. is there any solution or correction to the codes that can help me stabilize the motor?
#define GYRO 0x68 // gyro I2C address
#define REG_GYRO_X 0x1D // IMU-3000 Register address for GYRO_XOUT_H
#define ACCEL 0x53 // Accel I2c Address
#define ADXL345_POWER_CTL 0x2D
#define DATA_FORMAT 0x31
#define PI 3.14159
byte buffer[12]; // Array to store ADC values
int raw_gyro_x;
int raw_gyro_y;
int raw_gyro_z;
int raw_accel_x;
int raw_accel_y;
int raw_accel_z;
int i;
int angleX;
int angleY;
float gyro_y;
float gyro_x;
float gyro_z;
float accel_x;
float accel_y;
float accel_z;
float roll;
float pitch;
int compAngleX;
int compAngleY;
const int numReadings =10;
int xreadings[numReadings]; // the readings from the analog input
int xindex = 0; // the index of the current reading
int xtotal = 0; // the running total
int yreadings[numReadings]; // the readings from the analog input
int yindex = 0; // the index of the current reading
int ytotal = 0; // the running total
int averageX;
int averageY;
unsigned long time;
long interval = 10;
long previousMillis = 0;
uint32_t timer;
#include <Wire.h>
#include <math.h>
#include <Servo.h>
Servo pitchservo;
Servo rollservo;
int pos = 0;
void Transmission()
{
// Read the Gyro X, Y and Z and Accel X, Y and Z all through the gyro
// First set the register start address for X on Gyro
Wire.beginTransmission(GYRO);
Wire.write(REG_GYRO_X); //Register Address GYRO_XOUT_H
Wire.endTransmission();
// New read the 12 data bytes
Wire.beginTransmission(GYRO);
Wire.requestFrom(GYRO,12); // Read 12 bytes
i = 0;
while(Wire.available())
{
buffer[i] = Wire.read();
i++;
}
Wire.endTransmission();
}
void ReadGyro() //Combine bytes into integers
{
// Gyro format is MSB first
raw_gyro_x = buffer[0] << 8 | buffer[1];
raw_gyro_y = buffer[2] << 8 | buffer[3];
raw_gyro_z = buffer[4] << 8 | buffer[5];
// Accel is LSB first.
raw_accel_x = buffer[7] << 8 | buffer[6];
raw_accel_y = buffer[9] << 8 | buffer[8];
raw_accel_z = buffer[11] << 8 | buffer[10];
}
void Readaccel() //Convert the accelerometer value to G's.
{
accel_x = raw_accel_x * 0.0078;
accel_y = raw_accel_y * 0.0078;
accel_z = raw_accel_z * 0.0078;
}
void convertRawData() //Convert raw Gyro data to deg/s
{
//Sensitivity information (from Invensense's website):
// Range (deg/s) | Sensitivity (LSB/deg/s)
// +/-250 | 131
// +/-500 | 65.5
// +/-1000 | 32.8
// +/-2000 | 16.4
//This is where the sensitivity values come from:
//Gyro sensitivity: 16 bit range (65535) at +/- 500 deg/s (total range of 1000)
// 65535 / 1000 = 65.535
gyro_x = raw_gyro_x / 65.5;
gyro_y = raw_gyro_y / 65.5;
gyro_z = raw_gyro_z / 65.5;
}
void RollandPitchEquation() // Roll & Pitch Equations
{
// Roll rotation about X
roll = atan2(accel_y, sqrt( pow(accel_x,2) + pow(accel_z,2) )); // phi
// pitch rotation about Y
// to correct the direction of rotation about Y , add negative sign on ptich
pitch = - atan2(accel_x , sqrt( pow(accel_y,2) + pow(accel_z,2) )); // rho
}
void RadianToDegree() // convert the values from radians to degree
{
pitch = pitch * (180.0 / PI);
roll = roll * (180.0 / PI);
// using complementary filter, see http://www.pieter-jan.com/node/11
double dt = (double)(micros() - timer) / 1000000; // Calculate delta time
compAngleX = 0.93 * (compAngleX + gyro_x * dt) + 0.07 * roll; // Calculate the angle using a Complimentary filter
compAngleY = 0.93 * (compAngleY + gyro_y * dt) + 0.07 * pitch;
timer = micros();
}
void averageValue()
{
xtotal= xtotal - xreadings[xindex]; // read from the sensor:
xreadings[xindex] = compAngleX; // add the reading to the total:
xtotal= xtotal + xreadings[xindex]; // advance to the next position in the array:
xindex = xindex + 1;
if (xindex >= numReadings) // if we're at the end of the array...
xindex = 0; // ...wrap around to the beginning:
averageX = xtotal / numReadings; // calculate the average:
ytotal = ytotal - yreadings[yindex]; // read from the sensor
yreadings[yindex] = compAngleY; // add the reading to the total
ytotal = ytotal + yreadings[yindex]; // advance to the next position in the array
yindex = yindex + 1;
if (yindex >= numReadings)// if we're at the end of the array...
yindex = 0;// ...wrap around to the beginning
averageY = ytotal / numReadings;// calculate the average
}
void setup()
{
Serial.begin(9600);
pitchservo.attach(9);
rollservo.attach(6);
for (int thisReading = 0; thisReading < numReadings; thisReading++)
{
xreadings[thisReading] = 0;
yreadings[thisReading] = 0;
}
Wire.begin(); // Set Gyro settings
writeTo(GYRO, 0x16, 0x0B);
writeTo(GYRO, 0x18, 0x32); //set accel register data address
writeTo(GYRO, 0x14, ACCEL); // set accel i2c slave address
writeTo(GYRO, 0x3D, 0x08); // Set passthrough mode to Accel so we can turn it on
writeTo(ACCEL, ADXL345_POWER_CTL, 8); // set accel power control to 'measure'
writeTo(ACCEL, DATA_FORMAT, 0x01); //Write 0x00 for 2G, 0x01 for 4G, 0x0A for 8G,0x0B for 16G
writeTo(GYRO, 0x3D, 0x28); //cancel pass through to accel, gyro will now read accel for us
}
void writeTo(int device, byte address, byte val)// Write a value to address register on device
{
Wire.beginTransmission(device); // start transmission to device
Wire.write(address); // send register address
Wire.write(val); // send value to write
Wire.endTransmission(); // end transmission
}
void loop()
{
angleX = 90 + averageX;
pitchservo.write(angleX);
angleY = 90 + averageY ;
rollservo.write(angleY);
led();
Transmission();
ReadGyro();
Readaccel();
convertRawData();
RollandPitchEquation();
RadianToDegree();
averageValue();
}
}
}