MobBob:DIY Arduino Robot Controlado por Smartphone Android

Ferramentas e máquinas necessárias

	Ferro de soldar (genérico)
	Impressora 3D (genérica)

Aplicativos e serviços online

Arduino IDE

Sobre este projeto

Código

• código

código Arduino

 / * * =================================================================* Programa de controle MobBob - Versão serial do software Bluetooth * por Kevin Chan (também conhecido como Cevinius) * =====================================================================* * Este programa permite que o MobBob seja controlado por meio de comandos seriais. Nesta versão do código, os comandos * são recebidos por uma porta serial de software, com pinos definidos em #define próximo ao topo. * Isso significa que você pode usar qualquer placa compatível com Arduino e conectar uma placa bluetooth aos pinos definidos para * software serial. (Ao contrário da outra versão projetada para a placa Bluno da DFRobot.) * * Este programa é longo e contém 2 componentes principais - um programa de animação servo suave e um programa analisador de comandos serial *. * * Sistema de animação * ================* O programa de animação é projetado para animar matrizes de quadros-chave servo suavemente. O código tenta fazer o seu melhor * para ser fácil de usar. * * O sistema de animação irá enfileirar apenas 1 comando. ou seja, um comando pode estar em execução * e um comando pode ser enfileirado. Se você enviar mais comandos, eles sobrescreverão o comando enfileirado. * * O sistema de animação irá, por padrão, esperar para terminar a animação atual antes de iniciar a próxima. Isso * significa que se os dados da animação terminarem com o robô em sua pose base, as coisas se juntarão suavemente. Para * apoiar isso, o sistema de animação também possui um recurso em que uma animação pode ter uma "sequência de acabamento" * para colocar o robô de volta na pose base. Este recurso é usado para as animações de avanço / retrocesso. * Essas animações têm uma sequência final que coloca o robô de volta na pose básica. * * Quando uma animação terminar de ser reproduzida, o sistema de animação emitirá uma string de resposta para a porta serial. * Isso permite que os chamadores saibam quando as animações solicitadas terminaram de ser reproduzidas. Isso é útil * para os usuários sequenciarem animações - esperando que uma termine antes de iniciar outra. * * O código de animação tem muitas variáveis ​​para permitir que as coisas sejam ajustadas. Por exemplo. Freqüência de atualização, pinos de Arduino, etc. * * O formato de array de dados de animação também é projetado para ser fácil de editar manualmente. * * Analisador de comandos * ==============* Este sistema analisa os comandos recebidos via serial e os processa. Os comandos incluem um para definir diretamente * as posições do servo, bem como comandos para acionar animações e caminhadas predefinidas. * * Assim, o usuário que não quiser se preocupar com os detalhes da caminhada pode apenas utilizar as caminhadas / animações pré-definidas. * E os usuários que desejam controle total sobre os servos (para criar novas animações em tempo real) podem fazer isso também. * * Conforme mencionado acima, esses comandos podem ser usados ​​interativamente a partir do Arduino Serial Monitor. Eles também podem ser * enviados usando Bluetooth LE (quando um Bluno é usado). O aplicativo de telefone enviará os comandos via Bluetooth LE para o * Bluno. * * Comandos gerais:* ----------------- * Ready / OK Check: * Status check. A resposta é retornada imediatamente para verificar se o controlador está funcionando. * * Definir servo: * tempo - tempo para interpolar os ângulos especificados, 0 irá imediatamente pular para os ângulos * quadril esquerdo - microssegundos do centro. -ve é o quadril para dentro, + ve é o quadril para fora * Pé esquerdo - microssegundos do plano. -ve é o pé para baixo, + ve é o pé para cima * quadril direito - microssegundos do centro. -ve é o quadril para dentro, + ve é o quadril para fora * RightFoot - microsegs do plano. -ve é o pé para baixo, + ve é o pé para cima * Este comando é usado para obter controle total sobre os servos. Você pode interpolar o robô de sua * pose atual para a pose especificada ao longo da duração especificada. * * Parar / Redefinir: * Para o robô após a animação atual. Pode ser usado para interromper animações definidas para loop * indefinidamente. Isso também pode ser usado para colocar o robô em sua postura básica (em pé) * * Parar Imediato: * Para o robô imediatamente, sem esperar para completar a animação atual. Isso * interrompe a animação atual do robô. Potencialmente, o robô pode estar no meio da animação * e em uma pose instável, portanto, tome cuidado ao usá-lo. * * Comandos de caminhada padrão:* ----------------------- * Avançar:, -1 significa contínuo, 0 ou nenhum parâmetro é o mesmo que 1 vez. * Retroceder:, -1 significa contínuo, 0 ou nenhum parâmetro é o mesmo que 1 vez. * Vire à esquerda:, -1 significa contínuo, 0 ou nenhum parâmetro é o mesmo que 1 vez. * Vire à direita:, -1 significa contínuo, 0 ou nenhum parâmetro é o mesmo que 1 vez. * * Comandos de animação divertidos:* ----------------------- * Shake Head:, -1 significa contínuo, 0 ou nenhum parâmetro é o mesmo que 1 vez. * * Bounce:, -1 significa contínuo, 0 ou nenhum parâmetro é o mesmo que 1 vez. * * Wobble:, -1 significa contínuo, 0 ou nenhum parâmetro é o mesmo que 1 vez. * Wobble Left:, -1 significa contínuo, 0 ou nenhum parâmetro é o mesmo que 1 vez. * Wobble Right:, -1 significa contínuo, 0 ou nenhum parâmetro é o mesmo que 1 vez. * * Tap Feet:, -1 significa contínuo, 0 ou nenhum parâmetro é o mesmo que 1 vez. * Tap Left Foot:, -1 significa contínuo, 0 ou nenhum parâmetro é o mesmo que 1 vez. * Toque com o pé direito:, -1 significa contínuo, 0 ou nenhum parâmetro é o mesmo que 1 vez. * * Shake Legs:, -1 significa contínuo, 0 ou nenhum parâmetro é o mesmo que 1 vez. * Agitar a perna esquerda:, -1 significa contínuo, 0 ou nenhum parâmetro é o mesmo que 1 vez. * Agitar a perna direita:, -1 significa contínuo, 0 ou nenhum parâmetro é o mesmo que 1 vez. * * Além disso, o código enviará uma string de resposta de volta por meio de Serial quando os comandos * tiverem concluído a * execução. * * Se o comando terminou normalmente, a string de resposta é o código do comando sem * parâmetros. Por exemplo. Quando terminar de avançar, enviará a resposta "". * * Se um comando foi interrompido com , a animação atual pode ter sido interrompida no meio do caminho. * Nesse caso, o robô pode estar em uma pose intermediária esquisita e o finishAnims pode não ter sido * jogado. Para permitir ao usuário saber que isso aconteceu, a string de resposta terá o * parâmetro -1. Por exemplo, se uma caminhada foi interrompida no meio do caminho usando , a string de resposta seria *  para indicar que a caminhada foi interrompida, mas foi interrompida no meio do caminho. * (Observação:se você usar  para parar, isso irá esperar que o ciclo de animação atual seja concluído * antes de parar. Assim, as animações não serão interrompidas no meio do caminho nesse caso.) * * Porque as respostas são enviadas após um a animação estiver completa, o remetente do comando pode * procurar as strings de resposta para determinar quando o robô está pronto para um novo comando. * Por exemplo. Se você usar o comando , a string de resposta não será enviada até que todas as 3 etapas * (e terminar anim) sejam concluídas. Portanto, o remetente do comando pode esperar pela string de resposta * antes de dizer ao robô para fazer a próxima coisa. * / #include  #include  // -------------------------------- -------------------------------------------------- // Velocidade de comunicação serial - Defina esta para sua placa serial (bluetooth ).//------------------------------- -------------------------------------------------- - // Velocidade de comunicação serial com a placa bluetooth.// O padrão de algumas placas é 9600. A placa que tenho tem um valor padrão de 115200. # define SERIAL_SPEED 115200 // Configure uma porta serial de software nestes pinos.const int rxPin =11; // pino usado para receber dados int txPin =12; // pino usado para enviar dataSoftwareSerial softwareSerial (rxPin, txPin); // ---------------------------------- ------------------------------------------------ // Configure os pinos do Arduino - defina-os para o seu robô em particular.//------------------------------------- --------------------------------------------- const int SERVO_LEFT_HIP =5; const int SERVO_LEFT_FOOT =2; const int SERVO_RIGHT_HIP =3; const int SERVO_RIGHT_FOOT =4; // Quero que este código seja utilizável em todos os bípedes de 4 servo! (Como Bob, MobBob) // Notei que algumas construções montam os servos do quadril voltados para uma // maneira diferente de como eu fazia os MobBobs, então, esta configuração permite que você configure o código // para qualquer estilo de construção.// 1 para o estilo MobBob quadris voltados para a frente (junta para frente) // -1 para quadris estilo Bob voltados para trás (junta para trás) #define FRONT_JOINT_HIPS 1 // ------------------- -------------------------------------------------- ------------- // Constantes Servo Max / Min / Center - Defina-as para o seu robô em particular.//------------------ -------------------------------------------------- -------------- 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 int LEFT_FOOT_MIN =LEFT_FOOT_CENTRE - 500; 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 =1 465; const int RIGHT_FOOT_MIN =RIGHT_FOOT_CENTRE - 500; const int RIGHT_FOOT_MAX =RIGHT_FOOT_CENTRE + 500; // ------------------------------ ------------------------------------------------ // Funções auxiliares para ajudar a calcular os valores das juntas de uma maneira mais amigável.// Você pode ajustar os sinais aqui se os servos forem configurados de uma maneira diferente.// Atualizar aqui significa que os dados da animação não precisam ser modificados se o // servos são configurados de forma diferente.// (Ex. Os servos de quadril de Bob originais estão voltados para trás em relação aos de MobBob.) //// (Além disso, acho difícil lembrar os sinais a serem usados ​​para cada servo, pois // são diferentes para quadris esquerdo / direito e para pés esquerdo / direito.) // ------------------------------------------------ ------------------------------ int LeftHipCentre () {return LEFT_HIP_CENTRE; } int LeftHipIn (int milissegundos) {return LEFT_HIP_CENTRE + (FRONT_JOINT_HIPS * milissegundos); } int LeftHipOut (int milissegundos) {return LEFT_HIP_CENTRE - (FRONT_JOINT_HIPS * milissegundos); } int RightHipCentre () {return RIGHT_HIP_CENTRE; } int RightHipIn (int milissegundos) {return RIGHT_HIP_CENTRE - (FRONT_JOINT_HIPS * milissegundos); } int RightHipOut (int milissegundos) {return RIGHT_HIP_CENTRE + (FRONT_JOINT_HIPS * milissegundos); } int LeftFootFlat () {return LEFT_FOOT_CENTRE; } int LeftFootUp (int milissegundos) {return LEFT_FOOT_CENTRE - milissegundos; } int LeftFootDown (int milissegundos) {return LEFT_FOOT_CENTRE + milissegundos; } int RightFootFlat () {return RIGHT_FOOT_CENTRE; } Int RightFootUp (int milissegundos) {return RIGHT_FOOT_CENTRE + milissegundos; } int RightFootDown (int milissegundos) {return RIGHT_FOOT_CENTRE - milissegundos; } // ----------------------------------------------- ----------------------------------- // Dados de animação de quadro-chave para marcha padrão e outras animações servo // // O formato é {, ​​, , , } // milissegundos - tempo de interpolação para as posições deste quadro-chave. Por exemplo. 500 significa que levará 500ms para ir da // posição do robô no início deste quadro para a posição especificada neste quadro // leftHipMicros - posição do quadril esquerdo em servo microsecs.// leftFootMicros - posição do quadril esquerdo em servo microsecs.// rightHipMicros - posição do quadril esquerdo em microssegundos servos.// rightFootMicros - posição do quadril esquerdo em microssegundos servos.// // Os valores micro do servo suportam um valor especial de -1. Se este valor for fornecido, ele informa // ao código de animação para ignorar este servo neste quadro-chave. ou seja, esse servo // permanecerá na posição que tinha no início deste quadro-chave.//// Além disso, o primeiro elemento no array de dados de animação é especial. É um elemento de metadados.// O primeiro elemento é {, 0, 0, 0, 0}, que nos diz o número de frames // na animação. Portanto, o primeiro quadro-chave real está em animData [1], e o último quadro-chave // ​​está em animData []. (Onde  é o valor em animData [0] [0].) // ----------------------------- -------------------------------------------------- --- // Constantes para tornar o acesso aos arrays de quadro-chave mais legível para humanos.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 na animação de andar a pé data.const int FOOT_DELTA =150; const int HIP_DELTA =FRONT_JOINT_HIPS * 120; // Vai para a posição ereta padrão. Usado por stopAnim (). Int standStraightAnim [] [5] ={// Metadados. O primeiro elemento é o número de quadros. {1, 0, 0, 0, 0}, // Pés planos, Pés pares {300, LeftHipCentre (), LeftFootFlat (), RightHipCentre (), RightFootFlat ()}}; // Antes disso, faça o robô Pés planos, pés uniformes (ou seja, standStraightAnim) .int walkForwardAnim [] [5] ={// Metadados. O primeiro elemento é o número de quadros. {8, 0, 0, 0, 0}, // Inclinar para a esquerda, Pés uniformes {300, LeftHipCentre (), LeftFootUp (FOOT_DELTA), RightHipCentre (), RightFootDown (FOOT_DELTA)}, // Inclinar para a esquerda, pé direito forward {300, LeftHipIn (HIP_DELTA), LeftFootUp (FOOT_DELTA), RightHipOut (HIP_DELTA), RightFootDown (FOOT_DELTA)}, // Pés retos, pé direito à frente {300, LeftHipIn (HIP_DELTA), LeftFoot_DELTA (), RightFlatut (), RightFlatut (), RightFlatut RightFootFlat ()}, // Inclinar para a direita, Pé direito para frente {300, LeftHipIn (HIP_DELTA), LeftFootDown (FOOT_DELTA), RightHipOut (HIP_DELTA), RightFootUp (FOOT_DELTA)}, // Inclinar para a direita, FeetCentre {300, LeftHip (), LeftFootDown (FOOT_DELTA), RightHipCentre (), RightFootUp (FOOT_DELTA)}, // Inclinar para a direita, Pé esquerdo para frente {300, LeftHipOut (HIP_DELTA), LeftFootDown (FOOT_DELTA), RightHipIn (HIP_DELTA), RightHipIn (HIP_DELTA), RightHipIn (HIP_DELTA), RightHipIn (HIP_DELTA), RightHipIn (HIP_DELTA) , // Pés planos, Pé esquerdo para frente {300, LeftHipOut (HIP_DELTA), LeftFootFlat (), RightHipIn (HIP_DELTA), RightFootFlat ()}, // Inclinar para a esquerda, Pé esquerdo para frente {300, LeftHipOut (HIP_DEL TA), LeftFootUp (FOOT_DELTA), RightHipIn (HIP_DELTA), RightFootDown (FOOT_DELTA)}}; // Antes disso, coloque o robô em pés planos, pés uniformes (ou seja, standStraightAnim) .int walkBackwardAnim [] [5] ={// Metadados. O primeiro elemento é o número de quadros. {8, 0, 0, 0, 0}, // Inclinar para a esquerda, Pés uniformes {300, LeftHipCentre (), LeftFootUp (FOOT_DELTA), RightHipCentre (), RightFootDown (FOOT_DELTA)}, // Inclinar para a esquerda, Pé esquerdo forward {300, LeftHipOut (HIP_DELTA), LeftFootUp (FOOT_DELTA), RightHipIn (HIP_DELTA), RightFootDown (FOOT_DELTA)}, // Pés planos, Pé esquerdo à frente {300, LeftHipOut (HIP_DELTA), LeftFootFlat (), RightFootFlat () RightFootFlat ()}, // Inclinar para a direita, Pé esquerdo para frente {300, LeftHipOut (HIP_DELTA), LeftFootDown (FOOT_DELTA), RightHipIn (HIP_DELTA), RightFootUp (FOOT_DELTA)}, // Inclinar para a direita, FeetCentre {300, LeftHip (), LeftFootDown (FOOT_DELTA), RightHipCentre (), RightFootUp (FOOT_DELTA)}, // Inclinar para a direita, Pé direito para a frente {300, LeftHipIn (HIP_DELTA), LeftFootDown (FOOT_DELTA), RightHipOut (HIP_DELTA), RightHipOut (HIP_DELTA), RightHipOut (HIP_DELTA), , // Pés planos, pé direito para frente {300, LeftHipIn (HIP_DELTA), LeftFootFlat (), RightHipOut (HIP_DELTA), RightFootFlat ()}, // Inclinar para a esquerda, pé direito para frente {300, LeftHipIn (HIP_DELTA) ), LeftFootUp (FOOT_DELTA), RightHipOut (HIP_DELTA), RightFootDown (FOOT_DELTA)}}; // Concluir caminhada anim leva o robô do final de walkForwardAnim / walkBackwardAnim de volta para standStraightAnim.int walkEndAnim [] [5] ={// Metadata . O primeiro elemento é o número de quadros. {2, 0, 0, 0, 0}, // Inclinar para a esquerda, Pés nivelados {300, LeftHipCentre (), LeftFootUp (FOOT_DELTA), RightHipCentre (), RightFootDown (FOOT_DELTA)}, // Pés planos, Pés nivelados { 300, LeftHipCentre (), LeftFootFlat (), RightHipCentre (), RightFootFlat ()}}; // Antes disso, coloque o robô em Pés planos, pés uniformes (ou seja, standStraightAnim) .int turnLeftAnim [] [5] ={/ / Metadados. O primeiro elemento é o número de quadros. {6, 0, 0, 0, 0}, // Inclinar para a esquerda, Pés uniformes {300, LeftHipCentre (), LeftFootUp (FOOT_DELTA), RightHipCentre (), RightFootDown (FOOT_DELTA)}, // Inclinar para a esquerda, virar à esquerda quadril, Vire o quadril direito {300, LeftHipIn (HIP_DELTA), LeftFootUp (FOOT_DELTA), RightHipIn (HIP_DELTA), RightFootDown (FOOT_DELTA)}, // Pés retos, Vire o quadril esquerdo, Vire o quadril direito {300, LeftHipIn (HIP_DELTA), (), RightHipIn (HIP_DELTA), RightFootFlat ()}, // Inclinar para a direita, Vire o quadril esquerdo, Vire o quadril direito {300, LeftHipIn (HIP_DELTA), LeftFootDown (FOOT_DELTA), RightHipIn (HIP_DELTA), RightFootUp (FOOT_DELTA), // Incline para a direita, Pés uniformes {300, LeftHipCentre (), LeftFootDown (FOOT_DELTA), RightHipCentre (), RightFootUp (FOOT_DELTA)}, // Pés planos, Pés uniformes {300, LeftHipCentre (), LeftFootFlat (), RightHipCentre ( ), RightFootFlat ()}}; // Antes disso, coloque o robô em Feet Flat, Feet Even (ie standStraightAnim) .int turnRightAnim [] [5] ={// Metadata. O primeiro elemento é o número de quadros. {6, 0, 0, 0, 0}, // Inclinar para a direita, Pés uniformes {300, LeftHipCentre (), LeftFootDown (FOOT_DELTA), RightHipCentre (), RightFootUp (FOOT_DELTA)}, // Inclinar para a direita, virar à esquerda quadril, Vire o quadril direito {300, LeftHipIn (HIP_DELTA), LeftFootDown (FOOT_DELTA), RightHipIn (HIP_DELTA), RightFootUp (FOOT_DELTA)}, // Pés retos, Vire o quadril esquerdo, Vire o quadril direito {300, LeftHipIn (HIP_DELTA), (), RightHipIn (HIP_DELTA), RightFootFlat ()}, // Inclinar para a esquerda, Vire o quadril esquerdo, Vire o quadril direito {300, LeftHipIn (HIP_DELTA), LeftFootUp (FOOT_DELTA), RightHipIn (HIP_DELTA), RightFootDown (FOOT_DELTA), // Incline para a esquerda, Pés uniformes {300, LeftHipCentre (), LeftFootUp (FOOT_DELTA), RightHipCentre (), RightFootDown (FOOT_DELTA)}, // Pés planos, Pés uniformes {300, LeftHipCentre (), LeftFootFlat (), RightHipCentre ( ), RightFootFlat ()}}; // Balançar a cabeça anim. Esquerda direita rapidamente para emular o shake head.int shakeHeadAnim [] [5] ={// Metadados. O primeiro elemento é o número de quadros. {4, 0, 0, 0, 0}, // Pés retos, Twist left {150, LeftHipOut (HIP_DELTA), LeftFootFlat (), RightHipIn (HIP_DELTA), RightFootFlat ()}, // Pés retos, Pés pares {150 , LeftHipCentre (), LeftFootFlat (), RightHipCentre (), RightFootFlat ()}, // Pés planos, Twist right {150, LeftHipIn (HIP_DELTA), LeftFootFlat (), RightHipOut (HIP_DELTA), RightFootFlat ()}, // Pés flat, Feet even {150, LeftHipCentre (), LeftFootFlat (), RightHipCentre (), RightFootFlat ()}}; // Wobble anim. Incline para a esquerda e para a direita para fazer um divertido wobble.int wobbleAnim [] [5] ={// Metadados. O primeiro elemento é o número de quadros. {4, 0, 0, 0, 0}, // Inclinar para a esquerda, Pés uniformes {300, LeftHipCentre (), LeftFootUp (FOOT_DELTA), RightHipCentre (), RightFootDown (FOOT_DELTA)}, // Pés planos, Pés até {300 , LeftHipCentre (), LeftFootFlat (), RightHipCentre (), RightFootFlat ()}, // Inclinar para a direita, Pés uniformes {300, LeftHipCentre (), LeftFootDown (FOOT_DELTA), RightHipCentre (), RightFootUp (FOOT_DELTA)}, // Feet flat, Feet Even {300, LeftHipCentre (), LeftFootFlat (), RightHipCentre (), RightFootFlat ()}}; // Wobble left anim. Incline para a esquerda e para trás.int wobbleLeftAnim [] [5] ={// Metadados. O primeiro elemento é o número de quadros. {2, 0, 0, 0, 0}, // Inclinar para a esquerda, Pés nivelados {300, LeftHipCentre (), LeftFootUp (FOOT_DELTA), RightHipCentre (), RightFootDown (FOOT_DELTA)}, // Pés planos, Pés nivelados {300 , LeftHipCentre (), LeftFootFlat (), RightHipCentre (), RightFootFlat ()},}; // Wobble right anim. Incline para a direita e para trás.int wobbleRightAnim [] [5] ={// Metadados. O primeiro elemento é o número de quadros. {2, 0, 0, 0, 0}, // Inclinar para a direita, Pés nivelados {300, LeftHipCentre (), LeftFootDown (FOOT_DELTA), RightHipCentre (), RightFootUp (FOOT_DELTA)}, // Pés planos, Pés nivelados {300 , LeftHipCentre (), LeftFootFlat (), RightHipCentre (), RightFootFlat ()}}; // Toque os pés anim. Toque em ambos os pés.int tapFeetAnim [] [5] ={// Metadados. O primeiro elemento é o número de quadros. {2, 0, 0, 0, 0}, // Levante ambos os pés, Pés uniformes {500, LeftHipCentre (), LeftFootUp (FOOT_DELTA), RightHipCentre (), RightFootUp (FOOT_DELTA)}, // Pés planos, Pés nivelados { 500, LeftHipCentre (), LeftFootFlat (), RightHipCentre (), RightFootFlat ()},}; // Toque com o pé esquerdo anim.int tapLeftFootAnim [] [5] ={// Metadados. O primeiro elemento é o número de quadros. {2, 0, 0, 0, 0}, // Levante o pé esquerdo, Pés nivelados {500, LeftHipCentre (), LeftFootUp (FOOT_DELTA), RightHipCentre (), RightFootFlat ()}, // Pés nivelados, Pés nivelados {500 , LeftHipCentre (), LeftFootFlat (), RightHipCentre (), RightFootFlat ()},}; // Toque com o pé direito anim.int tapRightFootAnim [] [5] ={// Metadados. O primeiro elemento é o número de quadros. {2, 0, 0, 0, 0}, // Levantar o pé direito, Pés nivelados {500, LeftHipCentre (), LeftFootFlat (), RightHipCentre (), RightFootUp (FOOT_DELTA)}, // Pés nivelados, Pés nivelados {500 , LeftHipCentre (), LeftFootFlat (), RightHipCentre (), RightFootFlat ()},}; // Salta para cima e para baixo anim.int bounceAnim [] [5] ={// Metadados. O primeiro elemento é o número de quadros. {2, 0, 0, 0, 0}, // Levante ambos os pés, Pés uniformes {500, LeftHipCentre (), LeftFootDown (300), RightHipCentre (), RightFootDown (300)}, // Pés planos, Pés uniformes { 500, LeftHipCentre (), LeftFootFlat (), RightHipCentre (), RightFootFlat ()},}; // Shake Legs Animation.int shakeLegsAnim [] [5] ={// Metadados. O primeiro elemento é o número de quadros. {14, 0, 0, 0, 0}, // Inclinar para a esquerda, Pés uniformes {300, LeftHipCentre (), LeftFootUp (FOOT_DELTA), RightHipCentre (), RightFootDown (FOOT_DELTA)}, // Inclinar para a esquerda, quadril direito em { 100, LeftHipCentre (), LeftFootUp (FOOT_DELTA), RightHipIn (HIP_DELTA), RightFootDown (FOOT_DELTA)}, // Inclinar para a esquerda, Pés uniformes {100, LeftHipCentre (), LeftFootUp (FOOT_DELTA), RightHipDown (), RightFOOT , // Inclinar para a esquerda, quadril direito para fora {100, LeftHipCentre (), LeftFootUp (FOOT_DELTA), RightHipOut (HIP_DELTA), RightFootDown (FOOT_DELTA)}, // Inclinar para a esquerda, pés uniformes {100, LeftHipCentre (), LeftFootUp (FOOT_DELTA) , RightHipCentre (), RightFootDown (FOOT_DELTA)}, // Inclinar para a esquerda, Quadril direito em {100, LeftHipCentre (), LeftFootUp (FOOT_DELTA), RightHipIn (HIP_DELTA), RightFootDown (FOOT_DELTA)}, // Inclinar para a esquerda, Pés uniformes { 100, LeftHipCentre (), LeftFootUp (FOOT_DELTA), RightHipCentre (), RightFootDown (FOOT_DELTA)}, // Pés planos, Pés pares {300, LeftHipCentre (), LeftFootFlat (), RightHipCentre (), RightFootFlat ()}, // Equipamento de inclinação ht, Feet Even {300, LeftHipCentre (), LeftFootDown (FOOT_DELTA), RightHipCentre (), RightFootUp (FOOT_DELTA)}, // Inclinar para a direita, Quadril esquerdo em {100, LeftHipIn (HIP_DELTA), LeftFootDown (FOOT_DELTA), RightFootDown (FOOT_DELTA), RightHipIn , RightFootUp (FOOT_DELTA)}, // Inclinar para a direita, Pés uniformes {100, LeftHipCentre (), LeftFootDown (FOOT_DELTA), RightHipCentre (), RightFootUp (FOOT_DELTA)}, // Inclinar para a direita, Quadril esquerdo para fora {100, LeftHipOut (HIP_DELTA ), LeftFootDown (FOOT_DELTA), RightHipCentre (), RightFootUp (FOOT_DELTA)}, // Incline para a direita, Pés uniformes {100, LeftHipCentre (), LeftFootDown (FOOT_DELTA), RightHipCentre (), RightFootUp (FOOT_DELTA)}, // FeetUp (FOOT_DELTA) , Pés pares {300, LeftHipCentre (), LeftFootFlat (), RightHipCentre (), RightFootFlat ()}}; // Shake Left Leg Animation.int shakeLeftLegAnim [] [5] ={// Metadados. O primeiro elemento é o número de quadros. {12, 0, 0, 0, 0}, // Inclinar para a direita, Pés uniformes {300, LeftHipCentre (), LeftFootDown (FOOT_DELTA), RightHipCentre (), RightFootUp (FOOT_DELTA)}, // Inclinar para a direita, Quadril esquerdo em { 100, LeftHipIn (HIP_DELTA), LeftFootDown (FOOT_DELTA), RightHipCentre (), RightFootUp (FOOT_DELTA)}, // Inclinar para a direita, Pés uniformes {100, LeftHipCentre (), LeftFootDown (FOOT_DELTA), RightHipUpDireito (), RightFOOT , // Inclinar para a direita, Quadril esquerdo para fora {100, LeftHipOut (HIP_DELTA), LeftFootDown (FOOT_DELTA), RightHipCentre (), RightFootUp (FOOT_DELTA)}, // Inclinar para a direita, Pés uniformes {100, LeftHipCentre (), LeftFootDown (FOOT_DELTA) , RightHipCentre (), RightFootUp (FOOT_DELTA)}, // Inclinar para a direita, Quadril esquerdo em {100, LeftHipIn (HIP_DELTA), LeftFootDown (FOOT_DELTA), RightHipCentre (), RightFootUp (FOOT_DELTA)}, // Inclinar para a direita, Pés uniformes { 100, LeftHipCentre (), LeftFootDown (FOOT_DELTA), RightHipCentre (), RightFootUp (FOOT_DELTA)}, // Inclinar para a direita, Quadril esquerdo para fora {100, LeftHipOut (HIP_DELTA), LeftFootDown (FOOT_DELTA), RightHipCentre (), RightHipCentre (), RightFo otUp (FOOT_DELTA)}, // Inclinar para a direita, Pés uniformes {100, LeftHipCentre (), LeftFootDown (FOOT_DELTA), RightHipCentre (), RightFootUp (FOOT_DELTA)}, // Inclinar para a direita, Quadril esquerdo em {100, LeftHipIn (HIP_DELTA) , LeftFootDown (FOOT_DELTA), RightHipCentre (), RightFootUp (FOOT_DELTA)}, // Inclinar para a direita, Pés uniformes {100, LeftHipCentre (), LeftFootDown (FOOT_DELTA), RightHipCentre (), RightFootUp (FOOT_DELTA)}, // Pés Pés uniformes {300, LeftHipCentre (), LeftFootFlat (), RightHipCentre (), RightFootFlat ()}}; // Shake Right Leg Animation.int shakeRightLegAnim [] [5] ={// Metadados. O primeiro elemento é o número de quadros. { 12, 0, 0, 0, 0 }, // Tilt left, Feet even { 300, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootDown(FOOT_DELTA) }, // Tilt left, Right hip in { 100, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipIn(HIP_DELTA), RightFootDown(FOOT_DELTA) }, // Tilt left, Feet even { 100, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootDown(FOOT_DELTA) }, // Tilt left, Right hip out { 100, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipOut(HIP_DELTA), RightFootDown(FOOT_DELTA) }, // Tilt left, Feet even { 100, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootDown(FOOT_DELTA) }, // Tilt left, Right hip in { 100, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipIn(HIP_DELTA), RightFootDown(FOOT_DELTA) }, // Tilt left, Feet even { 100, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootDown(FOOT_DELTA) }, // Tilt left, Right hip out { 100, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipOut(HIP_DELTA), RightFootDown (FOOT_DELTA) }, // Tilt left, Feet even { 100, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootDown(FOOT_DELTA) }, // Tilt left, Right hip in { 100, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipIn(HIP_DELTA), RightFootDown(FOOT_DELTA) }, // Tilt left, Feet even { 100, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootDown(FOOT_DELTA) }, // Feet flat, Feet even { 300, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() }, };//----------------------------------------------------------------------------------// Special dynamic animation data for setting/tweening servo positions.//----------------------------------------------------------------------------------// These are 2 special anim data that we use for the SetServos() function. They have// a single frame. Those will change the data in these anim data and play them to // move the servos.int setServosAnim1[][5] ={ // Metadata. First element is number of frames. { 1, 0, 0, 0, 0 }, // Tilt left, Feet even { 0, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() }};int setServosAnim2[][5] ={ // Metadata. First element is number of frames. { 1, 0, 0, 0, 0 }, // Tilt left, Feet even { 0, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() }};//----------------------------------------------------------------------------------// Servo Variables//----------------------------------------------------------------------------------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. Retorna; } 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];}

Esquemas