Quadrotor (23 / 24 stap)

Stap 23: Code

Onze code vertegenwoordigt een feedbacksysteem controle PID (proportioneel integrale afgeleide) genoemd. Er werken momenteel alleen gebruik van de evenredige en afgeleide componenten. Met onze huidige code, de quadrotor zelf stabiliseert heel goed in de lucht, maar is een beetje instabiel op opstijgen. Deze instabiliteit kan echter worden verzacht door snel opstijgen.

We gebruikten om te zoeken naar de huidige bedragen van tilt op de X- en Y-as van de versnellingsmeter en gyro gegevens, een algoritme dat zou gemiddeld vorige versnellingsmeter-gegevens en het combineren met gyro gegevens te bereiken een hoekmeting die vrij veerkrachtig tot lineaire versnelling was.

We doen alleen 2 Pulsin opdrachten per lus (in plaats van 4) om de lus keer gesneden in de helft, waardoor het controlesysteem van de quadrotor veel sneller reageren.

neutrale versnellingsmeter/gyro posities
#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

motorsnelheid vars
int snelheden [4];

Gyro ingangen - huidige kantelen vars
zweven pitch, roll, yaw;
int pitchzero, rollzero;
versnellingsmeter ingangen - huidige versnelling vars
float xin, yin, zin;

menselijke ingangen - besturingselement info vars
zweven pitchin rollin, yawin, zhuman;

willekeurige andere vars
zweven xaverage = 0, yaverage = 0;
int y = 0;
int bla;

evenredigheid-constanten zijn
zweven p = 2.5; P-evenredigheid-constante
zweven d = 0,5; D evenredigheid constante

VOID Setup {}
zhuman = 0;
Rollin = 0;
Serial.begin(9600);
voor (int x = 6; x < 10; x ++) {}
pinMode (x, OUTPUT);
}

Stuur bovengrens voor menselijke ingangen naar de motorsnelheid controllers
voor (int x = 6; x < 10; x ++) {}
pulsout(x,2000);
}
delay(5000);

krijgen van nullen voor pitch en roll menselijke ingangen
voor (int x = 0; x < 10; x ++) {}
y=y+analogRead(3);
}
pitchzero = y/10;
y = 0;
voor (int x = 0; x < 10; x ++) {}
y=y+analogRead(4);
}
rollzero = y/10;
}

void loop () {}
versnellingsmeter en gyro ingangen varieerden-232 tot en met 232?
Xin = ((0) analogRead-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;

krijgen menselijke ingangen via radio hier bereik van -30 tot 30 met uitzondering van zhuman die een ideale bereik van 1000-2000, slechts 2 pulsen per lus heeft
if(blah==0) {}
yawin = 0,06 * ((ondertekende int) pulseIn(2,HIGH)-1500);
pitchin = 0,06 * ((ondertekende int) pulseIn(3,HIGH)-1500);
blah = 1;
}
else {}
zhuman =(signed int) pulseIn(4,HIGH);
Rollin = 0,06 * ((ondertekende int) pulseIn(5,HIGH)-1400); 1400 in plaats van 1500 is om te corrigeren voor de ondermaatse motor #4 door trimmen in code
blah = 0;
}

gemiddeld, enz.
xaverage = SEN_CON * (xaverage + TIME_CON * worp) + (1 - SEN_CON) * xin;
yaverage = SEN_CON * (yaverage + TIME_CON * roll) + (1 - SEN_CON) * yin;

berekenen van de motor snelheden
if(zhuman<1150) {}
voor (int x = 0; x < 4; x ++) {}
snelheden [x] = zhuman;
}
}
else {}
Als (zhuman > 1450) {}
zhuman = 1450;
}
snelheden [0] = zhuman - p (xaverage - pitchin) - p*(yawin) - d * toonhoogte;
snelheden [1] = zhuman - p (pitchin - xaverage) - p*(yawin) + d * toonhoogte;
snelheden [2] = zhuman - p * (yaverage - rollin) + p*(yawin) - d * rollen;
snelheden [3] = zhuman - p (rollin - yaverage) + p*(yawin) + d * rollen;
}
instellen van de bovenste en onderste grenzen voor de snelheden van de motor (1000 = geen snelheid, 1600 = snelheid bovengrens, 2000 = maximale snelheid)
voor (int x = 0; x < 4; x ++) {}
snelheidslimiet tussen 1000 en 1600
Als (snelheden [x] < 1000) {}
snelheden [x] = 1000;
}
Als (snelheden [x] > 1600) {}
snelheden [x] = 1600;
}
}

pulsouts naar de motor snelheidsregelaars
voor (int x = 0; x < 4; x ++) {}
pulsout(x+6,speeds[x]);
}
}
VOID pulsout (int pin, int duur) {}
digitalWrite (pin, hoge);
delayMicroseconds(duration);
digitalWrite (pin, laag);
}