Hi!
I am new to arduino programming (embedded programming) but have previous experience in programming. I want to try to make a drone. I have gathered some code online and tried to put it together to understand it better, now the pwmRight drifts away by time... This is what ive come up with this far:
#include <Servo.h>
#include "mpu9250.h"
Mpu9250 imu(&Wire, 0x68);
int status;
/* Temporary without remote */
float input_throttle = 1200;
/* Valut */
float elapsedTime, time, timePrev;
long loop_timer;
/* Roll PID */
float roll_PID, pid_throttle_L_F, pid_throttle_L_B, pid_throttle_R_F, pid_throttle_R_B, roll_error, roll_previous_error;
float roll_pid_p = 0;
float roll_pid_i = 0;
float roll_pid_d = 0;
/* Roll PID Constants */
double roll_kp = 7; //3.55
double roll_ki = 0.006; //0.003
double roll_kd = 1.2; //2.05
float roll_desired_angle = 0;
/* Pitch PID */
float pitch_PID, pitch_error, pitch_previous_error;
float pitch_pid_p = 0;
float pitch_pid_i = 0;
float pitch_pid_d = 0;
/* Pitch PID Constants */
double pitch_kp = 72; //3.55
double pitch_ki = 0.006; //0.003
double pitch_kd = 1.22; //2.05
float pitch_desired_angle = 0;
/* Yaw PID */
float yaw_PID, yaw_error, yaw_previous_error;
float yaw_pid_p = 0;
float yaw_pid_i = 0;
float yaw_pid_d = 0;
/* Yaw PID Constants */
double yaw_kp = 72; //3.55
double yaw_ki = 0.006; //0.003
double yaw_kd = 1.22; //2.05
float yaw_desired_angle = 0;
void setup() {
Serial.begin(115200);
while (!Serial) {}
if (!imu.Begin()) {
Serial.println("IMU initialization unsuccessful");
while (1) {}
}
loop_timer = micros();
}
void loop() {
/* Read the sensor */
if (imu.Read()) {
timePrev = time;
time = millis();
elapsedTime = (time - timePrev) / 1000;
/* PID */
roll_error = roll_desired_angle - (imu.gyro_x_radps() * (180 / 3.14));
pitch_error = pitch_desired_angle - (imu.gyro_y_radps() * (180 / 3.14));
yaw_error = pitch_desired_angle - (imu.gyro_z_radps() * (180 / 3.14));
/* Prospect */
roll_pid_p = roll_kp * roll_error;
pitch_pid_p = pitch_kp * pitch_error;
yaw_pid_p = yaw_kp * yaw_error;
/* Integral */
if (-3 < roll_error < 3)
{
roll_pid_i = roll_pid_i + (roll_ki * roll_error);
}
if (-3 < pitch_error < 3)
{
pitch_pid_i = pitch_pid_i + (pitch_ki * pitch_error);
}
if (-3 < yaw_error < 3)
{
yaw_pid_i = yaw_pid_i + (yaw_ki * yaw_error);
}
/* Derivate */
roll_pid_d = roll_kd * ((roll_error - roll_previous_error) / elapsedTime);
pitch_pid_d = pitch_kd * ((pitch_error - pitch_previous_error) / elapsedTime);
yaw_pid_d = yaw_kd * ((yaw_error - yaw_previous_error) / elapsedTime);
/* PID summery */
roll_PID = roll_pid_p + roll_pid_i + roll_pid_d;
pitch_PID = pitch_pid_p + pitch_pid_i + pitch_pid_d;
yaw_PID = yaw_pid_p + yaw_pid_i + yaw_pid_d;
/* Regulate PID output for ESCs */
if (roll_PID < -400) {
roll_PID = -400;
}
if (roll_PID > 400) {
roll_PID = 400;
}
if (pitch_PID < -400) {
pitch_PID = -400;
}
if (pitch_PID > 400) {
pitch_PID = 400;
}
if (yaw_PID < -400) {
yaw_PID = -400;
}
if (yaw_PID > 400) {
yaw_PID = 400;
}
/* Set the throttle PID for each motor */
pid_throttle_R_F = input_throttle - roll_PID - pitch_PID + yaw_PID;
pid_throttle_L_F = input_throttle + roll_PID - pitch_PID - yaw_PID;
pid_throttle_R_B = input_throttle - roll_PID + pitch_PID - yaw_PID;
pid_throttle_L_B = input_throttle + roll_PID + pitch_PID + yaw_PID;
/* Regulate throttle for ESCs */
//Right front
if (pid_throttle_R_F < 1100)
{
pid_throttle_R_F = 1100;
}
if (pid_throttle_R_F > 2000)
{
pid_throttle_R_F = 2000;
}
//Left front
if (pid_throttle_L_F < 1100)
{
pid_throttle_L_F = 1100;
}
if (pid_throttle_L_F > 2000)
{
pid_throttle_L_F = 2000;
}
//Right back
if (pid_throttle_R_B < 1100)
{
pid_throttle_R_B = 1100;
}
if (pid_throttle_R_B > 2000)
{
pid_throttle_R_B = 2000;
}
//Left back
if (pid_throttle_L_B < 1100)
{
pid_throttle_L_B = 1100;
}
if (pid_throttle_L_B > 2000)
{
pid_throttle_L_B = 2000;
}
/* Save Previous Error */
roll_error = roll_previous_error;
pitch_error = pitch_previous_error;
yaw_error = yaw_previous_error;
/* Display the data */
Serial.print(imu.gyro_x_radps(), 6);
Serial.print("\t");
Serial.print(imu.gyro_y_radps(), 6);
Serial.print("\t");
Serial.print(imu.gyro_z_radps(), 6);
/*
Serial.print(imu.mag_x_ut(), 6);
Serial.print("\t");
Serial.print(imu.mag_y_ut(), 6);
Serial.print("\t");
Serial.print(imu.mag_z_ut(), 6);
Serial.print("\t");
Serial.println(imu.die_temperature_c(), 6);
*/
}
while (micros() - loop_timer < 4000);
loop_timer = micros();
}
Thanks beforehand!
Best regards Max
