Étape 6: Code
Le code implémente essentiellement un régulateur PID pour l’inclinaison. Il a un certain terme intégral haut, ce qui provoque le bot de remettre les gaz quand il corrige pour un coup de pouce. Qui tend à null par tendance à vouloir du bot dériver le long, équilibrage debout mais non stationnaires. Il s’agit de la ligne de touche qui résume les termes de PID :
Vitesse = tilt_rads_estimate * TILT_FACT +
tilt_int_rads * TILT_INT_FACT +
gyro_rads_per_sec * D_TILT_FACT ;
Il y a plusieurs endroits que vous devrez peut-être modifier le code pour le faire fonctionner avec votre bot :
La constante GYRO_V_PER_DEG_PER_SEC définit combien la sortie de votre gyro particulier change pour un changement d’un degré par seconde en vitesse de rotation. J’ai trouvé que mine était à la fin de la courbe en cloche dans la feuille de données.
De même, la constante ACCEL_V_PER_G définit la valeur de l’accéléromètre.
Vous pouvez également utiliser la forme d’entrées les pots de modifier d’autres valeurs comme le décalage X, qui s’incline le bot en arrière ou en avant. Cela doit être défini correctement sinon le bot n’équilibre pas.
Le bouton poussoir provoque le bot passer en mode étalonnage pendant 10 secondes. Pour l’utiliser, posez le bot sur son dos et appuyez sur le bouton puis laissez-le pendant 10 secondes. Il s’étalonne automatiquement le point null taux de gyroscope.