Hi friends, I'm lack of knowledge in programming so hope anyone here could help me out with my idea.
I'm building a camera stabilizer with 3 servo motors and a MPU 6050 module with my Arduino Mega 2560. So far I can make it stabilize with PID controlling method, and now im thinking of improving my stabilizer, making its reference position(desired position) adjustable according to the user with just a single click of button.
The problem I am facing now is I do not know how to record just a single data/ angle value from an all time changing values of accelerometer at the moment the button is clicked. Can anyone help me with this or is anyone know another method to achieve the same result?
Thank you in advance.
Here is my code prior to the idea i just mentioned.
#include <Servo.h>
#include <Wire.h>
//Declaring some global variables
int gyro_x, gyro_y, gyro_z;
long gyro_x_cal, gyro_y_cal, gyro_z_cal;
boolean set_gyro_angles;
long acc_x, acc_y, acc_z, acc_total_vector;
float angle_roll_acc, angle_pitch_acc;
float angle_pitch, angle_roll;
int angle_pitch_buffer, angle_roll_buffer;
float angle_pitch_output, angle_roll_output;
long loop_timer;
int temp;
//variables for servo motor
byte PWMOutput;
long PID;
int P;
int I;
int D;
int Error;
Servo servo;
void setup() {
Wire.begin(); //Start I2C as master
setup_mpu_6050_registers(); //Setup the registers of the MPU-6050
for (int cal_int = 0; cal_int < 1000 ; cal_int ++){ //Read the raw acc and gyro data from the MPU-6050 for 1000 times
read_mpu_6050_data();
gyro_x_cal += gyro_x; //Add the gyro x offset to the gyro_x_cal variable
gyro_y_cal += gyro_y; //Add the gyro y offset to the gyro_y_cal variable
gyro_z_cal += gyro_z; //Add the gyro z offset to the gyro_z_cal variable
delay(3); //Delay 3us to have 250Hz for-loop
}
// divide by 1000 to get avarage offset
gyro_x_cal /= 1000;
gyro_y_cal /= 1000;
gyro_z_cal /= 1000;
Serial.begin(115200);
loop_timer = micros(); //Reset the loop timer
servo.attach(9);
servo1.attach(10);
}
void loop(){
static int displayPrescaler = 0;
read_mpu_6050_data();
//Subtract the offset values from the raw gyro values
gyro_x -= gyro_x_cal;
gyro_y -= gyro_y_cal;
gyro_z -= gyro_z_cal;
//Gyro angle calculations . Note 0.0000611 = 1 / (250Hz x 65.5)
angle_pitch += gyro_x * 0.0000611; //Calculate the traveled pitch angle and add this to the angle_pitch variable
angle_roll += gyro_y * 0.0000611; //Calculate the traveled roll angle and add this to the angle_roll variable
//0.000001066 = 0.0000611 * (3.142(PI) / 180degr) The Arduino sin function is in radians
angle_pitch += angle_roll * sin(gyro_z * 0.000001066); //If the IMU has yawed transfer the roll angle to the pitch angel
angle_roll -= angle_pitch * sin(gyro_z * 0.000001066); //If the IMU has yawed transfer the pitch angle to the roll angel
//Accelerometer angle calculations
acc_total_vector = sqrt((acc_x*acc_x)+(acc_y*acc_y)+(acc_z*acc_z)); //Calculate the total accelerometer vector
//57.296 = 1 / (3.142 / 180) The Arduino asin function is in radians
angle_pitch_acc = asin((float)acc_y/acc_total_vector)* 57.296; //Calculate the pitch angle
angle_roll_acc = asin((float)acc_x/acc_total_vector)* -57.296; //Calculate the roll angle
angle_pitch_acc -= 0.0; //Accelerometer calibration value for pitch
angle_roll_acc -= 0.0; //Accelerometer calibration value for roll
if(set_gyro_angles){ //If the IMU is already started
angle_pitch = angle_pitch * 0.9996 + angle_pitch_acc * 0.0004; //Correct the drift of the gyro pitch angle with the accelerometer pitch angle
angle_roll = angle_roll * 0.9996 + angle_roll_acc * 0.0004; //Correct the drift of the gyro roll angle with the accelerometer roll angle
}
else{ //At first start
angle_pitch = angle_pitch_acc; //Set the gyro pitch angle equal to the accelerometer pitch angle
angle_roll = angle_roll_acc; //Set the gyro roll angle equal to the accelerometer roll angle
set_gyro_angles = true; //Set the IMU started flag
}
//To dampen the pitch and roll angles a complementary filter is used
angle_pitch_output = angle_pitch_output * 0.9 + angle_pitch * 0.1; //Take 90% of the output pitch value and add 10% of the raw pitch value
angle_roll_output = angle_roll_output * 0.9 + angle_roll * 0.1; //Take 90% of the output roll value and add 10% of the raw roll value
// Serial.print(" | Angle = "); Serial.println(angle_pitch_output);
//Serial.print(" | Angle2 = "); Serial.println(angle_roll_output);
if(displayPrescaler++ >=100){
displayPrescaler = 0;
Serial.print(" | Angle = "); Serial.println(angle_pitch_output);
Serial.print(" | Angle2 = "); Serial.println(angle_roll_output);
}
while(micros() - loop_timer < 4000); //Wait until the loop_timer reaches 4000us (250Hz) before starting the next loop
loop_timer = micros(); //Reset the loop timer
GetError();
CalculatePID();
servo.write(PWMOutput);
}
void GetError(void)
{
byte i=0;
word ActualPosition_roll = angle_pitch_output; //due to setup of structure, roll direction of MPU
6050 is my pitch direction
Serial.print("ActPos_roll =");
Serial.println(ActualPosition_roll,DEC);
int DesiredPosition_roll = -3;
Serial.print("DesPos_roll=");
Serial.println(DesiredPosition_roll, DEC);
Error = DesiredPosition_roll - ActualPosition_roll;
}
void CalculatePID(void)
{
P = 1.0;
I = 1.0;
D = 4.0;
int dt = 0.1;
int pre_error = 0;
int derivative;
int integral=0;
int proportional = P*Error;
integral = integral + Error*dt;
derivative = (Error - pre_error)*dt;
PID = proportional + I*integral + D*derivative;
pre_error = Error;
Serial.print("PID =");
Serial.println(PID, DEC);
if( PID >= 90 )
PID = 90;
if(PID <= -90)
PID = -90;
PWMOutput = 90+PID;
Serial.print("PWMOutput=");
Serial.println(PWMOutput,DEC);
}
}
