Drone Hacker 123
Drone Hacker 123
Drone Hacker 123
if (devStatus == 0) {
mpu.setDMPEnabled(true);
attachInterrupt(0, dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();
dmpReady = true;
packetSize = mpu.dmpGetFIFOPacketSize();
} else {
Serial.print(F("DMP Initialization failed (code "));
Serial.print(devStatus);
Serial.println(F(")"));
}
}
void loop() {
static uint8_t bUpdateFlags;
if (!dmpReady) return;
while (!mpuInterrupt && fifoCount < packetSize) {
}
mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();
fifoCount = mpu.getFIFOCount();
if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
mpu.resetFIFO();
} else if (mpuIntStatus & 0x02) {
while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
mpu.getFIFOBytes(fifoBuffer, packetSize);
fifoCount -= packetSize;
}
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
mpuYaw = ypr[0] * 180/M_PI;
mpuRoll = ypr[1] * 180/M_PI;
mpuPitch = ypr[2] * 180/M_PI;
if(bUpdateFlagsShared) {
power = 0;
noInterrupts();
bUpdateFlags = bUpdateFlagsShared;
if(bUpdateFlags & THROTTLE_FLAG) {
unThrottleIn = unThrottleInShared;
}
if(bUpdateFlags & PITCH_FLAG) {
unPitchIn = unPitchInShared;
}
if(bUpdateFlags & ROLL_FLAG) {
unRollIn = unRollInShared;
}
bUpdateFlagsShared = 0;
interrupts();
}
if(!bUpdateFlags) {
// Iniciar contador
power++;
}
if(power > 2) {
Serial.println("No Signal!");
outputTL = 1000;
outputTR = 1000;
outputBL = 1000;
outputBR = 1000;
delay(500);
}
// RADIO CONTROLLER DEBUG
//Serial.println("Pitch value: ");
//Serial.println(unPitchIn);
//Serial.println("Roll value: ");
//Serial.println(unRollIn);
pitchSetpoint = mpuPitch; // Valor Deseado (en grados)
pitchInput = map(unPitchIn, 900, 2000, -30, 30); // Valor de entrada (necesita convertirse a grados)
double Pitchgap = abs(pitchSetpoint-pitchInput); // Distancia hasta setpoint (Error)
if(Pitchgap<5) { // Estamos lejos del setpoint, usar parametros conservativos
pitchPID.SetTunings(PitchconsKp, PitchconsKi, PitchconsKd);
} else {
// Estamos muy cerca del setpoint, usa parametros agresivos
pitchPID.SetTunings(PitchaggKp, PitchaggKi, PitchaggKd);
}
pitchPID.Compute();
rollSetpoint = mpuRoll; // Valor deseado (necesita convertirse a grados)
rollInput = map(unRollIn, 900, 2000, -30, 30); // Valor de entrada (necesita convertirse a grados)
double Rollgap = abs(rollSetpoint-rollInput); // Distancia hasta setpoint (Error)
if(Rollgap<5) { // Estamos lejos del setpoint, usar parametros conservativos
rollPID.SetTunings(RollconsKp, RollconsKi, RollconsKd);
} else {
// Estamos muy cerca del setpoint, usa parametros agresivos
rollPID.SetTunings(RollaggKp, RollaggKi, RollaggKd);
}
rollPID.Compute();
// MPU DEBUG
/**
//Serial.println(pitchOutput);
Serial.println("Pitch: ");
Serial.println(mpuPitch);
Serial.println("Roll: ");
Serial.println(mpuRoll);
//Serial.println(rollOutput);
**/
if(bUpdateFlags & THROTTLE_FLAG) {
if(servoMotorTL.readMicroseconds() && servoMotorTR.readMicroseconds()
&& servoMotorBL.readMicroseconds() && servoMotorBR.readMicroseconds()
!= unThrottleIn) {
outputTR = unThrottleIn;
outputTL = unThrottleIn;
outputBL = unThrottleIn;
outputBR = unThrottleIn;
auxTR = unThrottleIn;
auxTL = unThrottleIn;
auxBL = unThrottleIn;
auxBR = unThrottleIn;
/**
* ESTABILIZADOR
* AUTOMATICO
*/
// EJE "X" // ROLL
// Girar Izquierda (Regresa Grados Positivos)
if(mpuRoll > 0) {
outputTL = unThrottleIn + (rollOutput + motorGain); // Convertir grados a RPMs
outputBL = unThrottleIn + (rollOutput + motorGain);
}
// Girar Derecha (Regresa Grados Negativos)
if(mpuRoll < 0) {
outputTR = unThrottleIn + (rollOutput + motorGain); // Convertir grados a RPMs
outputBR = unThrottleIn + (rollOutput + motorGain);
}
// Inclinar Adelante (Grados negativos)
if(mpuPitch < 0) {
outputTL = unThrottleIn + (pitchOutput + motorGain); // Convertir grados a RPMs
outputTR = unThrottleIn + (pitchOutput + motorGain);
}
// Inclinar Atras (Grados Positivos)
if(mpuPitch > 0) {
outputBL = unThrottleIn + (pitchOutput + motorGain); // Convertir grados a RPMs
outputBR = unThrottleIn + (pitchOutput + motorGain);
}
/**
* FIN
* ESTABILIZADOR
* AUTOMATICO
*/
}
}
/**
if(bUpdateFlags & PITCH_FLAG) {
if(servoMotorTL.readMicroseconds() && servoMotorTR.readMicroseconds()
&& servoMotorBL.readMicroseconds() && servoMotorBR.readMicroseconds()
!= unPitchIn) {
//Serial.println(unPitchIn);
pitchInput = map(unPitchIn, 900, 2000, -30, 30); // Valor deseado (necesita convertirse a grados)
pitchSetpoint = mpuPitch; // Valor deseado en grados
double gap = abs(pitchSetpoint-pitchInput); //distance away from setpoint
if(gap<5)
{ //we're close to setpoint, use conservative tuning parameters
pitchPID.SetTunings(PitchconsKp, PitchconsKi, PitchconsKd);
}
else
{
//we're far from setpoint, use aggressive tuning parameters
pitchPID.SetTunings(PitchaggKp, PitchaggKi, PitchaggKd);
}
pitchPID.Compute();
// Ir A Derecha
if(unPitchIn > 1550) {
outputTL = auxTL + (pitchOutput + motorGain); // Convertir grados a RPMs
outputBL = auxTL + (pitchOutput + motorGain);
}
// Ir A La Izquierda
if(unPitchIn < 1450) {
outputTR = auxTR + (pitchOutput + motorGain);
outputBR = auxBR + (pitchOutput + motorGain);
}
}
}
if(bUpdateFlags & ROLL_FLAG) {
if(servoMotorTL.readMicroseconds() && servoMotorTR.readMicroseconds()
&& servoMotorBL.readMicroseconds() && servoMotorBR.readMicroseconds()
!= unRollIn) {
//Serial.println(unRollIn);
rollInput = map(unRollIn, 900, 2000, -30, 30); // Valor deseado (necesita convertirse a grados)
rollSetpoint = mpuRoll; // Valor deseado en grados
double gap = abs(rollSetpoint-rollInput); //distance away from setpoint
if(gap<5)
{ //we're close to setpoint, use conservative tuning parameters
rollPID.SetTunings(RollconsKp, RollconsKi, RollconsKd);
}
else
{
//we're far from setpoint, use aggressive tuning parameters
rollPID.SetTunings(RollaggKp, RollaggKi, RollaggKd);
}
pitchPID.Compute();
if(unRollIn > 1550) {
outputTL = auxTL + (rollOutput + motorGain);
outputTR = auxTR + (rollOutput + motorGain);
}
if(unRollIn < 1450) {
outputBL = auxBL + (rollOutput + motorGain);
outputBR = auxBR + (rollOutput + motorGain);
}
}
}
**/
initMotors(
outputTL,
outputTR,
outputBR,
outputBL
);
bUpdateFlags = 0;
}
void calcThrottle() {
if(digitalRead(THROTTLE_IN_PIN) == HIGH) {
ulThrottleStart = micros();
} else{
unThrottleInShared = (uint16_t)(micros() - ulThrottleStart);
bUpdateFlagsShared |= THROTTLE_FLAG;
}
}
void calcPitch() {
if(digitalRead(PITCH_IN_PIN) == HIGH) {
ulPitchStart = micros();
} else{
unPitchInShared = (uint16_t)(micros() - ulPitchStart);
bUpdateFlagsShared |= PITCH_FLAG;
}
}
void calcRoll() {
if(digitalRead(ROLL_IN_PIN) == HIGH) {
ulRollStart = micros();
} else{
unRollInShared = (uint16_t)(micros() - ulRollStart);
bUpdateFlagsShared |= ROLL_FLAG;
}
}
void initMotors(int tl, int tr, int br, int bl) {
Serial.println(tl);
Serial.println(tr);
Serial.println(br);
Serial.println(bl);
servoMotorTL.writeMicroseconds(tl);
servoMotorTR.writeMicroseconds(tr);
servoMotorBR.writeMicroseconds(br);
servoMotorBL.writeMicroseconds(bl);
}
void arm() {
servoMotorTL.writeMicroseconds(1000);
servoMotorTR.writeMicroseconds(1000);
servoMotorBL.writeMicroseconds(1000);
servoMotorBR.writeMicroseconds(1000);
}
/********************************************************
* PID Adaptive Tuning Example
* One of the benefits of the PID library is that you can
* change the tuning parameters at any time. this can be
* helpful if we want the controller to be agressive at some
* times, and conservative at others. in the example below
* we set the controller to use Conservative Tuning Parameters
* when we're near setpoint and more agressive Tuning
* Parameters when we're farther away.
********************************************************/
#include <PID_v1.h>
//Define Variables we'll be connecting to
double Setpoint, Input, Output;
//Define the aggressive and conservative Tuning Parameters
double aggKp=4, aggKi=0.2, aggKd=1;
double consKp=1, consKi=0.05, consKd=0.25;
//Specify the links and initial tuning parameters
PID myPID(&Input, &Output, &Setpoint, consKp, consKi, consKd, DIRECT);
void setup()
{
//initialize the variables we're linked to
Input = analogRead(0);
Setpoint = 100;
//turn the PID on
myPID.SetMode(AUTOMATIC);
}
void loop()
{
Input = analogRead(0);
double gap = abs(Setpoint-Input); //distance away from setpoint
if(gap<10)
{ //we're close to setpoint, use conservative tuning parameters
myPID.SetTunings(consKp, consKi, consKd);
}
else
{
//we're far from setpoint, use aggressive tuning parameters
myPID.SetTunings(aggKp, aggKi, aggKd);
}
myPID.Compute();
analogWrite(3,Output);
}
/********************************************************
* PID Basic Example
* Reading analog input 0 to control analog PWM output 3
********************************************************/
#include <PID_v1.h>
//Define Variables we'll be connecting to
double Setpoint, Input, Output;
//Specify the links and initial tuning parameters
PID myPID(&Input, &Output, &Setpoint,2,5,1, DIRECT);
void setup()
{
//initialize the variables we're linked to
Input = analogRead(0);
Setpoint = 100;
//turn the PID on
myPID.SetMode(AUTOMATIC);
}
void loop()
{
Input = analogRead(0);
myPID.Compute();
analogWrite(3,Output);
}
/********************************************************
* PID RelayOutput Example
* Same as basic example, except that this time, the output
* is going to a digital pin which (we presume) is controlling
* a relay. the pid is designed to Output an analog value,
* but the relay can only be On/Off.
* to connect them together we use "time proportioning
* control" it's essentially a really slow version of PWM.
* first we decide on a window size (5000mS say.) we then
* set the pid to adjust its output between 0 and that window
* size. lastly, we add some logic that translates the PID
* output into "Relay On Time" with the remainder of the
* window being "Relay Off Time"
********************************************************/
#include <PID_v1.h>
#define RelayPin 6
//Define Variables we'll be connecting to
double Setpoint, Input, Output;
//Specify the links and initial tuning parameters
PID myPID(&Input, &Output, &Setpoint,2,5,1, DIRECT);
int WindowSize = 5000;
unsigned long windowStartTime;
void setup()
{
windowStartTime = millis();
//initialize the variables we're linked to
Setpoint = 100;
//tell the PID to range between 0 and the full window size
myPID.SetOutputLimits(0, WindowSize);
//turn the PID on
myPID.SetMode(AUTOMATIC);
}
void loop()
{
Input = analogRead(0);
myPID.Compute();
/************************************************
* turn the output pin on/off based on pid output
************************************************/
if(millis() - windowStartTime>WindowSize)
{ //time to shift the Relay Window
windowStartTime += WindowSize;
}
if(Output < millis() - windowStartTime) digitalWrite(RelayPin,HIGH);
else digitalWrite(RelayPin,LOW);
}