Can you help me to make a diy drone ? i cannot go forward. i use mpu6050 and nrf24l01+ . I read a lot blogs about it but not found enough knowledge .
Some words are written in turkish but you can understand by functions.
HATA = ERROR
GELEN = joysticks data array etc.
#include <SPI.h>
#include <RF24.h>
#include <nRF24L01.h>
#include <MPU6050_light.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>>
RF24 radio(7, 8); // CE, CSN
const byte address[6] = "00001";
int gelen[4] ;
MPU6050 mpu(Wire);
int Kp; int Ki; int Kd; // DEĞER ATARIZ .
int HATA_X = 0 ;
int eHATA_X= 0 ;
int HD_X;
float P_X, I_X=0, D_X ; //
int HATA_Y = 0 ;
int eHATA_Y= 0 ;
int HD_Y;
float P_Y, I_Y=0, D_Y ; //
int HATA_Z = 0 ;
int eHATA_Z= 0 ;
int HD_Z;
float P_Z, I_Z=0, D_Z ; //
int pidX ;
int pidY ;
int pidZ ;
unsigned long previousTime = 0 ;
#define ESCPIN_1 3
#define ESCPIN_2 5
#define ESCPIN_3 6
#define ESCPIN_4 9
void setup(){
while (!Serial);
Serial.begin(115200);
Wire.begin();
byte status = mpu.begin();
Serial.print(F("MPU6050 status: "));
Serial.println(status);
while (status != 0) { } // stop everything if could not connect to MPU6050
Serial.println(F("Calculating offsets, do not move MPU6050"));
delay(1000);
mpu.calcOffsets(); // gyro and accelero
Serial.println("Done!n");
radio.begin();
radio.openReadingPipe(0, address);
radio.startListening();
pinMode(ESCPIN_1 , OUTPUT ) ;
pinMode(ESCPIN_2 , OUTPUT ) ;
pinMode(ESCPIN_3 , OUTPUT ) ;
pinMode(ESCPIN_4 , OUTPUT ) ;
}
void loop() {
if (radio.available())
{
radio.read(&gelen, sizeof(gelen));
Serial.print(gelen[0]);
Serial.print(" " );
Serial.print(gelen[1]);
Serial.print(" " );
Serial.print(gelen[2]);
Serial.print(" " );
Serial.println(gelen[3]);
int gaz = gelen[0]; //bunu nasıl kullanacağımı bilmiyorum. throttle bu .
int istenilen_açi_z_ekseninin = gelen[1];
int istenilen_açi_x_ekseninin = gelen[2];
int istenilen_açi_y_ekseninin = gelen[3];
}
radio.read(&istenen_konum[ ], sizeof(istenen_konum[ ]));
mpu.update();
int mpu_6050_alinan_deger_x_ekseninin = mpu.getAngleX();
int mpu_6050_alinan_deger_y_ekseninin = mpu.getAngleY();
int mpu_6050_alinan_deger_z_ekseninin = mpu.getAngleZ();
/*if ((millis() - timer) > 10) { // print data every 10ms
Serial.print("X : ");
Serial.print(X);
Serial.print("tY : ");
Serial.print(Y);
Serial.print("tZ : ");
Serial.println(Z);
timer = millis();
}*/
unsigned long currentTime = millis();
dt = (currentTime - previousTime) / 1000.0; // ms -> saniyeye çevir
previousTime = currentTime;
eHATA_X = HATA_X ;
HATA_X = istenilen_açi_x_ekseninin - mpu_6050_alinan_deger_x_ekseninin ;
eHATA_Y = HATA_Y ;
HATA_Y = istenilen_açi_y_ekseninin - mpu_6050_alinan_deger_y_ekseninin ;
eHATA_Z = HATA_Z ;
HATA_Z = istenilen_açi_z_ekseninin - mpu_6050_alinan_deger_z_ekseninin ;
HD_X =HATA_X - eHATA_X;
D_X=(Kd * HD_X ) /dt;
HD_Y =HATA_Y - eHATA_X ;
D_Y=(Kd * HD_Y ) /dt ;
HD_Z =HATA_Z - eHATA_Z ;
D_Z=(Kd * HD_Z ) /dt ;
P_X = Kp * HATA_X ;
P_Y = Kp * HATA_Y ;
P_Z = Kp * HATA_Z ;
I_X = I_X + (Ki * HATA_X * dt); // dt her PID fonksiyonu uyguladığında geçen süre
I_Y = I_Y + (Ki * HATA_Y * dt);// dt her PID fonksiyonu uyguladığında geçen süre
I_Z = I_Z + (Ki * HATA_Z * dt);// dt her PID fonksiyonu uyguladığında geçen süre
pidX = P_X*Kp+ I_X*Ki+ D_X*Kd ;
pidY = P_Y*Kp+ I_Y*Ki+ D_Y*Kd ;
pidY = P_Y*Kp+ I_Y*Ki+ D_Y*Kd ;
constrain(pidX,0, 255 ) ;
constrain(pidY,0, 255 ) ;
constrain(pidZ,0, 255 ) ;
analogWrite(ESCPIN_1 , pidX) ;
analogWrite(ESCPIN_1 , pidY) ;
analogWrite(ESCPIN_1 , pidZ) ;
analogWrite(ESCPIN_2 , pidX) ;
analogWrite(ESCPIN_2 , pidY) ;
analogWrite(ESCPIN_2 , pidZ) ;
analogWrite(ESCPIN_3 , pidX) ;
analogWrite(ESCPIN_3 , pidY) ;
analogWrite(ESCPIN_3 , pidZ) ;
analogWrite(ESCPIN_4 , pidX) ;
analogWrite(ESCPIN_4 , pidY) ;
analogWrite(ESCPIN_4 , pidZ) ;
}