Quadrotor (23 / 24 steg)

Steg 23: kod

Vår kod representerar ett feedbacksystem kallas PID (proportionell integrerad derivat). Det sysselsätter för närvarande, endast användning av proportionell och derivat. Med vår nuvarande kod, quadrotor själv stabiliserar ganska bra i luften, men är lite instabil på starten. Denna instabilitet kan dock mildras genom att ta bort snabbt.

För att hitta de aktuella belopp av tilt på X- och Y-axeln från accelerometer och gyro data, använde vi en algoritm som skulle medeltal tidigare accelerometer data och kombinera den med gyro data att nå en vinkel mätning som var ganska motståndskraftig till linjär acceleration.

Vi bara göra 2 Pulsin kommandon per slinga (istället för 4) för att minska tid som loop i hälften, vilket gör systemet för kontroll av quadrotor mycket mer lyhörd.

neutral accelerometer/gyro positioner
#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

varvtal vars
int hastigheter [4].

gyro ingångar - nuvarande luta vars
flyta pitch, rulla, yaw;
int pitchzero, rollzero;
Accelerometer ingångar - nuvarande acceleration vars
float xin, yin, zin;

mänskliga ingångar - kontroll information vars
flyta pitchin rollin, yawin, zhuman;

slumpmässiga andra vars
flyta xaverage = 0, yaverage = 0;
int y = 0;
int bla;

proportionalitet konstanter
flyta p = 2.5. P proportionalitet konstant
flyta d = 0,5; D proportionalitet konstant

void setup() {
zhuman = 0;
Rollin = 0;
Serial.BEGIN(9600);
för (int x = 6; x < 10; x ++) {
pinMode (x, produktionen);
}

Skicka övre gräns för människans ingångar till de motoriska Hastighetsregulatorer
för (int x = 6; x < 10; x ++) {
pulsout(x,2000);
}
Delay(5000);

få nollor för pitch och rulla mänskliga ingångar
för (int x = 0; x < 10; x ++) {
y=y+analogRead(3);
}
pitchzero = y/10;
y = 0;
för (int x = 0; x < 10; x ++) {
y=y+analogRead(4);
}
rollzero = y/10;
}

Ogiltig loop () {
accelerometer och gyro ingångar varierade-232-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;
Yaw = (analogRead (5)-YAW_ZERO) * GYRO_CON;

får mänskliga ingångar via radio här utbud av -30 till 30 utom zhuman som har ett perfekt urval av 1000-2000, endast 2 pulser per slinga
IF(blah==0) {
yawin = 0,06 * ((signerad int) pulseIn(2,HIGH)-1500);
Pitchin = 0,06 * ((signerad int) pulseIn(3,HIGH)-1500);
bla = 1;
}
annat {
zhuman =(signed int) pulseIn(4,HIGH);
Rollin = 0,06 * ((signerad int) pulseIn(5,HIGH)-1400); 1400 i stället för 1500 är att korrigera för underpowered motorn #4 genom att finjustera det i kod
bla = 0;
}

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

beräkna den motoriska hastigheten
IF(zhuman<1150) {
för (int x = 0; x < 4; x ++) {
hastigheter [x] = zhuman;
}
}
annat {
om (zhuman > 1450) {
zhuman = 1450;
}
hastigheter [0] = zhuman - Persson (xaverage - pitchin) - p*(yawin) - d * Beck;
hastigheter [1] = zhuman - Persson (pitchin - xaverage) - p*(yawin) + d * Beck;
hastigheter [2] = zhuman - p * (yaverage - rollin) + p*(yawin) - d * rulla;
hastigheter [3] = zhuman - Persson (rollin - yaverage) + p*(yawin) + d * rulla;
}
ställa in övre och nedre gränserna för motor hastigheter (1000 = ingen hastighet, 1600 = övre hastighetsgräns, 2000 = högsta möjliga hastighet)
för (int x = 0; x < 4; x ++) {
hastighetsgränsen mellan 1000 och 1600
om (hastigheter [x] < 1000) {
hastigheter [x] = 1000;
}
om (hastigheter [x] > 1600) {
hastigheter [x] = 1600;
}
}

pulsouts till motor Hastighetsregulatorer
för (int x = 0; x < 4; x ++) {
pulsout(x+6,Speeds[x]);
}
}
void pulsout (int pin, int längd) {
digitalWrite (pin, hög);
delayMicroseconds(duration);
digitalWrite (pin, låg);
}