MobBob :Robot Arduino DIY contrôlé par smartphone Android
Composants et fournitures
Applications et services en ligne
À propos de ce projet
J'ai réalisé ce robot selon les instructions du site Une excellente idée de symbiose entre Arduino et Android.
MobBob est un robot contrôlé par smartphone. En exploitant la puissance de votre smartphone, MobBob est un robot qui marche et qui parle avec reconnaissance vocale et vision par ordinateur.
Pièces imprimées en 3D :https://www.thingverse.com/thing:715688
Fichier Android.apk :https://apkpure.com/mobbob/com.cevinius.MobBob
Dans le code, vous voudrez :
- Mettez à jour les variables d'épingle pour qu'elles correspondent à votre build
- Ajustez le centre du servo, les valeurs min et max
- Réglez "FRONT_JOINT_HIPS" sur 1 ou -1 selon la façon dont vos servos de hanche sont montés. Je le monte avec le servo axe à l'avant de MobBob. Pour cette configuration, définissez cette valeur sur 1.
Code
codeArduino
/* * =============================================================* Programme de contrôle MobBob - Version du logiciel Bluetooth série * par Kevin Chan (alias Cevinius) * ==============================================================* * Ce programme permet à MobBob d'être contrôlé à l'aide de commandes série. Dans cette version du code, les commandes * sont reçues via un port série logiciel, avec des broches définies dans le #define près du haut. * Cela signifie que vous pouvez utiliser n'importe quelle carte compatible Arduino et brancher une carte Bluetooth dans les broches définies pour * la série logicielle. (Par opposition à l'autre version de celui-ci conçu pour la carte Bluno de DFRobot.) * * Ce programme est long et contient 2 composants principaux - un programme d'animation servo fluide et un programme d'analyseur de commandes * série. * * Système d'animation * ================* Le programme d'animation est conçu pour animer les tableaux d'images clés servo en douceur. Le code essaie de faire de son mieux pour être facile à utiliser. * * Le système d'animation ne mettra en file d'attente qu'une seule commande. c'est-à-dire qu'une commande peut être en cours d'exécution, * et une commande peut être mise en file d'attente. Si vous envoyez plus de commandes, elles écraseront la commande en file d'attente. * * Le système d'animation attendra par défaut la fin de l'animation en cours avant de commencer la suivante. Cela * signifie que si les données d'animation se terminent avec le robot dans sa pose de base, les choses se joindront en douceur. Pour * prendre en charge cela, le système d'animation dispose également d'une fonctionnalité où une animation peut avoir une "séquence de fin" * pour remettre le robot dans la pose de base. Cette fonctionnalité est utilisée pour les animations de marche avant/arrière. * Ces animations ont une séquence finale qui remet le robot dans la pose de base. * * Lorsqu'une animation est terminée, le système d'animation émet une chaîne de réponse sur le port série. * Cela permet aux appelants de savoir quand les animations qu'ils ont demandées sont terminées. C'est utile * pour les utilisateurs de séquencer des animations - en attendant la fin de l'une avant d'en commencer une autre. * * Le code d'animation a de nombreuses variables pour permettre aux choses d'être modifiées. Par exemple. Fréquence de mise à jour, broches arduino, etc. * * Le format de tableau de données d'animation est également conçu pour être facile à modifier à la main. * * Command Parser * ==============* Ce système analyse les commandes reçues via série et les traite. Les commandes incluent une * pour régler directement les positions des servos, ainsi que des commandes pour déclencher des animations et des promenades prédéfinies. * * Ainsi, les utilisateurs qui ne veulent pas se soucier des détails de la marche peuvent simplement utiliser les promenades/animations prédéfinies. * Et les utilisateurs qui souhaitent un contrôle total sur les servos (pour créer de nouvelles animations à la volée) peuvent également le faire. * * Comme mentionné ci-dessus, ces commandes peuvent être utilisées de manière interactive à partir du moniteur série Arduino. Ils peuvent également être * envoyés via Bluetooth LE (lorsqu'un Bluno est utilisé). L'application pour téléphone enverra les commandes via Bluetooth LE au * Bluno. * * Commandes générales :* ----------------- * Vérification Prêt/OK : * Vérification de l'état. La réponse est renvoyée immédiatement pour vérifier si le contrôleur fonctionne. * * Set Servo : * temps - temps d'interpolation aux angles spécifiés, 0 sautera immédiatement aux angles * leftHip - microsecs du centre. -ve est hip in, +ve est hip out * leftFoot - microsecs de plat. -ve est le pied vers le bas, +ve est le pied vers le haut * rightHip - à quelques microsecondes du centre. -ve est hip in, +ve est hip out * rightFoot - microsecs de plat. -ve est le pied vers le bas, +ve est le pied vers le haut * Cette commande est utilisée pour obtenir un contrôle total sur les servos. Vous pouvez interpoler le robot de sa * pose actuelle à la pose spécifiée sur la durée spécifiée. * * Stop/Reset : * Arrête le robot après l'animation en cours. Peut être utilisé pour arrêter les animations définies en boucle * indéfiniment. Cela peut également être utilisé pour mettre le robot dans sa pose de base (debout droit) * * Arrêt immédiat : * Arrête le robot immédiatement sans attendre la fin de l'animation en cours. Ce * interrompt l'animation en cours du robot. Potentiellement, le robot peut être à mi-animation * et dans une pose instable, alors soyez prudent lorsque vous l'utilisez. * * Commandes de marche standard :* ----------------------- * Avant :, -1 signifie continu, 0 ou aucun paramètre n'est le même que 1 fois. * En arrière :, -1 signifie continu, 0 ou aucun paramètre équivaut à 1 fois. * Tourner à gauche :, -1 signifie continu, 0 ou aucun paramètre équivaut à 1 fois. * Tourner à droite :, -1 signifie continu, 0 ou aucun paramètre équivaut à 1 fois. * * Commandes d'animation amusantes :* ----------------------- * Shake Head :, -1 signifie continu, 0 ou aucun paramètre est le même que 1 fois. * * Rebond :, -1 signifie continu, 0 ou aucun paramètre équivaut à 1 fois. * * Wobble :, -1 signifie continu, 0 ou aucun paramètre équivaut à 1 fois. * Wobble Left :, -1 signifie continu, 0 ou aucun paramètre équivaut à 1 fois. * Wobble Right :, -1 signifie continu, 0 ou aucun paramètre équivaut à 1 fois. * * Tap Feet :, -1 signifie continu, 0 ou aucun paramètre équivaut à 1 fois. * Appuyez sur le pied gauche :, -1 signifie continu, 0 ou aucun paramètre équivaut à 1 fois. * Appuyez sur le pied droit :, -1 signifie continu, 0 ou aucun paramètre équivaut à 1 fois. * * Shake Legs :, -1 signifie continu, 0 ou aucun paramètre équivaut à 1 fois. * Secouer la jambe gauche :, -1 signifie continu, 0 ou aucun paramètre équivaut à 1 fois. * Secouez la jambe droite :, -1 signifie continu, 0 ou aucun paramètre équivaut à 1 fois. * * En outre, le code renverra une chaîne de réponse sur Serial lorsque les commandes auront * fini de s'exécuter. * * Si la commande s'est terminée normalement, la chaîne de réponse est le code de commande sans * paramètres. Par exemple. Quand il a fini d'avancer, il enverra la réponse "". * * Si une commande a été interrompue avec , l'animation en cours peut avoir été arrêtée à mi-chemin. * Dans ce cas, le robot peut être dans une étrange pose à mi-chemin, et finishAnims peut ne pas avoir été * joué. Pour informer l'utilisateur que cela s'est produit, la chaîne de réponse aura le paramètre * -1. Par exemple, si une marche était arrêtée à mi-chemin en utilisant , la chaîne de réponse serait * pour indiquer que la marche s'est arrêtée, mais qu'elle a été arrêtée à mi-chemin. * (Remarque :si vous utilisez pour arrêter, cela attendra la fin du cycle d'animation en cours * avant de s'arrêter. Ainsi, les animations ne seront pas arrêtées à mi-chemin dans ce cas.) * * Parce que les réponses sont envoyées après un l'animation est terminée, l'expéditeur de la commande peut * rechercher les chaînes de réponse pour déterminer quand le robot est prêt pour une nouvelle commande. * Par exemple. Si vous utilisez la commande , la chaîne de réponse n'est pas envoyée tant que les 3 étapes * (et terminer l'animation) ne sont pas terminées. Ainsi, l'expéditeur de la commande peut attendre la réponse * chaîne avant de dire au robot de faire la chose suivante. */ #include #include //-------------------------------- -------------------------------------------------- // Vitesse de communication série - Réglez ceci pour votre carte série (bluetooth).//------------------------------- -------------------------------------------------- -// Vitesse de communication série avec la carte bluetooth.// Certaines cartes sont par défaut à 9600. La carte que j'ai a une valeur par défaut de 115200.#define SERIAL_SPEED 115200// Configurez un port série logiciel sur ces broches.const int rxPin =11; // broche utilisée pour recevoir dataconst int txPin =12; // broche utilisée pour envoyer des donnéesSoftwareSerial softwareSerial(rxPin, txPin);//------------------------------------------------- ------------------------------------------------// Configurer les broches Arduino - Définissez-les pour votre robot particulier.//------------------------------------- -------------------------------------------------------------const int SERVO_LEFT_HIP =5;const int SERVO_LEFT_FOOT =2;const int SERVO_RIGHT_HIP =3;const int SERVO_RIGHT_FOOT =4;// Je veux que ce code soit utilisable sur tous les bipèdes à 4 servos ! (Comme Bob, MobBob) // J'ai remarqué que certaines versions montent les servos de hanche d'une manière différente// de celle de MobBob. Ce paramètre vous permet de configurer le code // pour l'un ou l'autre style de construction. // 1 pour le style MobBob hanches face à l'avant (articulation vers l'avant)// -1 pour les hanches face à l'arrière style Bob (articulation vers l'arrière)#define FRONT_JOINT_HIPS 1//------------------- -------------------------------------------------- -------------// Constantes Servo Max/Min/Centre - Réglez-les pour votre robot particulier.//------------------ -------------------------------------------------- --------------const int LEFT_HIP_CENTRE =1580;const int LEFT_HIP_MIN =LEFT_HIP_CENTRE - 500;const int LEFT_HIP_MAX =LEFT_HIP_CENTRE + 500;const int LEFT_FOOT_CENTRE =1410;const - 500_MIN LEFT_CENTRE =LEFT_CENTRE + 500;const int LEFT_FOOT_CENTRE =1410;const - 500_MIN LEFT_FOOT =const int LEFT_FOOT_MAX =LEFT_FOOT_CENTRE + 500;const int RIGHT_HIP_CENTRE =1500;const int RIGHT_HIP_MIN =RIGHT_HIP_CENTRE - 500;const int RIGHT_HIP_MAX =RIGHT_HIP_CENTRE + 500;const int RIGHT_FOOT_CENTRE =1FOOT_CENTRE 465;const int RIGHT_FOOT_MIN =RIGHT_FOOT_CENTRE - 500;const int RIGHT_FOOT_MAX =RIGHT_FOOT_CENTRE + 500;//------------------------------ ------------------------------------------------// Fonctions d'assistance pour aider à calculer les valeurs des articulations d'une manière plus conviviale.// Vous pouvez ajuster les signes ici si les servos sont configurés d'une manière différente.// La mise à jour ici signifie que les données d'animation n'ont pas besoin d'être modifiées si le // les servos sont configurés différemment.// (par exemple Les servos de hanche d'origine de Bob sont à l'envers de ceux de MobBob.) //// (De plus, j'ai du mal à me souvenir des signes à utiliser pour chaque servo car ils // sont différents pour les hanches gauche/droite et pour les pieds gauche/droit.) //------------------------------------------------ ------------------------------int LeftHipCentre() { return LEFT_HIP_CENTRE; }int LeftHipIn(int millisecs) { return LEFT_HIP_CENTRE + (FRONT_JOINT_HIPS * millisecs); }int LeftHipOut(int millisecs) { return LEFT_HIP_CENTRE - (FRONT_JOINT_HIPS * millisecs); }int RightHipCentre() { return RIGHT_HIP_CENTRE; }int RightHipIn(int millisecs) { return RIGHT_HIP_CENTRE - (FRONT_JOINT_HIPS * millisecs); }int RightHipOut(int millisecs) { return RIGHT_HIP_CENTRE + (FRONT_JOINT_HIPS * millisecs); }int LeftFootFlat() { return LEFT_FOOT_CENTRE; }int LeftFootUp(int millisecs) { return LEFT_FOOT_CENTRE - millisecs; }int LeftFootDown(int millisecs) { return LEFT_FOOT_CENTRE + millisecs; }int RightFootFlat() { return RIGHT_FOOT_CENTRE; }int RightFootUp(int millisecs) { return RIGHT_FOOT_CENTRE + millisecs; }int RightFootDown(int millisecs) { return RIGHT_FOOT_CENTRE - millisecs; }//------------------------------------------------ ----------------------------------// Données d'animation d'images clés pour la marche standard et autres animations d'asservissement// // Le format est { , , , , }// millisecondes - temps d'interpolation aux positions de cette image clé. Par exemple. 500 signifie qu'il faudra 500 ms pour passer de// la position du robot au début de cette image à la position spécifiée dans cette image // leftHipMicros - position de la hanche gauche en servo microsec. // leftFootMicros - position de la hanche gauche en servo microsecs.// rightHipMicros - position de la hanche gauche en servo microsecs.// rightFootMicros - position de la hanche gauche en servo microsecs.// // Les valeurs micro servo prennent en charge une valeur spéciale de -1. Si cette valeur est donnée, il indique // au code d'animation d'ignorer ce servo dans cette image clé. c'est-à-dire que ce servo/serveur restera dans la position qu'il avait au début de cette image clé.//// De plus, le premier élément dans les données d'animation arry est spécial. C'est un élément de métadonnées.// Le premier élément est { , 0, 0, 0, 0 }, qui nous indique le nombre d'images// dans l'animation. Ainsi, la première image clé réelle est dans animData[1], et la dernière image clé // est dans animData[]. (Où est la valeur dans animData[0][0].)//-------------------------------------------- -------------------------------------------------- ---// Constantes pour rendre l'accès aux tableaux d'images clés plus lisible.const int TWEEN_TIME_VALUE =0;const int LEFT_HIP_VALUE =1;const int LEFT_FOOT_VALUE =2;const int RIGHT_HIP_VALUE =3;const int RIGHT_FOOT_VALUE =4;// Constantes utilisées dans l'animation de marche data.const int FOOT_DELTA =150;const int HIP_DELTA =FRONT_JOINT_HIPS * 120;// Passe à la position droite debout par défaut. Utilisé par stopAnim().int standStraightAnim[][5] ={ // Métadonnées. Le premier élément est le nombre d'images. { 1, 0, 0, 0, 0 }, // Pieds à plat, Pieds égaux { 300, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() }} ;// Avant cela, amenez le robot à Pieds plats, pieds égaux (c'est-à-dire standStraightAnim).int walkForwardAnim[][5] ={ // Métadonnées. Le premier élément est le nombre d'images. { 8, 0, 0, 0, 0 }, // Inclinaison vers la gauche, pieds égaux { 300, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootDown(FOOT_DELTA) }, // Inclinaison vers la gauche, pied droit avant { 300, LeftHipIn(HIP_DELTA), LeftFootUp(FOOT_DELTA), RightHipOut(HIP_DELTA), RightFootDown(FOOT_DELTA) }, // Pieds à plat, Pied droit en avant { 300, LeftHipIn(HIP_DELTA), LeftFootFlat(), RightHipOut(HIP_DELTA), RightFootFlat() }, // Inclinaison vers la droite, Pied droit en avant { 300, LeftHipIn(HIP_DELTA), LeftFootDown(FOOT_DELTA), RightHipOut(HIP_DELTA), RightFootUp(FOOT_DELTA) }, // Inclinaison vers la droite, Pieds égaux { 300, LeftHipCentre (), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) }, // Inclinaison vers la droite, pied gauche en avant { 300, LeftHipOut(HIP_DELTA), LeftFootDown(FOOT_DELTA), RightHipIn(HIP_DELTA), RightFootUp(FOOT_DELTA) , // Pieds à plat, pied gauche en avant { 300, LeftHipOut(HIP_DELTA), LeftFootFlat(), RightHipIn(HIP_DELTA), RightFootFlat() }, // Inclinaison vers la gauche, pied gauche en avant { 300, LeftHipOut(HIP_DEL TA), LeftFootUp(FOOT_DELTA), RightHipIn(HIP_DELTA), RightFootDown(FOOT_DELTA) }} ;// Avant cela, amenez le robot à Pieds plats, pieds égaux (c'est-à-dire standStraightAnim).int walkBackwardAnim[][5] ={ // Métadonnées. Le premier élément est le nombre d'images. { 8, 0, 0, 0, 0 }, // Inclinaison vers la gauche, pieds égaux { 300, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootDown(FOOT_DELTA) }, // Inclinaison vers la gauche, pied gauche avant { 300, LeftHipOut(HIP_DELTA), LeftFootUp(FOOT_DELTA), RightHipIn(HIP_DELTA), RightFootDown(FOOT_DELTA) }, // Pieds à plat, Pied gauche en avant { 300, LeftHipOut(HIP_DELTA), LeftFootFlat(), RightHipIn(HIP_DELTA), RightFootFlat() }, // Inclinaison vers la droite, Pied gauche en avant { 300, LeftHipOut(HIP_DELTA), LeftFootDown(FOOT_DELTA), RightHipIn(HIP_DELTA), RightFootUp(FOOT_DELTA) }, // Inclinaison vers la droite, Pieds égaux { 300, LeftHipCentre (), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) }, // Inclinaison vers la droite, Pied droit en avant { 300, LeftHipIn(HIP_DELTA), LeftFootDown(FOOT_DELTA), RightHipOut(HIP_DELTA), RightFootUp(FOOT_DELTA) , // Pieds à plat, Pied droit en avant { 300, LeftHipIn(HIP_DELTA), LeftFootFlat(), RightHipOut(HIP_DELTA), RightFootFlat() }, // Inclinaison vers la gauche, Pied droit en avant { 300, LeftHipIn(HIP_DELTA ), LeftFootUp(FOOT_DELTA), RightHipOut(HIP_DELTA), RightFootDown(FOOT_DELTA) }} ;// Finish walk anim ramène le robot de la fin de walkForwardAnim/walkBackwardAnim à standStraightAnim.int walkEndAnim[][5] ={ // Métadonnées . Le premier élément est le nombre d'images. { 2, 0, 0, 0, 0 }, // Inclinaison vers la gauche, Pieds pairs { 300, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootDown(FOOT_DELTA) }, // Pieds à plat, Pieds pairs { 300, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() }} ;// Avant cela, placez le robot à Pieds plats, Pieds égaux (c'est-à-dire standStraightAnim).int turnLeftAnim[][5] ={ / / Métadonnées. Le premier élément est le nombre d'images. { 6, 0, 0, 0, 0 }, // Inclinaison vers la gauche, Pieds égaux { 300, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootDown(FOOT_DELTA) }, // Inclinaison vers la gauche, Tourner à gauche hanche, tourner la hanche droite { 300, LeftHipIn(HIP_DELTA), LeftFootUp(FOOT_DELTA), RightHipIn(HIP_DELTA), RightFootDown(FOOT_DELTA) }, // Pieds à plat, tourner la hanche gauche, tourner la hanche droite { 300, LeftHipIn(HIP_DELTA), LeftFootFlat (), RightHipIn(HIP_DELTA), RightFootFlat() }, // Inclinaison vers la droite, Tourner la hanche gauche, Tourner la hanche droite { 300, LeftHipIn(HIP_DELTA), LeftFootDown(FOOT_DELTA), RightHipIn(HIP_DELTA), RightFootUp(FOOT_DELTA) }, // Inclinaison vers la droite, Pieds pairs { 300, LeftHipCentre(), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) }, // Pieds à plat, Pieds pairs { 300, LeftHipCentre(), LeftFootFlat(), RightHipCentre( ), RightFootFlat() }};// Avant cela, amenez le robot à Pieds plats, pieds égaux (c'est-à-dire standStraightAnim).int turnRightAnim[][5] ={ // Métadonnées. Le premier élément est le nombre d'images. { 6, 0, 0, 0, 0 }, // Inclinaison vers la droite, Pieds égaux { 300, LeftHipCentre(), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) }, // Inclinaison vers la droite, Tourner à gauche hanche, tourner la hanche droite { 300, LeftHipIn(HIP_DELTA), LeftFootDown(FOOT_DELTA), RightHipIn(HIP_DELTA), RightFootUp(FOOT_DELTA) }, // Pieds à plat, tourner la hanche gauche, tourner la hanche droite { 300, LeftHipIn(HIP_DELTA), LeftFootFlat (), RightHipIn(HIP_DELTA), RightFootFlat() }, // Inclinaison vers la gauche, Tourner la hanche gauche, Tourner la hanche droite { 300, LeftHipIn(HIP_DELTA), LeftFootUp(FOOT_DELTA), RightHipIn(HIP_DELTA), RightFootDown(FOOT_DELTA) }, // Inclinaison vers la gauche, Pieds pairs { 300, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootDown(FOOT_DELTA) }, // Pieds à plat, Pieds pairs { 300, LeftHipCentre(), LeftFootFlat(), RightHipCentre( ), RightFootFlat() }};// Secouer la tête anim. Gauche droite rapidement pour émuler secouer head.int shakeHeadAnim[][5] ={ // Métadonnées. Le premier élément est le nombre d'images. { 4, 0, 0, 0, 0 }, // Pieds à plat, Twist à gauche { 150, LeftHipOut(HIP_DELTA), LeftFootFlat(), RightHipIn(HIP_DELTA), RightFootFlat() }, // Pieds à plat, Pieds pairs { 150 , LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() }, // Pieds à plat, Twist right { 150, LeftHipIn(HIP_DELTA), LeftFootFlat(), RightHipOut(HIP_DELTA), RightFootFlat() }, // Pieds plat, Pieds pairs { 150, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() } } ; // Wobble anim. Inclinez à gauche et à droite pour faire un wobble.int amusant wobbleAnim[][5] ={ // Métadonnées. Le premier élément est le nombre d'images. { 4, 0, 0, 0, 0 }, // Inclinaison vers la gauche, Pieds pairs { 300, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootDown(FOOT_DELTA) }, // Pieds à plat, Pieds pairs { 300 , LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() }, // Inclinaison à droite, Pieds égaux { 300, LeftHipCentre(), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) }, // Pieds plat, pieds pairs { 300, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() } } ; // Wobble anim gauche. Inclinez vers la gauche et vers l'arrière.int wobbleLeftAnim[][5] ={ // Métadonnées. Le premier élément est le nombre d'images. { 2, 0, 0, 0, 0 }, // Inclinaison vers la gauche, Pieds pairs { 300, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootDown(FOOT_DELTA) }, // Pieds à plat, Pieds pairs { 300 , LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() },};// Wobble anim à droite. Inclinez vers la droite et vers l'arrière.int wobbleRightAnim[][5] ={ // Métadonnées. Le premier élément est le nombre d'images. { 2, 0, 0, 0, 0 }, // Inclinaison à droite, Pieds pairs { 300, LeftHipCentre(), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) }, // Pieds à plat, Pieds pairs { 300 , LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() } };// Appuyez sur les pieds anim. Appuyez sur les deux pieds.int tapFeetAnim[][5] ={ // Métadonnées. Le premier élément est le nombre d'images. { 2, 0, 0, 0, 0 }, // Lever les deux pieds, Pieds pairs { 500, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) }, // Pieds à plat, Pieds pairs { 500, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() },};// Appuyez sur le pied gauche anim.int tapLeftFootAnim[][5] ={ // Métadonnées. Le premier élément est le nombre d'images. { 2, 0, 0, 0, 0 }, // Lever le pied gauche, Pieds pairs { 500, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootFlat() }, // Pieds à plat, Pieds pairs { 500 , LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() },};// Appuyez sur le pied droit anim.int tapRightFootAnim[][5] ={ // Métadonnées. Le premier élément est le nombre d'images. { 2, 0, 0, 0, 0 }, // Lever le pied droit, Pieds pairs { 500, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootUp(FOOT_DELTA) }, // Pieds à plat, Pieds pairs { 500 , LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() },};// Rebondir de haut en bas anim.int bounceAnim[][5] ={ // Métadonnées. Le premier élément est le nombre d'images. { 2, 0, 0, 0, 0 }, // Lever les deux pieds, Pieds pairs { 500, LeftHipCentre(), LeftFootDown(300), RightHipCentre(), RightFootDown(300) }, // Pieds à plat, Pieds pairs { 500, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() },};// Shake Legs Animation.int shakeLegsAnim[][5] ={ // Métadonnées. Le premier élément est le nombre d'images. { 14, 0, 0, 0, 0 }, // Inclinaison vers la gauche, Pieds égaux { 300, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootDown(FOOT_DELTA) }, // Inclinaison vers la gauche, Hanche droite en { 100, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipIn(HIP_DELTA), RightFootDown(FOOT_DELTA) }, // Inclinaison vers la gauche, Pieds pairs { 100, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootDown(FOOT}_DELTA) , // Inclinaison vers la gauche, hanche droite vers l'extérieur { 100, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipOut(HIP_DELTA), RightFootDown(FOOT_DELTA) }, // Inclinaison vers la gauche, pieds pairs { 100, LeftHipCentre(), LeftFootUp(FOOT_DELTA) , RightHipCentre(), RightFootDown(FOOT_DELTA) }, // Inclinaison vers la gauche, Hanche droite vers { 100, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipIn(HIP_DELTA), RightFootDown(FOOT_DELTA) }, // Inclinaison vers la gauche, Pieds égaux { 100, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootDown(FOOT_DELTA) }, // Pieds à plat, Pieds pairs { 300, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() }, // gréement inclinable ht, Pieds pairs { 300, LeftHipCentre(), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) }, // Inclinaison à droite, Hanche gauche en { 100, LeftHipIn(HIP_DELTA), LeftFootDown(FOOT_DELTA), RightHipCentre() , RightFootUp(FOOT_DELTA) }, // Inclinaison vers la droite, Pieds égaux { 100, LeftHipCentre(), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) }, // Inclinaison vers la droite, Hanche gauche en sortie { 100, LeftHipOut(HIP_DELTA) ), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) }, // Inclinaison à droite, Pieds égaux { 100, LeftHipCentre(), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) }, // Pieds à plat , Pieds pairs { 300, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() } };// Shake Left Leg Animation.int shakeLeftLegAnim[][5] ={ // Métadonnées. Le premier élément est le nombre d'images. { 12, 0, 0, 0, 0 }, // Inclinaison vers la droite, Pieds égaux { 300, LeftHipCentre(), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) }, // Inclinaison vers la droite, Hanche gauche en { 100, LeftHipIn(HIP_DELTA), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) }, // Inclinaison à droite, Pieds pairs { 100, LeftHipCentre(), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT}_DELTA) , // Inclinaison vers la droite, hanche gauche vers l'extérieur { 100, LeftHipOut(HIP_DELTA), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) }, // Inclinaison vers la droite, pieds pairs { 100, LeftHipCentre(), LeftFootDown(FOOT_DELTA) , RightHipCentre(), RightFootUp(FOOT_DELTA) }, // Inclinaison vers la droite, Hanche gauche vers { 100, LeftHipIn(HIP_DELTA), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) }, // Inclinaison vers la droite, Pieds égaux { 100, LeftHipCentre(), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) }, // Inclinaison à droite, hanche gauche en dehors { 100, LeftHipOut(HIP_DELTA), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFo otUp(FOOT_DELTA) }, // Inclinaison vers la droite, Pieds égaux { 100, LeftHipCentre(), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) }, // Inclinaison vers la droite, Hanche gauche en { 100, LeftHipIn(HIP_DELTA) , LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) }, // Inclinaison à droite, Pieds égaux { 100, LeftHipCentre(), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) }, // Pieds à plat, Pieds pairs { 300, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() } } ;// Shake Right Leg Animation.int shakeRightLegAnim[][5] ={ // Métadonnées. Le premier élément est le nombre d'images. { 12, 0, 0, 0, 0 }, // Inclinaison vers la gauche, Pieds égaux { 300, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootDown(FOOT_DELTA) }, // Inclinaison vers la gauche, Hanche droite en { 100, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipIn(HIP_DELTA), RightFootDown(FOOT_DELTA) }, // Inclinaison vers la gauche, Pieds pairs { 100, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootDown(FOOT}_DELTA) , // Inclinaison vers la gauche, hanche droite vers l'extérieur { 100, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipOut(HIP_DELTA), RightFootDown(FOOT_DELTA) }, // Inclinaison vers la gauche, pieds pairs { 100, LeftHipCentre(), LeftFootUp(FOOT_DELTA) , RightHipCentre(), RightFootDown(FOOT_DELTA) }, // Inclinaison vers la gauche, Hanche droite vers { 100, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipIn(HIP_DELTA), RightFootDown(FOOT_DELTA) }, // Inclinaison vers la gauche, Pieds égaux { 100, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootDown(FOOT_DELTA) }, // Inclinaison vers la gauche, hanche droite vers l'extérieur { 100, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipOut(HIP_DELTA), RightFootDown (FOOT_DELTA) }, // Inclinaison vers la gauche, Pieds égaux { 100, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootDown(FOOT_DELTA) }, // Inclinaison vers la gauche, Hanche droite en { 100, LeftHipCentre(), LeftFootUp (FOOT_DELTA), RightHipIn(HIP_DELTA), RightFootDown(FOOT_DELTA) }, // Inclinaison à gauche, Pieds égaux { 100, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootDown(FOOT_DELTA) }, // Pieds à plat, Pieds même { 300, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() }, } ;//------------------------- -------------------------------------------------- -------// Données d'animation dynamique spéciales pour le réglage/l'interpolation des positions des servos.//---------------------------- -------------------------------------------------- ----// Ce sont 2 données d'animation spéciales que nous utilisons pour la fonction SetServos(). Ils ont // un seul cadre. Ceux-ci modifieront les données de ces données d'animation et les joueront pour // déplacer le servos.int setServosAnim1[][5] ={ // Metadata. Le premier élément est le nombre d'images. { 1, 0, 0, 0, 0 }, // Inclinaison vers la gauche, Pieds égaux { 0, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() }};int setServosAnim2[][5] ={ / / Métadonnées. Le premier élément est le nombre d'images. { 1, 0, 0, 0, 0 }, // Inclinaison vers la gauche, Pieds égaux { 0, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() }} ; //-------- -------------------------------------------------- ------------------------// Variables d'asservissement//-------------------- -------------------------------------------------- ------------Servo servoLeftHip;Servo servoLeftFoot;Servo servoRightHip;Servo servoRightFoot;//------------------------ -------------------------------------------------- --------// State variables for playing animations.//-------------------------------- -------------------------------------------------- // Milliseconds between animation updates.const int millisBetweenAnimUpdate =20;// Time when we did the last animation update.long timeAtLastAnimUpdate;// Related to currently playing anim.int (*currAnim)[5]; // Current animation we're playing.int (*finishAnim)[5]; // Animation to play when the currAnim finishes or is stopped.long timeAtStartOfFrame; // millis() at last keyframe - frame we're lerping fromint targetFrame; // Frame we are lerping toint animNumLoops; // Number of times to play the animation. -1 means loop forever.char animCompleteStr[3] ="--"; // This is a 2 character string. When the anim is complete, // we print out the status as "<" + animComplereStr + ">".// Related to anim queue. I.e. Next anim to play.bool animInProgress; // Whether an animation is playingint (*nextAnim)[5]; // This is the next animation to play once the current one is done. // i.e. It's like a queue of size 1! // If curr is non-looping, we play this at the end of the current anim. // If curr is looping, this starts at the end of the current loop, // replacing curr anim. // If nothing is playing, this starts right away. int (*nextFinishAnim)[5]; // This is the finish animation for the queued animation.int nextAnimNumLoops; // Number of times to play the animation. -1 means loop forever.char nextAnimCompleteStr[3] ="--"; // This is a 2 character string. When the anim is complete, // we print out the status as "<" + animComplereStr + ">".bool interruptInProgressAnim; // Whether to change anim immediately, interrupting the current one.// Curr servo positionsint currLeftHip;int currLeftFoot;int currRightHip;int currRightFoot;// Servo positions at start of current keyframeint startLeftHip;int startLeftFoot;int startRightHip;int startRightFoot;//-------------------------------------------------------------------------------// Parser Variables//-------------------------------------------------------------------------------// Constant delimiter tag charsconst char START_CHAR ='<';const char END_CHAR ='>';const char SEP_CHAR =',';// Constants and a variable for the parser state.const int PARSER_WAITING =0; // Waiting for '<' to start parsing.const int PARSER_COMMAND =1; // Reading the command string.const int PARSER_PARAM1 =2; // Reading param 1.const int PARSER_PARAM2 =3; // Reading param 2.const int PARSER_PARAM3 =4; // Reading param 3.const int PARSER_PARAM4 =5; // Reading param 3.const int PARSER_PARAM5 =6; // Reading param 3.const int PARSER_EXECUTE =7; // Finished parsing a command, so execute it.// Current parser state.int currParserState =PARSER_WAITING; // String for storing the command. 2 chars for the command and 1 char for '\0'.// We store the command here as we're parsing.char currCmd[3] ="--";// For tracking which letter we are in the command.int currCmdIndex;// Max command length.const int CMD_LENGTH =2;// Current param values. Store them here after we parse them.int currParam1Val;int currParam2Val;int currParam3Val;int currParam4Val;int currParam5Val;// Variable for tracking which digit we're parsing in a param.// We use this to convert the single digits back into a decimal value.int currParamIndex;// Whether the current param is negative.boolean currParamNegative;// Max parameter length. Stop parsing if it exceeds this.const int MAX_PARAM_LENGTH =6;//===============================================================================// Arduino setup() and loop().//===============================================================================void setup() { // Setup the main serial port softwareSerial.begin(SERIAL_SPEED); // Setup the Servos servoLeftHip.attach( SERVO_LEFT_HIP, LEFT_HIP_MIN, LEFT_HIP_MAX); servoLeftFoot.attach( SERVO_LEFT_FOOT, LEFT_FOOT_MIN, LEFT_FOOT_MAX); servoRightHip.attach( SERVO_RIGHT_HIP, RIGHT_HIP_MIN, RIGHT_HIP_MAX); servoRightFoot.attach(SERVO_RIGHT_FOOT, RIGHT_FOOT_MIN, RIGHT_FOOT_MAX); // Set things up for the parser. setup_Parser(); // Set things up for the animation code. setup_Animation();}void loop() { // Update the parser. loop_Parser(); // Update the animation. loop_Animation();}//===============================================================================// Related to the parser//===============================================================================// Sets up the parser stuff. Called in setup(). Should not be called elsewhere.void setup_Parser(){ // Wait for first command. currParserState =PARSER_WAITING; // Print this response to say we've booted and are ready. softwareSerial.println("");}// Loop() for the parser stuff. Called in loop(). Should not be called elsewhere.void loop_Parser(){ //--------------------------------------------------------- // PARSER // // If there is data, parse it and process it. //--------------------------------------------------------- // Read from pin serial port and write it out on USB port. if (softwareSerial.available()> 0) { char c =softwareSerial.read(); // If we're in WAITING state, look for the START_CHAR. if (currParserState ==PARSER_WAITING) { // If it's the START_CHAR, move out of this state... if (c ==START_CHAR) { // Start parsing the command. currParserState =PARSER_COMMAND; // Reset thing ready for parsing currCmdIndex =0; currCmd[0] ='-'; currCmd[1] ='-'; currParam1Val =0; currParam2Val =0; currParam3Val =0; currParam4Val =0; currParam5Val =0; } // Otherwise, stay in this state. } // In the state to look for the command. else if (currParserState ==PARSER_COMMAND) { // Else if it's a separator, parse parameter 1. But make sure it's not // empty, or else it's a parse error. if (c ==SEP_CHAR) { if (currCmdIndex ==CMD_LENGTH) { currParserState =PARSER_PARAM1; currParamIndex =0; currParamNegative =false; } else { currParserState =PARSER_WAITING; } } // Else if it's the end char, there are no parameters, so we're ready to // process. But make sure it's not empty. Otherwise, it's a parse error. else if (c ==END_CHAR) { if (currCmdIndex ==CMD_LENGTH) { currParserState =PARSER_EXECUTE; } else { currParserState =PARSER_WAITING; } } // If we've got too many letters here, we have a parse error, // so abandon and go back to PARSER_WAITING else if ( (currCmdIndex>=CMD_LENGTH) || (c <'A') || (c> 'Z') ) { currParserState =PARSER_WAITING; } // Store the current character. else { currCmd[currCmdIndex] =c; currCmdIndex++; } } // In the state to parse param 1. else if (currParserState ==PARSER_PARAM1) { // Else if it's a separator, parse parameter 1. if (c ==SEP_CHAR) { if (currParamNegative) { currParam1Val =-1 * currParam1Val; } currParserState =PARSER_PARAM2; currParamIndex =0; currParamNegative =false; } // Else if it's the end char, there are no parameters, so we're ready to // process. else if (c ==END_CHAR) { if (currParamNegative) { currParam1Val =-1 * currParam1Val; } currParserState =PARSER_EXECUTE; } // Check for negative at the start. else if ( (currParamIndex ==0) &&(c =='-') ) { currParamNegative =true; currParamIndex++; } // If it's too long, or the character is not a digit, then it's // a parse error, so abandon and go back to PARSER_WAITING. else if ( (currParamIndex>=MAX_PARAM_LENGTH) || (c <'0') || (c> '9') ) { currParserState =PARSER_WAITING; } // It's a valid character, so process it. else { // Shift existing value across and add new digit at the bottom. int currDigitVal =c - '0'; currParam1Val =(currParam1Val * 10) + currDigitVal; currParamIndex++; } } // In the state to parse param 2. else if (currParserState ==PARSER_PARAM2) { // Else if it's a separator, parse parameter 2. if (c ==SEP_CHAR) { if (currParamNegative) { currParam2Val =-1 * currParam2Val; } currParserState =PARSER_PARAM3; currParamIndex =0; currParamNegative =false; } // Else if it's the end char, there are no parameters, so we're ready to // process. else if (c ==END_CHAR) { if (currParamNegative) { currParam2Val =-1 * currParam2Val; } currParserState =PARSER_EXECUTE; } // Check for negative at the start. else if ( (currParamIndex ==0) &&(c =='-') ) { currParamNegative =true; currParamIndex++; } // If it's too long, or the character is not a digit, then it's // a parse error, so abandon and go back to PARSER_WAITING. else if ( (currParamIndex>=MAX_PARAM_LENGTH) || (c <'0') || (c> '9') ) { currParserState =PARSER_WAITING; } // It's a valid character, so process it. else { // Shift existing value across and add new digit at the bottom. int currDigitVal =c - '0'; currParam2Val =(currParam2Val * 10) + currDigitVal; currParamIndex++; } } // In the state to parse param 3. else if (currParserState ==PARSER_PARAM3) { // Else if it's a separator, parse parameter 2. if (c ==SEP_CHAR) { if (currParamNegative) { currParam3Val =-1 * currParam3Val; } currParserState =PARSER_PARAM4; currParamIndex =0; currParamNegative =false; } // Else if it's the end char, there are no parameters, so we're ready to // process. else if (c ==END_CHAR) { if (currParamNegative) { currParam3Val =-1 * currParam3Val; } currParserState =PARSER_EXECUTE; } // Check for negative at the start. else if ( (currParamIndex ==0) &&(c =='-') ) { currParamNegative =true; currParamIndex++; } // If it's too long, or the character is not a digit, then it's // a parse error, so abandon and go back to PARSER_WAITING. else if ( (currParamIndex>=MAX_PARAM_LENGTH) || (c <'0') || (c> '9') ) { currParserState =PARSER_WAITING; } // It's a valid character, so process it. else { // Shift existing value across and add new digit at the bottom. int currDigitVal =c - '0'; currParam3Val =(currParam3Val * 10) + currDigitVal; currParamIndex++; } } // In the state to parse param 4. else if (currParserState ==PARSER_PARAM4) { // Else if it's a separator, parse parameter 2. if (c ==SEP_CHAR) { if (currParamNegative) { currParam4Val =-1 * currParam4Val; } currParserState =PARSER_PARAM5; currParamIndex =0; currParamNegative =false; } // Else if it's the end char, there are no parameters, so we're ready to // process. else if (c ==END_CHAR) { if (currParamNegative) { currParam4Val =-1 * currParam4Val; } currParserState =PARSER_EXECUTE; } // Check for negative at the start. else if ( (currParamIndex ==0) &&(c =='-') ) { currParamNegative =true; currParamIndex++; } // If it's too long, or the character is not a digit, then it's // a parse error, so abandon and go back to PARSER_WAITING. else if ( (currParamIndex>=MAX_PARAM_LENGTH) || (c <'0') || (c> '9') ) { currParserState =PARSER_WAITING; } // It's a valid character, so process it. else { // Shift existing value across and add new digit at the bottom. int currDigitVal =c - '0'; currParam4Val =(currParam4Val * 10) + currDigitVal; currParamIndex++; } } // In the state to parse param 5. else if (currParserState ==PARSER_PARAM5) { // If it's the end char, there are no parameters, so we're ready to // process. if (c ==END_CHAR) { if (currParamNegative) { currParam5Val =-1 * currParam5Val; } currParserState =PARSER_EXECUTE; } // Check for negative at the start. else if ( (currParamIndex ==0) &&(c =='-') ) { currParamNegative =true; currParamIndex++; } // If it's too long, or the character is not a digit, then it's // a parse error, so abandon and go back to PARSER_WAITING. else if ( (currParamIndex>=MAX_PARAM_LENGTH) || (c <'0') || (c> '9') ) { currParserState =PARSER_WAITING; } // It's a valid character, so process it. else { // Shift existing value across and add new digit at the bottom. int currDigitVal =c - '0'; currParam5Val =(currParam5Val * 10) + currDigitVal; currParamIndex++; } } //--------------------------------------------------------- // PARSER CODE HANDLER (Still part of Parser, but section that // processes completed commands) // // If the most recently read char completes a command, // then process the command, and clear the state to // go back to looking for a new command. // // The parsed items are stored in:// currCmd, currParam1Val, currParam2Val, currParam3Val, // currParam4Val, currParam5Val //--------------------------------------------------------- if (currParserState ==PARSER_EXECUTE) { // Ready/OK Check: if ((currCmd[0] =='O') &&(currCmd[1] =='K')) { softwareSerial.println(""); } // Set Servo: // time - time to tween to specified angles // leftHip - microsecs from centre. -ve is hip in, +ve is hip out // leftFoot - microsecs from flat. -ve is foot down, +ve is foot up // rightHip - microsecs from centre. -ve is hip in, +ve is hip out // rightFoot - microsecs from flat. -ve is foot down, +ve is foot up else if ((currCmd[0] =='S') &&(currCmd[1] =='V')) { int tweenTime =currParam1Val; if (currParam1Val <0) { tweenTime =0; } SetServos(tweenTime, currParam2Val, currParam3Val, currParam4Val, currParam5Val, "SV"); } // Stop/Reset:, Stops current anim. Also can be used to put robot into reset position. else if ((currCmd[0] =='S') &&(currCmd[1] =='T')) { StopAnim("ST"); } // Stop Immediate: else if ((currCmd[0] =='S') &&(currCmd[1] =='I')) { StopAnimImmediate("SI"); } // Forward:, -1 means continuous, 0 or no param is the same as 1 time. else if ((currCmd[0] =='F') &&(currCmd[1] =='W')) { int numTimes =currParam1Val; if (currParam1Val <0) { numTimes =-1; } PlayAnimNumTimes(walkForwardAnim, walkEndAnim, numTimes, "FW"); } // Backward:, -1 means continuous, 0 or no param is the same as 1 time. else if ((currCmd[0] =='B') &&(currCmd[1] =='W')) { int numTimes =currParam1Val; if (currParam1Val <0) { numTimes =-1; } PlayAnimNumTimes(walkBackwardAnim, walkEndAnim, numTimes, "BW"); } // Turn Left:, -1 means continuous, 0 or no param is the same as 1 time. else if ((currCmd[0] =='L') &&(currCmd[1] =='T')) { int numTimes =currParam1Val; if (currParam1Val <0) { numTimes =-1; } PlayAnimNumTimes(turnLeftAnim, NULL, numTimes, "LT"); } // Turn Right:, -1 means continuous, 0 or no param is the same as 1 time. else if ((currCmd[0] =='R') &&(currCmd[1] =='T')) { int numTimes =currParam1Val; if (currParam1Val <0) { numTimes =-1; } PlayAnimNumTimes(turnRightAnim, NULL, numTimes, "RT"); } // Shake Head:, -1 means continuous, 0 or no param is the same as 1 time. else if ((currCmd[0] =='S') &&(currCmd[1] =='X')) { int numTimes =currParam1Val; if (currParam1Val <0) { numTimes =-1; } PlayAnimNumTimes(shakeHeadAnim, NULL, numTimes, "SX"); } // Bounce:, -1 means continuous, 0 or no param is the same as 1 time. else if ((currCmd[0] =='B') &&(currCmd[1] =='X')) { int numTimes =currParam1Val; if (currParam1Val <0) { numTimes =-1; } PlayAnimNumTimes(bounceAnim, NULL, numTimes, "BX"); } // Wobble:, -1 means continuous, 0 or no param is the same as 1 time. else if ((currCmd[0] =='W') &&(currCmd[1] =='X')) { int numTimes =currParam1Val; if (currParam1Val <0) { numTimes =-1; } PlayAnimNumTimes(wobbleAnim, NULL, numTimes, "WX"); } // Wobble Left:, -1 means continuous, 0 or no param is the same as 1 time. else if ((currCmd[0] =='W') &&(currCmd[1] =='Y')) { int numTimes =currParam1Val; if (currParam1Val <0) { numTimes =-1; } PlayAnimNumTimes(wobbleLeftAnim, NULL, numTimes, "WY"); } // Wobble Right:, -1 means continuous, 0 or no param is the same as 1 time. else if ((currCmd[0] =='W') &&(currCmd[1] =='Z')) { int numTimes =currParam1Val; if (currParam1Val <0) { numTimes =-1; } PlayAnimNumTimes(wobbleRightAnim, NULL, numTimes, "WZ"); } // Tap Feet:, -1 means continuous, 0 or no param is the same as 1 time. else if ((currCmd[0] =='T') &&(currCmd[1] =='X')) { int numTimes =currParam1Val; if (currParam1Val <0) { numTimes =-1; } PlayAnimNumTimes(tapFeetAnim, NULL, numTimes, "TX"); } // Tap Left Foot:, -1 means continuous, 0 or no param is the same as 1 time. else if ((currCmd[0] =='T') &&(currCmd[1] =='Y')) { int numTimes =currParam1Val; if (currParam1Val <0) { numTimes =-1; } PlayAnimNumTimes(tapLeftFootAnim, NULL, numTimes, "TY"); } // Tap Right Foot:, -1 means continuous, 0 or no param is the same as 1 time. else if ((currCmd[0] =='T') &&(currCmd[1] =='Z')) { int numTimes =currParam1Val; if (currParam1Val <0) { numTimes =-1; } PlayAnimNumTimes(tapRightFootAnim, NULL, numTimes, "TZ"); } // Shake Legs:, -1 means continuous, 0 or no param is the same as 1 time. else if ((currCmd[0] =='L') &&(currCmd[1] =='X')) { int numTimes =currParam1Val; if (currParam1Val <0) { numTimes =-1; } PlayAnimNumTimes(shakeLegsAnim, NULL, numTimes, "LX"); } // Shake Left Leg:, -1 means continuous, 0 or no param is the same as 1 time. else if ((currCmd[0] =='L') &&(currCmd[1] =='Y')) { int numTimes =currParam1Val; if (currParam1Val <0) { numTimes =-1; } PlayAnimNumTimes(shakeLeftLegAnim, NULL, numTimes, "LY"); } // Shake Right Leg:, -1 means continuous, 0 or no param is the same as 1 time. else if ((currCmd[0] =='L') &&(currCmd[1] =='Z')) { int numTimes =currParam1Val; if (currParam1Val <0) { numTimes =-1; } PlayAnimNumTimes(shakeRightLegAnim, NULL, numTimes, "LZ"); } //-------------------------------------------------- // Clear the state and wait for the next command! // This must be done! //-------------------------------------------------- currParserState =PARSER_WAITING; } }}//===============================================================================// Related to playing servo animations.//===============================================================================// Call this to play the given animation once. Pass in NULL if there is no finishAnim.void PlayAnim(int animToPlay[][5], int finishAnim[][5], const char *completeStr){ // Put this in the queue. PlayAnimNumTimes(animToPlay, finishAnim, 1, completeStr);}// Call this to loop the given animation. Pass in NULL if there is no finishAnim.void LoopAnim(int animToPlay[][5], int finishAnim[][5], const char *completeStr){ // Put this in the queue. PlayAnimNumTimes(animToPlay, finishAnim, -1, completeStr);}// Call this to play the given animation the specified number of times. // -1 number of times will make it loop forever.// Pass in NULL if there is no finishAnim.void PlayAnimNumTimes(int animToPlay[][5], int finishAnim[][5], int numTimes, const char *completeStr){ // Put this in the queue. nextAnim =animToPlay; nextFinishAnim =finishAnim; nextAnimNumLoops =numTimes; // Save the completeStr if (completeStr ==NULL) { nextAnimCompleteStr[0] ='-'; nextAnimCompleteStr[1] ='-'; } else { nextAnimCompleteStr[0] =completeStr[0]; nextAnimCompleteStr[1] =completeStr[1]; }}// Stop after the current animation.void StopAnim(const char *completeStr){ // Put this in the queue. PlayAnimNumTimes(standStraightAnim, NULL, 1, completeStr);}// Stop immediately and lerp robot to zero position, interrupting // any animation that is in progress.void StopAnimImmediate(const char *completeStr){ // Put this in the queue. interruptInProgressAnim =true; PlayAnimNumTimes(standStraightAnim, NULL, 1, completeStr);}// Moves servos to the specified positions. Time 0 will make it immediate. Otherwise,// it'll tween it over a specified time.// For positions, 0 means centered.// For hips, -ve is hip left, +ve is hip right// For feet, -ve is foot down, +ve is foot upvoid SetServos(int tweenTime, int leftHip, int leftFoot, int rightHip, int rightFoot, const char* completeStr){ // Save the completeStr if (completeStr ==NULL) { nextAnimCompleteStr[0] ='-'; nextAnimCompleteStr[1] ='-'; } else { nextAnimCompleteStr[0] =completeStr[0]; nextAnimCompleteStr[1] =completeStr[1]; } // Decide which tween data we use. We don't want to over-write the one that is // in progress. We have and reuse these to keep memory allocation fixed. int (*tweenServoData)[5]; if (currAnim !=setServosAnim1) { tweenServoData =setServosAnim1; } else { tweenServoData =setServosAnim2; } // Set the tween information into the animation data. tweenServoData[1][TWEEN_TIME_VALUE] =tweenTime; tweenServoData[1][LEFT_HIP_VALUE] =LeftHipIn(leftHip); tweenServoData[1][LEFT_FOOT_VALUE] =LeftFootUp(leftFoot); tweenServoData[1][RIGHT_HIP_VALUE] =RightHipIn(rightHip); tweenServoData[1][RIGHT_FOOT_VALUE] =RightFootUp(rightFoot); // Queue this tween to be played next. PlayAnim(tweenServoData, NULL, completeStr);}// Set up variables for animation. This is called in setup(). Should be not called by anywhere else.void setup_Animation(){ // Set the servos to the feet flat, feet even position. currLeftHip =LEFT_HIP_CENTRE; currLeftFoot =LEFT_FOOT_CENTRE; currRightHip =RIGHT_HIP_CENTRE; currRightFoot =RIGHT_FOOT_CENTRE; UpdateServos(); // Set the "start" positions to the current ones. So, when // we pay the next anim, we will tween from the current positions. startLeftHip =currLeftHip; startLeftFoot =currLeftFoot; startRightHip =currRightHip; startRightFoot =currRightFoot; // No animation is playing yet, and nothing in the queue yet. timeAtLastAnimUpdate =millis(); animInProgress =false; interruptInProgressAnim =false; currAnim =NULL; finishAnim =NULL; nextAnim =NULL; nextFinishAnim =NULL;}// Loop function for processing animation. This is called in every loop(). Should be be called by anywhere else.//// NOTE:The way looping animations work is that they basically add themselves back to the queue// when a cycle is done, and if there's nothing already queued up! This way, looping animations// work in a similar way to single-play animations, and fits into the queueing system.void loop_Animation(){ // Get the time at the start of this frame. long currTime =millis(); //-------------------------------------------------------------------------------------- // Decide if we want to perform the animation update. We don't execute this every frame. //-------------------------------------------------------------------------------------- if (timeAtLastAnimUpdate + millisBetweenAnimUpdate> currTime) { // Not yet time to do an anim update, so jump out. return; } else { // We reset the timer, and then proceed below to handle the current anim update. timeAtLastAnimUpdate =currTime; } //-------------------------------------------------------------------------------------- // Decide if we need to setup and start a new animation. We do if there's no anim // playing or we've been asked to interrupt the anim. //-------------------------------------------------------------------------------------- if ( (nextAnim !=NULL) &&(!animInProgress || interruptInProgressAnim) ) { // If this was an interrupt, we also set the "start" servo positions // to the current ones. This way, the animation system will tween from the // current positions. if (interruptInProgressAnim) { // This is the place to notify someone of an animation finishing after getting interrupted // Print the command string we just finished. -1 parameter indicates it was interrupted. softwareSerial.print("<"); softwareSerial.print(animCompleteStr); softwareSerial.println(",-1>"); // Set the "start" positions to the current ones. So, when // we pay the next anim, we will tween from the current positions. startLeftHip =currLeftHip; startLeftFoot =currLeftFoot; startRightHip =currRightHip; startRightFoot =currRightFoot; // We've handled any interrupt request, so clear the flag. interruptInProgressAnim =false; } // Store the animation we are now playing. currAnim =nextAnim; finishAnim =nextFinishAnim; animCompleteStr[0] =nextAnimCompleteStr[0]; animCompleteStr[1] =nextAnimCompleteStr[1]; nextAnim =NULL; // Queue is cleared. nextFinishAnim =NULL; nextAnimCompleteStr[0] ='-'; nextAnimCompleteStr[1] ='-'; // Record the number of times to play the animation. animNumLoops =nextAnimNumLoops; // Treat current time as start of frame for the initial lerp to the first frame. timeAtStartOfFrame =currTime; // Set the frame counters. targetFrame =1; // First frame we are lerping to. Index 0 is metadata, so skip. // An animation is now in progress animInProgress =true; } //-------------------------------------------------------------------------------------- // If we are currently playing an animation, then update the animation state and the // servo positions. //-------------------------------------------------------------------------------------- if (animInProgress) { // Determine if we need to switch to the next frame. int timeInCurrFrame =currTime - timeAtStartOfFrame; if (timeInCurrFrame> currAnim[targetFrame][TWEEN_TIME_VALUE]) { // Set the servo positions to the targetFrame's values. // We only set this if the value is> 0. -ve values means that // the current target keyframe did not alter that servos position. if (currAnim[targetFrame][LEFT_HIP_VALUE]>=0) { currLeftHip =currAnim[targetFrame][LEFT_HIP_VALUE]; } if (currAnim[targetFrame][LEFT_FOOT_VALUE]>=0) { currLeftFoot =currAnim[targetFrame][LEFT_FOOT_VALUE]; } if (currAnim[targetFrame][RIGHT_HIP_VALUE]>=0) { currRightHip =currAnim[targetFrame][RIGHT_HIP_VALUE]; } if (currAnim[targetFrame][RIGHT_FOOT_VALUE]>=0) { currRightFoot =currAnim[targetFrame][RIGHT_FOOT_VALUE]; } UpdateServos(); // These current values are now the start of frame values. startLeftHip =currLeftHip; startLeftFoot =currLeftFoot; startRightHip =currRightHip; startRightFoot =currRightFoot; // Now, we try to move to the next frame. // - If there is a next frame, set that as the new target, and proceed. // - If there's no next frame, but it's looping, we re-add this animation // to the queue. // - If there's no next frame, and this is not looping, we stop animating. // (Remember that targetFrame is 1-based since the first element of the animation // data array is metadata) // Increment targetFrame, and reset time in the current frame. targetFrame++; timeAtStartOfFrame =currTime; // If there is no next frame, we stop this current animation. // If it is looping, then we re-queue the current animation if the queue is empty. if (targetFrame> NumOfFrames(currAnim)) { // Stop the current animation. animInProgress =false; // If we're looping forever, and there's no next anim, re-queue the // animation if the queue is empty. if ((animNumLoops <0) &&(nextAnim ==NULL)) { LoopAnim(currAnim, finishAnim, animCompleteStr); } // If we're looping forever, and there is something in the queue, then // finish the animation and proceed. else if ((animNumLoops <0) &&(nextAnim !=NULL)) { if (finishAnim !=NULL) { // Switch to the finish anim. currAnim =finishAnim; finishAnim =NULL; // Record the number of times to play the animation. animNumLoops =1; // Treat current time as start of frame for the initial lerp to the first frame. timeAtStartOfFrame =currTime; // Set the frame counters. targetFrame =1; // First frame we are lerping to. Index 0 is metadata, so skip. // An animation is now in progress animInProgress =true; } else { // We've stopped, so can notify if needed. // Print the command string we just finished. softwareSerial.print("<"); softwareSerial.print(animCompleteStr); softwareSerial.println(">"); } } // If we're looping a limited number of times, and there's no next anim, // re-queue the animation if the queue is empty. else if ((animNumLoops> 1) &&(nextAnim ==NULL)) { PlayAnimNumTimes(currAnim, finishAnim, animNumLoops-1, animCompleteStr); } // In this case, numAnimLoops is 1, this is the last loop through, so // we're done. We play the finishAnim first if needed. else { // If there is a finish animation, switch to that animation. if (finishAnim !=NULL) { // Switch to the finish anim. currAnim =finishAnim; finishAnim =NULL; // Record the number of times to play the animation. animNumLoops =1; // Treat current time as start of frame for the initial lerp to the first frame. timeAtStartOfFrame =currTime; // Set the frame counters. targetFrame =1; // First frame we are lerping to. Index 0 is metadata, so skip. // An animation is now in progress animInProgress =true; } // Otherwise, we're done! We've played the finishAnim if there was one. else { // Print the command string we just finished. softwareSerial.print("<"); softwareSerial.print(animCompleteStr); softwareSerial.println(">"); } } } } // If we're still animating (i.e. the previous check didn't find that // we've finished the current animation), then proceed. if (animInProgress) { // Set the servos per data in the current frame. We only update the servos that have target // microsecond values> 0. This is to support the feature where we leave a servo at its // existing position if an animation data item is -1. float frameTimeFraction =(currTime - timeAtStartOfFrame) / ((float) currAnim[targetFrame][TWEEN_TIME_VALUE]); if (currAnim[targetFrame][LEFT_HIP_VALUE]>=0) { currLeftHip =startLeftHip + ((currAnim[targetFrame][LEFT_HIP_VALUE] - startLeftHip) * frameTimeFraction); } if (currAnim[targetFrame][LEFT_FOOT_VALUE]>=0) { currLeftFoot =startLeftFoot + ((currAnim[targetFrame][LEFT_FOOT_VALUE] - startLeftFoot) * frameTimeFraction); } if (currAnim[targetFrame][RIGHT_HIP_VALUE]>=0) { currRightHip =startRightHip + ((currAnim[targetFrame][RIGHT_HIP_VALUE] - startRightHip) * frameTimeFraction); } if (currAnim[targetFrame][RIGHT_FOOT_VALUE]>=0) { currRightFoot =startRightFoot + ((currAnim[targetFrame][RIGHT_FOOT_VALUE] - startRightFoot) * frameTimeFraction); } UpdateServos(); } }}// Move all the servo to the positions set in the curr... variables.// In the code, we update those variables and then call this to set the servos.void UpdateServos(){ servoLeftHip.writeMicroseconds(currLeftHip); servoLeftFoot.writeMicroseconds(currLeftFoot); servoRightHip.writeMicroseconds(currRightHip); servoRightFoot.writeMicroseconds(currRightFoot);}// Return the number of frames in the given animation data.// Have this helper function to avoid the "magic number" reference of animData[0][0].int NumOfFrames(int animData[][5]){ return animData[0][0];} Schémas
