MATERIALES DRONE

1. UN GIROSCOPIO

2. UN ARDUINO NANO

3. UN SOPORTE
4. 4 MOTORES MINI
5. UNA BATERIA LIPO
6. Cámara arduino
Código para arduino:
<p>#include "I2Cdev.h"<br>#include <servo.h>
#include <spi.h>
#include "RF24.h"
#include "MPU6050_6Axis_MotionApps20.h"
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
//we need this to see nrf24l01 configuration in serial
#include "printf.h"</spi.h></servo.h></p><p>//end of libraries
###############################################</p><p>MPU6050 mpu;</p><p>float
x_rotation, y_rotation, z_rotation;
Servo motor1;
Servo motor2;
Servo motor3;
Servo motor4;
bool first_loop = true;
//radio
RF24 radio(9,10);
uint8_t data[6];
const uint64_t pipe = 0xE8E8F0F0E1LL;
long last_received;
int controll_number = 159;</p><p>//values = 5.2, 0.02, 1500
//variables for movement and positions ###########################################
//for my quadcopter this are the best settings for pid
float x_kp = 5, x_ki = 0.02, x_kd = 1100; //values for PID X axis
int max_pid = 300;
float x_p_out, x_i_out, x_d_out, x_output; //outputs for PID
float x_now, x_lasttime = 0, x_timechange;
float x_input, x_lastinput = 0, x_setpoint = 0;
float x_error, x_errorsum = 0, x_d_error, x_lasterror;</p><p>//values = 5.2, 0.02, 1500
float y_kp = 5, y_ki = 0.02, y_kd = 1100; //values for PID Y axis
float y_p_out, y_i_out, y_d_out, y_output; //outputs for PID
float y_now, y_lasttime = 0, y_timechange;
float y_input, y_lastinput = 0, y_setpoint = 0;
float y_error, y_errorsum = 0, y_d_error, y_lasterror;</p><p>float z_kp = 2, z_ki = 0, z_kd = 0;
//values for PID Z axis
float z_p_out, z_i_out, z_d_out, z_output; //outputs for PID
float z_now, z_lasttime = 0, z_timechange;
float z_input, z_lastinput = 0, z_setpoint = 0;
float z_error, z_errorsum = 0, z_d_error, z_lasterror;</p><p>//set it to 0 and see on serial port
what is the value for x and y rotation, use only if your flightcontroller board is not perfevtly
leveled. If your board is perfectly leveled set it to 0
float x_level_error = 0;
float y_level_error = 0;</p><p>/*
*
*
 * JUNE 2016 - APRIL 2017
* C by Nikodem Bartnik
* [email protected]
* nikodembartnik.pl
*
*
*
*/</p><p>#define INTERRUPT_PIN 2 // use pin 2 on Arduino Uno & most boards
int motor1_power;
int motor2_power;
int motor3_power;
int motor4_power;</p><p>float allmotors_power = 600;</p><p>// MPU control/status vars
bool dmpReady = false; // set true if DMP init was successful
uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
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</p><p>// orientation/motion vars
Quaternion q; // [w, x, y, z] quaternion container</p><p>VectorFloat gravity; // [x, y, z]
gravity vector</p><p>float rotation[3]; // [yaw, pitch, roll] yaw/pitch/roll container and
gravity vector
int safe_lock = 1;</p><p>volatile bool mpuInterrupt = false; // indicates whether MPU interrupt
pin has gone high
void dmpDataReady() {
mpuInterrupt = true;
}</p><p>void setup() {
// join I2C bus (I2Cdev library doesn't do this automatically)
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation
difficulties
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif</p><p>printf_begin();</p><p> Serial.begin(115200);</p><p>
Serial.println("Initializing I2C devices...");
mpu.initialize();
pinMode(INTERRUPT_PIN, INPUT);</p><p> // verify connection
Serial.println("Testing device connections...");
Serial.println(mpu.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection
failed");
// bmp.begin();
radio.begin();
delay(1000);
radio.setDataRate(RF24_250KBPS);
radio.setPALevel(RF24_PA_MAX);</p><p> radio.openReadingPipe(1,pipe);
radio.startListening();

</p><p> // load and configure the DMP

 Serial.println("Initializing DMP...");
devStatus = mpu.dmpInitialize();</p><p> // gyro offsets here
mpu.setXGyroOffset(87);
mpu.setYGyroOffset(77);
mpu.setZGyroOffset(110);
mpu.setZAccelOffset(2287);
mpu.setYAccelOffset(-1283);
mpu.setXAccelOffset(-3083);</p><p> // make sure it worked (returns 0 if so)
if (devStatus == 0) {</p><p> Serial.println("Enabling DMP...");
mpu.setDMPEnabled(true);</p><p>
attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();</p><p>
dmpReady = true;</p><p> 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("DMP Initialization failed (code ");
Serial.print(devStatus);
Serial.println(")");
}</p><p> motor1.attach(5);
motor2.attach(8);
motor3.attach(7);
motor4.attach(4);
pinMode(A0, INPUT);
pinMode(A1, INPUT);
digitalWrite(A0, LOW);
motor1.write(0);
motor2.write(0);
motor3.write(0);
motor4.write(0);</p><p>radio.printDetails();
}</p><p>void loop() {</p><p> if (radio.available()) {

bool done = false;

while (!done){

done = radio.read(data, sizeof(data));</p><p> // Serial.print("Controll number: ");

//Serial.println(data[0]);

if((millis()-last_received) < 3000){

if(data[0] == controll_number){
Serial.print("DATA1: ");
Serial.println(data[1]);
allmotors_power = map(data[1], 0, 255, 800, 1500);
if(allmotors_power < 0){
allmotors_power = 0;
}</p><p>
 //allmotors_power = map(data[1], 0, 255, 800, 1600);
x_setpoint = data[3] - 20;
y_setpoint = data[2] - 20;
Serial.print("PID OUT X: ");
Serial.print(x_rotation);
Serial.print(" PID OUT Y: ");
Serial.print(y_rotation);
Serial.print("Z NOW: ");
Serial.println(z_rotation);
Serial.print(" TIME: ");
Serial.println(millis());</p><p>Serial.print("MOTORS POWER: ");
Serial.println(allmotors_power);

}
}
// Serial.println(data[1]);
if(done == true){
last_received = millis();
}
}
}</p><p> if((millis()-last_received) > 3000 && allmotors_power > 0){
safe_lock = 0;
}

// if programming failed, don't try to do anything

// if (!dmpReady) return;</p><p>
while (!mpuInterrupt && fifoCount < packetSize) {

}</p><p> // reset interrupt flag and get INT_STATUS byte

mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();</p><p> // get current FIFO count
fifoCount = mpu.getFIFOCount();</p><p> // check for overflow (this should never happen
unless our code is too inefficient)
if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
</p><p> // otherwise, check for DMP data ready interrupt (this should happen frequently)
} else if (mpuIntStatus & 0x02) {
// wait for correct available data length, should be a VERY short wait
while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();</p><p> // read a packet
from FIFO
mpu.getFIFOBytes(fifoBuffer, packetSize);

// track FIFO count here in case there is > 1 packet available

// (this lets us immediately read more without waiting for an interrupt)
fifoCount -= packetSize;</p><p> if(safe_lock == 1){

mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
 mpu.dmpGetYawPitchRoll(rotation, &q, &gravity);</p><p> x_rotation = rotation[1] *
180/M_PI - x_level_error;
y_rotation = rotation[2] * 180/M_PI - y_level_error;
z_rotation = rotation[0] * 180/M_PI;</p><p>/*
if(pressure_loop_number == 10){
// Serial.print("Preasure: ");
//Serial.println(bmp.readAltitude());
pressure_loop_number = 0;
allmotors_power = 1000;
}
pressure_loop_number++;
*/</p><p> if(first_loop == true){
z_setpoint = z_rotation;
// current_altitude = bmp.readAltitude();
//set_altitude = current_altitude;
first_loop = false;
}

motor1_power = allmotors_power;
motor2_power = allmotors_power;
motor3_power = allmotors_power;
motor4_power = allmotors_power;
if(allmotors_power > 1500){
allmotors_power = 1500;

}</p><p>
x_output = calculatePID(0, x_rotation);
y_output = calculatePID(1, y_rotation);
z_output = calculatePID(2, z_rotation);</p><p> motor1_power += x_output/2;
motor1_power += z_output;
motor4_power -= x_output/2;
motor4_power += z_output;</p><p> motor2_power -= y_output/2;
motor2_power -= z_output;
motor3_power += y_output/2;
motor3_power -= z_output;

motor1.writeMicroseconds(motor1_power);
motor4.writeMicroseconds(motor4_power);
motor2.writeMicroseconds(motor2_power);
motor3.writeMicroseconds(motor3_power);
mpu.resetFIFO();

}else{
 motor1.write(0);
motor2.write(0);
motor3.write(0);
motor4.write(0);
}
}
}</p><p> float calculatePID(int _axis, float _angel){</p><p> // X AXIS
if(_axis == 0){
x_now = millis();
x_timechange = x_now - x_lasttime;
x_error = x_setpoint - _angel;
x_p_out = (x_kp * x_error);

x_errorsum = (x_errorsum + x_error);

if(x_errorsum > 1023){
x_errorsum = 1023;
}
if(x_errorsum < -1023){
x_errorsum = -1023;
}
x_i_out = x_ki * x_errorsum;
x_d_error = (x_error - x_lasterror) / x_timechange;
x_d_out = x_kd * x_d_error;
x_lasterror = x_error;
x_output = x_p_out + x_i_out + x_d_out;
if(x_output > max_pid){
x_output = max_pid;
}else if(x_output < -(max_pid)){
x_output = -(max_pid);
}
x_lasttime = millis();
return x_output;
}</p><p> // Y AXIS
else if(_axis == 1){
y_now = millis();
y_timechange = y_now - y_lasttime;
y_error = y_setpoint - _angel;
y_p_out = (y_kp * y_error);</p><p> y_errorsum = (y_errorsum + y_error) *
y_timechange;
if(y_errorsum > 1023){
y_errorsum = 1023;
}
if(y_errorsum < -1023){
y_errorsum = -1023;
}
y_i_out = y_ki * y_errorsum;
y_d_error = (y_error - y_lasterror) / y_timechange;
y_d_out = y_kd * y_d_error;
 y_lasterror = y_error;
y_output = y_p_out + y_i_out + y_d_out;
if(y_output > max_pid){
y_output = max_pid;
}else if(y_output < -(max_pid)){
y_output = -(max_pid);
}
y_lasttime = millis();
return y_output;</p><p> // ALTITUDE
// } else if(_axis == 2){
// return (set_altitude - current_altitude) * 20;
}else if(_axis == 2){
z_now = millis();
z_timechange = z_now - z_lasttime;
z_error = z_setpoint - _angel;
z_p_out = (z_kp * z_error);</p><p> z_errorsum = (z_errorsum + z_error) *
z_timechange;
if(z_errorsum > 1023){
z_errorsum = 1023;
}
if(z_errorsum < -1023){
z_errorsum = -1023;
}
z_i_out = z_ki * z_errorsum;
z_d_error = (z_error - z_lasterror) / z_timechange;
z_d_out = z_kd * y_d_error;
z_lasterror = y_error;
z_output = z_p_out + z_i_out + z_d_out;
if(z_output > max_pid){
z_output = max_pid;
}else if(z_output < -(max_pid)){
z_output = -(max_pid);
}
z_lasttime = millis();
return z_output;</p><p> // ALTITUDE
// } else if(_axis == 2){
// return (set_altitude - current_altitude) * 20;
}else{
return 0;
}</p><p>

}</p>
<p>/*</p><p> *
*
* JUNE 2016 - APRIL 2017
* C by Nikodem Bartnik
* [email protected]
* nikodembartnik.pl
*
*
*