Cuadrotor (23 / 24 paso)

Paso 23: código de

Nuestro código representa un sistema de control de retroalimentación conocido como PID (proporcional integral derivado). Actualmente, sólo emplea el uso de los componentes proporcionales y derivados. Con nuestro código actual, el cuadrotor uno estabiliza muy bien en el aire, pero es un poco inestable en el despegue. Sin embargo, esta inestabilidad puede ser mitigada por sacar rápidamente.

Para encontrar las cantidades actuales de inclinación en el eje X y Y del acelerómetro y giróscopo datos, usamos un algoritmo que promedio anterior datos de acelerómetro y combinar con datos de girocompás para llegar a una medida del ángulo que era bastante resistente a la aceleración lineal.

Sólo hacemos 2 comandos Pulsin por loop (en lugar de 4) para reducir el tiempo de lazo en medio, que hace que el sistema de control cuadrotor mucho más sensibles.

posiciones de neutro acelerómetro/giroscopio
#define X_ZERO 332
#define Y_ZERO 324
#define Z_ZERO 396
#define PITCH_ZERO 249
#define ROLL_ZERO 249
#define YAW_ZERO 248

#define GYRO_CON 1.47
#define ACCEL_CON 0.93

#define TIME_CON 0.02
#define SEN_CON 0.95

vars de velocidad del motor
velocidades de int [4];

entradas del girocompás - actuales inclinación var
flotador de pitch, roll, desvío;
int pitchzero, rollzero;
entradas de acelerómetro - vars de aceleración actual
flotador de xin, yin, zin;

humanos insumos - control info vars
flotador pitchin rollin, yawin, zhuman;

al azar otros vars
Float xaverage = 0, yaverage = 0;
int y = 0;
int bla;

constantes de proporcionalidad
flotador p = 2.5; Constante de proporcionalidad P
flotador de d = 0,5; Constante de proporcionalidad D

void setup() {}
zhuman = 0;
Rollin = 0;
Serial.Begin(9600);
para (int x = 6; x < 10; x ++) {}
pinMode (x, salida);
}

enviar límite superior para las entradas de humanas a los controladores de velocidad motor
para (int x = 6; x < 10; x ++) {}
pulsout(x,2000);
}
Delay(5000);

obtener ceros para echada y rodillo humanos entradas
para (int x = 0; x < 10; x ++) {}
y=y+analogRead(3);
}
pitchzero = y/10;
y = 0;
para (int x = 0; x < 10; x ++) {}
y=y+analogRead(4);
}
rollzero = y/10;
}

void loop () {}
¿entradas de acelerómetro y giroscopio extendieron-232 a 232?
Xin = (analogRead (0)-X_ZERO) * ACCEL_CON;
Yin = (analogRead 1-Y_ZERO) * ACCEL_CON;
Zin = (analogRead 2-Z_ZERO) * ACCEL_CON;
pitch=(pitchzero-analogRead(3)) * GYRO_CON;
Roll=(rollzero-analogRead(4)) * GYRO_CON;
desvío = (analogRead 5-YAW_ZERO) * GYRO_CON;

obtener insumos humanos a través de radio aquí rango de -30 a 30 excepto zhuman que tiene un rango ideal de 1000-2000, sólo 2 pulsos por ciclo
{if(Blah==0)}
Yawin = 0.06 * (pulseIn(2,HIGH)-1500) (int firmado);
Pitchin = 0.06 * (pulseIn(3,HIGH)-1500) (int firmado);
bla = 1;
}
Else {}
zhuman =(signed int) pulseIn(4,HIGH);
Rollin = 0.06 * (pulseIn(5,HIGH)-1400) (int firmado); 1400 en lugar de 1500 es corregir para el motor underpowered #4 por recorte en código
bla = 0;
}

promedio, etc..
xaverage = SEN_CON * (xaverage + TIME_CON * pitch) + (1 - SEN_CON) * xin;
yaverage = SEN_CON * (yaverage + TIME_CON * rodillo) + (1 - SEN_CON) * yin;

calcular las velocidades del motor
if(zhuman<1150) {}
para (int x = 0; x < 4; x ++) {}
velocidades [x] = zhuman;
}
}
Else {}
Si {} (zhuman > 1450)
zhuman = 1450;
}
velocidades [0] = zhuman - p * (xaverage - pitchin) - p*(yawin) - d * de la echada;
velocidades [1] = zhuman - p * (pitchin - xaverage) - p*(yawin) + d * echada;
velocidades [2] = zhuman - p * (yaverage - rollin) + p*(yawin) - d * rodillo;
velocidad [3] = zhuman - p * (rollin - yaverage) + p*(yawin) + d * rodillo;
}
establecer los límites superiores e inferiores para velocidades de motor (1000 no = ninguna velocidad, 1600 = límite superior de velocidad, 2000 = máxima velocidad)
para (int x = 0; x < 4; x ++) {}
límite de velocidad entre 1000 y 1600
Si (velocidades [x] < 1000) {}
velocidades [x] = 1000;
}
Si (velocidades [x] > 1600) {}
velocidades [x] = 1600;
}
}

pulsouts a los controladores de velocidad motor
para (int x = 0; x < 4; x ++) {}
pulsout(x+6,speeds[x]);
}
}
{} void pulsout (pin int, int duración)
digitalWrite (pin, HIGH);
delayMicroseconds(duration);
digitalWrite (pin, LOW);
}