Quadrotor (23 / 24 étapes)

Etape 23 : Code

Notre code représente un système de contrôle appelé PID (proportionnel intégral dérivé). Actuellement, il emploie seulement utilisation des composantes proportionnelles et dérivés. Avec notre code actuel, le quadrotor se stabilise assez bien dans l’air, mais il est un peu instable pendant le décollage. Toutefois, cette instabilité peut être atténuée en enlevant rapidement.

Pour trouver les montants actuels de l’inclinaison sur l’axe X et Y de données d’accéléromètre et gyroscope, nous avons utilisé un algorithme qui aurait les données provenant des précédentes en moyenne et combinez-le avec données de gyroscope pour atteindre une mesure d’angle qui était assez résistante à une accélération linéaire.

Nous faisons seulement 2 Pulsin commandes par boucle (au lieu de 4) afin de réduire le temps de la boucle à moitié, ce qui rend le système de contrôle de quadrotor beaucoup plus réceptif.

position neutre accéléromètre/gyroscope
#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

Vitesse du moteur vars
vitesses int [4] ;

entrées de gyro - actuelles incliner vars
flotter pitch, roulis, lacet ;
int pitchzero, rollzero ;
entrées d’accéléromètre - actuelle accélération vars
flotteur xin, yin, zin ;

entrées humaines - contrôle d’informations vars
float pitchin, rollin, youness, zhuman ;

aléatoire autres vars
flotteur Xmoyen = 0, yaverage = 0 ;
int y = 0 ;
int bla ;

constantes de proportionnalité
flotteur p = 2,5 ; Constante de proportionnalité P
flotteur d = 0,5 ; Constante de proportionnalité D

void setup() {}
Zhuman = 0 ;
Rollin = 0 ;
Serial.Begin(9600) ;
pour (int x = 6; x < 10; x ++) {}
pinMode (x, sortie) ;
}

Envoyer la limite supérieure pour les entrées humaines sur les contrôleurs de vitesse du moteur
pour (int x = 6; x < 10; x ++) {}
pulsout(x,2000) ;
}
Delay(5000) ;

obtenir des zéros pour pitch et rouler entrées humaines
pour (int x = 0; x < 10; x ++) {}
y=y+analogRead(3) ;
}
pitchzero = y/10 ;
y = 0 ;
pour (int x = 0; x < 10; x ++) {}
y=y+analogRead(4) ;
}
rollzero = y/10 ;
}

void loop () {}
accéléromètre et gyroscope entrées varient de-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 ;

obtention des intrants humains par le biais de radio ici plage de -30 à 30 à l’exception de zhuman qui a une plage idéale de 1000-2000, seulement 2 impulsions par boucle
{if(blah==0)}
youness = 0,06 * (pulseIn(2,HIGH)-1500) (int signé) ;
Pitchin = 0,06 * (pulseIn(3,HIGH)-1500) (int signé) ;
bla = 1 ;
}
else {}
Zhuman =(signed int) pulseIn(4,HIGH) ;
Rollin = 0,06 * (pulseIn(5,HIGH)-1400) (int signé) ; 1400 au lieu de 1500 vise à remédier à la faible puissance moteur #4 en taillant il dans le code
bla = 0 ;
}

en moyenne, etc..
Xmoyen = SEN_CON * (Xmoyen + TIME_CON * hauteur) + (1 - SEN_CON) * xin ;
yaverage = SEN_CON * (yaverage + TIME_CON * rouleau) + (1 - SEN_CON) * yin ;

calculer les vitesses du moteur
if(Zhuman<1150) {}
pour (int x = 0; x < 4; x ++) {}
vitesses [x] = zhuman ;
}
}
else {}
Si {(zhuman > 1450)
Zhuman = 1450 ;
}
vitesses [0] = zhuman - p * (Xmoyen - pitchin) - p*(yawin) - d * pitch ;
vitesses [1] = zhuman - p * (pitchin - Xmoyen) - p*(yawin) + d * pitch ;
vitesses [2] = zhuman p * (yaverage - rollin) - + p*(yawin) - d * rouleau ;
vitesse [3] = zhuman - p * (rollin - yaverage) + p*(yawin) + d * rouleau ;
}
définir les limites supérieures et inférieures pour les vitesses du moteur (1000 ne = aucune vitesse, 1600 = limite supérieure de la vitesse, 2000 = vitesse maximale possible)
pour (int x = 0; x < 4; x ++) {}
limite de vitesse entre 1000 et 1600
Si (vitesses [x] < 1000) {}
vitesses [x] = 1000 ;
}
Si (vitesses [x] > 1600) {}
vitesses [x] = 1600 ;
}
}

pulsouts aux contrôleurs de vitesse du moteur
pour (int x = 0; x < 4; x ++) {}
pulsout(x+6,speeds[x]) ;
}
}
void pulsout (int pin, int durée) {}
digitalWrite (tige, haute) ;
delayMicroseconds(duration) ;
digitalWrite (broches, faible) ;
}