Arduino Robot Controlled By Android Smartphone

Made by mircemk / 3D Printing / Robotics / Vehicles / Voice
About the project

A great idea for symbiosis between Arduino and Android
Project info

Difficulty: Easy
Platforms: Android, Arduino
Estimated time: 2 hours
License: GNU General Public License, version 3 or later (GPL3+)
Items used in this project

Hardware components

	HC05 CORE + 8K RAM + EPR Bluetooth module	x 1
	Servo Mount - Servo Plate B	x 4
	Arduino Nano 3.0 (with ATMEGA328)	x 1
Software apps and online services

arduino IDE
Hand tools and fabrication machines

	Soldering Iron	x 1
	3D printer	x 1
Story

By harnessing the power of your smart phone, MobBob is a walking, talking robot with voice recognition and computer vision.
Details

This robot is a great example of the symbiosis between the Arduino and the Android platform. The hardware part controls the Arduino, and the software processing is performed by a smartphone. I made this robot according to the instructions on the cevinius.com website but with standard Arduino Nano and HC05 bluetooth board. The robot can perform a number of functions including moving forward, backward, left, right and a few playing modes.
The most interesting is the interactive part where you can give him voice commands or activate face recognition system with which the robot follow your movements.This is possible with the help of speech recognition and face tracking tools on Android smartphone which is a very powerful toy.
You can download .stl 3-D printing parts at Thingiverse (https://www.thingiverse.com/thing:715688) and Android.apk file(https://apkpure.com/mobbob/com.cevinius.MobBob).
In the code you will want to: - Update the pin variables to match your build - Tweak the servo center, min and max values - Set "FRONT_JOINT_HIPS" to 1 or -1 depending on which way your hip servos are mounted. I mount it with the servo axle at the front of MobBob. For this configuration, set this value to 1.
Schematic diagram is very simple and is shown in the figure below
Schematics, diagrams and documents

Schematics

Code

Arduino code

/*
 * =============================================================
 *   MobBob Control Program - Software Serial Bluetooth Version
 *   by Kevin Chan (aka Cevinius)
 * =============================================================
 *
 * This program enables MobBob to be controlled using serial commands. In this version of the code, the
 * commands are received over a software serial port, with pins defined in the #define near the top.
 * This means you can use any Arduino compatible board, and plug a bluetooth card into the pins set for
 * software serial. (As opposed to the other version of this designed for the Bluno board from DFRobot.)
 *
 * This program is long and contains 2 main components - a smooth servo animation program and a serial
 * command parser program.
 *
 * Animation System
 * ================
 * The animation program is designed to animate servo keyframe arrays smoothly. The code tries to do its
 * best to be easy to use.
 *
 * The animation system will only queue 1 command. i.e. One command can be running,
 * and one command can be queued up. If you send more commands, they will over-write the queued command.
 *
 * The animation system will by default wait to finish the current animation before starting the next. This
 * means that if the animation data ends with the robot in its base pose, things will join smoothly. To
 * support this, the animation system also has a feature where an animation can have a "finish sequence"
 * to put the robot back into the base pose. This feature is used for the walk forward/backward animations.
 * Those animations have a final sequence which puts the robot back into the base pose.
 *
 * When an animation is finished playing, the animation system will output a response string to the Serial port.
 * This enables the callers to know when the animations they've requested have finished playing. This is useful
 * for users to sequence animations - waiting for one to finish before starting another.
 *
 * The animation code has many variables to enable things to be tweaked. E.g. Update frequency, arduino pins, etc.
 *
 * The animation data array format is also designed to be easy to edit by hand.
 *
 * Command Parser
 * ==============
 * This system parses commands received over serial, and processes them. The commands include one for directly
 * setting servo positions, as well as commands for triggering pre-defined animations and walks.
 *
 * So, users who don't want to worry about the details of walking can just use the pre-defined walks/animations.
 * And, users who want complete control over the servos (to create new animations on the fly) can do that too.
 *
 * As mentioned above, these commands can be used interactively from the Arduino Serial Monitor. They can also be
 * sent in using Bluetooth LE (when a Bluno is used). The phone app will send the commands over Bluetooth LE to the
 * Bluno.
 *
 * General Commands:
 * -----------------
 *   Ready/OK Check: <OK>
 *     Status check. The response is returned immediately to check if the controller is working.
 *
 *   Set Servo: <SV, time, leftHip, leftFoot, rightHip, rightFoot>
 *                time      - time to tween to specified angles, 0 will immediately jump to 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
 *     This command is used to get full control over the servos. You can tween the robot from its
 *     current pose to the specified pose over the duration specified.
 *
 *   Stop/Reset: <ST>
 *     Stops the robot after the current animation. Can be used to stop animations set to loop
 *     indefinitely. This can also be used to put the robot into its base pose (standing straight)
 *
 *   Stop Immediate: <SI>
 *     Stops the robot immediately without waiting to complete the current animation. This
 *     interrupts the robots current animation. Potentially the robot can be mid-animation
 *     and in an unstable pose, so be careful when using this.
 *
 * Standard Walk Commands:
 * -----------------------
 *   Forward:    <FW, #Times>, -1 means continuous, 0 or no param is the same as 1 time.
 *   Backward:   <BW, #Times>, -1 means continuous, 0 or no param is the same as 1 time.
 *   Turn Left:  <LT, #Times>, -1 means continuous, 0 or no param is the same as 1 time.
 *   Turn Right: <RT, #Times>, -1 means continuous, 0 or no param is the same as 1 time.
 *
 * Fun Animation Commands:
 * -----------------------
 *   Shake Head:      <SX, #Times>, -1 means continuous, 0 or no param is the same as 1 time.
 *
 *   Bounce:          <BX, #Times>, -1 means continuous, 0 or no param is the same as 1 time.
 *
 *   Wobble:          <WX, #Times>, -1 means continuous, 0 or no param is the same as 1 time.
 *   Wobble Left:     <WY, #Times>, -1 means continuous, 0 or no param is the same as 1 time.
 *   Wobble Right:    <WZ, #Times>, -1 means continuous, 0 or no param is the same as 1 time.
 *
 *   Tap Feet:        <TX, #Times>, -1 means continuous, 0 or no param is the same as 1 time.
 *   Tap Left Foot:   <TY, #Times>, -1 means continuous, 0 or no param is the same as 1 time.
 *   Tap Right Foot:  <TZ, #Times>, -1 means continuous, 0 or no param is the same as 1 time.
 *
 *   Shake Legs:      <LX, #Times>, -1 means continuous, 0 or no param is the same as 1 time.
 *   Shake Left Leg:  <LY, #Times>, -1 means continuous, 0 or no param is the same as 1 time.
 *   Shake Right Leg: <LZ, #Times>, -1 means continuous, 0 or no param is the same as 1 time.
 *
 * Also, the code will send a response string back over Serial when commands have finished 
 * executing. 
 *
 * If the command finished normally, the response string is the command code without
 * parameters. E.g. When it has finished moving forward, it will send the response "<FW>".
 *
 * If a command was interrupted with <SI>, the current animation may have been stopped midway.
 * In this case, the robot could be in a weird mid-way pose, and finishAnims may not have been
 * played. To let the user know this has happened, the response string will have the 
 * parameter -1. E.g If a walk was stopped midway using <SI>, the response string would be
 * <FW,-1> to indicate that the walk has stopped, but it was stopped midway.
 * (Note: If you use <ST> to stop, that will wait for the current animation cycle to complete
 * before stopping. So, animations won't get stopped midway in that case.)
 * 
 * Because the responses are sent after an animation is complete, the command sender can
 * look for the response strings to determine when the robot is ready for a new command.
 * E.g. If you use the command <FW,3>, the response string isn't sent until all 3 steps
 * (and finish anim) are completed. So, the command sender can wait for the response
 * string before telling the robot to do the next thing.
 */
 
#include <Servo.h>
#include <SoftwareSerial.h>

//----------------------------------------------------------------------------------
// Speed of serial communication - Set this for your serial (bluetooth) card.
//----------------------------------------------------------------------------------

// Serial communication speed with the bluetooth board.
// Some boards default to 9600. The board I have has a default value of 115200.
#define SERIAL_SPEED 115200

// Setup a Software Serial port on these pins.
const int rxPin = 11; // pin used to receive data
const int txPin = 12; // pin used to send data
SoftwareSerial softwareSerial(rxPin, txPin);


//----------------------------------------------------------------------------------
// Setup Arduino Pins - Set these for your particular robot.
//----------------------------------------------------------------------------------

const int SERVO_LEFT_HIP   = 5;
const int SERVO_LEFT_FOOT  = 2;
const int SERVO_RIGHT_HIP  = 3;
const int SERVO_RIGHT_FOOT = 4;

// I want this code to be usable on all 4-servo bipeds! (Like Bob, MobBob)
// I noticed that some builds mount the hip servos facing a different
// way to how I did MobBob's so, this setting lets you configure the code
// for either build style.
// 1  for MobBob style front facing hips (joint towards the front)
// -1 for Bob style back facing hips (joint towards the back)
#define FRONT_JOINT_HIPS 1


//----------------------------------------------------------------------------------
// Servo Max/Min/Centre Constants - Set these for your particular robot.
//----------------------------------------------------------------------------------

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 = 1465;
const int RIGHT_FOOT_MIN    = RIGHT_FOOT_CENTRE - 500;
const int RIGHT_FOOT_MAX    = RIGHT_FOOT_CENTRE + 500;


//------------------------------------------------------------------------------
// Helper functions to help calculate joint values in a more user-friendly way.
// You can adjust the signs here if the servos are setup in a different way.
// Updating here means the animation data doesn't need to be modified if the
// servos are setup differently.
// (E.g. Original Bob's hip servos are backwards to MobBob's.)
//
// (Also, I find it hard to remember the signs to use for each servo since they 
// are different for left/right hips, and for left/right feet.)
//------------------------------------------------------------------------------


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; }


//----------------------------------------------------------------------------------
// Keyframe animation data for standard walking gait and other servo animations
//
// Format is { <millseconds>, <leftHipMicros>, <leftFootMicrosecs>, <rightHipMicrosecs>, <rightFootMicrosecs> }
//     milliseconds    - time to tween to to this keyframe's positions. E.g. 500 means it'll take 500ms to go from the
//                       robot's position at the start of this frame to the position specified in this frame
//     leftHipMicros   - position of left hip in servo microsecs.
//     leftFootMicros  - position of left hip in servo microsecs.
//     rightHipMicros  - position of left hip in servo microsecs.
//     rightFootMicros - position of left hip in servo microsecs.
// 
// The servo micro values, support a special value of -1. If this value is give, it tells
// the animation code to ignore this servo in this keyframe. i.e. That servo will
// stay in the position it had at the start of this keyframe.
//
// Also, the first element in the animation data arry is special. It is a metadata element.
// The first element is { <Num Frames>, 0, 0, 0, 0 }, which tells us the number of frames
// in the animation. So, the first actual keyframe is in animData[1], and the last keyframe
// is in animData[<Num Frames>]. (Where <Num Frames> is the value in animData[0][0].)
//----------------------------------------------------------------------------------

// Constants to make accessing the keyframe arrays more human readable.
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;


// Constants used in the walking gait animation data.
const int FOOT_DELTA = 150;
const int HIP_DELTA  = FRONT_JOINT_HIPS * 120;


// Goes to the default standing straight position. Used by stopAnim().
int standStraightAnim[][5] = {
    // Metadata. First element is number of frames.
    { 1, 0, 0, 0, 0 },
    
    // Feet flat, Feet even
    { 300, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() }
};


// Prior to this, get the robot to Feet Flat, Feet Even (i.e. standStraightAnim).
int walkForwardAnim[][5] = {
    // Metadata. First element is number of frames.
    { 8, 0, 0, 0, 0 },
    
    // Tilt to left, Feet even
    { 300, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootDown(FOOT_DELTA) },
    
    // Tilt to left, Right foot forward
    { 300, LeftHipIn(HIP_DELTA), LeftFootUp(FOOT_DELTA), RightHipOut(HIP_DELTA), RightFootDown(FOOT_DELTA) },
    
    // Feet flat, Right foot forward
    { 300, LeftHipIn(HIP_DELTA), LeftFootFlat(), RightHipOut(HIP_DELTA), RightFootFlat() },
    
    // Tilt to right, Right foot forward
    { 300, LeftHipIn(HIP_DELTA), LeftFootDown(FOOT_DELTA), RightHipOut(HIP_DELTA), RightFootUp(FOOT_DELTA) },
    
    // Tilt to right, Feet even
    { 300, LeftHipCentre(), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) },
    
    // Tilt to right, Left foot forward
    { 300, LeftHipOut(HIP_DELTA), LeftFootDown(FOOT_DELTA), RightHipIn(HIP_DELTA), RightFootUp(FOOT_DELTA) },
    
    // Feet flat, Left foot forward
    { 300, LeftHipOut(HIP_DELTA), LeftFootFlat(), RightHipIn(HIP_DELTA), RightFootFlat() },
    
    // Tilt to left, Left foot forward
    { 300, LeftHipOut(HIP_DELTA), LeftFootUp(FOOT_DELTA), RightHipIn(HIP_DELTA), RightFootDown(FOOT_DELTA) }
};


// Prior to this, get the robot to Feet Flat, Feet Even (i.e. standStraightAnim).
int walkBackwardAnim[][5] = {
    // Metadata. First element is number of frames.
    { 8, 0, 0, 0, 0 },
    
    // Tilt to left, Feet even
    { 300, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootDown(FOOT_DELTA) },
    
    // Tilt to left, Left foot forward
    { 300, LeftHipOut(HIP_DELTA), LeftFootUp(FOOT_DELTA), RightHipIn(HIP_DELTA), RightFootDown(FOOT_DELTA) },

    // Feet flat, Left foot forward
    { 300, LeftHipOut(HIP_DELTA), LeftFootFlat(), RightHipIn(HIP_DELTA), RightFootFlat() },
        
    // Tilt to right, Left foot forward
    { 300, LeftHipOut(HIP_DELTA), LeftFootDown(FOOT_DELTA), RightHipIn(HIP_DELTA), RightFootUp(FOOT_DELTA) },
    
    // Tilt to right, Feet even
    { 300, LeftHipCentre(), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) },
    
    // Tilt to right, Right foot forward
    { 300, LeftHipIn(HIP_DELTA), LeftFootDown(FOOT_DELTA), RightHipOut(HIP_DELTA), RightFootUp(FOOT_DELTA) },
    
    // Feet flat, Right foot forward
    { 300, LeftHipIn(HIP_DELTA), LeftFootFlat(), RightHipOut(HIP_DELTA), RightFootFlat() },
    
    // Tilt to left, Right foot forward
    { 300, LeftHipIn(HIP_DELTA), LeftFootUp(FOOT_DELTA), RightHipOut(HIP_DELTA), RightFootDown(FOOT_DELTA) }
};

// Finish walk anim takes the robot from the end of walkForwardAnim/walkBackwardAnim back to standStraightAnim.
int walkEndAnim[][5] = {
    // Metadata. First element is number of frames.
    { 2, 0, 0, 0, 0 },
    
    // Tilt to left, Feet even
    { 300, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootDown(FOOT_DELTA) },

    // Feet flat, Feet even
    { 300, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() }
};


// Prior to this, get the robot to Feet Flat, Feet Even (i.e. standStraightAnim).
int turnLeftAnim[][5] = {
    // Metadata. First element is number of frames.
    { 6, 0, 0, 0, 0 },
    
    // Tilt to left, Feet even
    { 300, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootDown(FOOT_DELTA) },
    
    // Tilt to left, Turn left hip, Turn right hip
    { 300, LeftHipIn(HIP_DELTA), LeftFootUp(FOOT_DELTA), RightHipIn(HIP_DELTA), RightFootDown(FOOT_DELTA) },

    // Feet flat, Turn left hip, Turn right hip
    { 300, LeftHipIn(HIP_DELTA), LeftFootFlat(), RightHipIn(HIP_DELTA), RightFootFlat() },
        
    // Tilt to right, Turn left hip, Turn right hip
    { 300, LeftHipIn(HIP_DELTA), LeftFootDown(FOOT_DELTA), RightHipIn(HIP_DELTA), RightFootUp(FOOT_DELTA) },
    
    // Tilt to right, Feet even
    { 300, LeftHipCentre(), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) },
    
    // Feet flat, Feet even
    { 300, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() }
};


// Prior to this, get the robot to Feet Flat, Feet Even (i.e. standStraightAnim).
int turnRightAnim[][5] = {
    // Metadata. First element is number of frames.
    { 6, 0, 0, 0, 0 },
    
    // Tilt to right, Feet even
    { 300, LeftHipCentre(), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) },
    
    // Tilt to right, Turn left hip, Turn right hip
    { 300, LeftHipIn(HIP_DELTA), LeftFootDown(FOOT_DELTA), RightHipIn(HIP_DELTA), RightFootUp(FOOT_DELTA) },

    // Feet flat, Turn left hip, Turn right hip
    { 300, LeftHipIn(HIP_DELTA), LeftFootFlat(), RightHipIn(HIP_DELTA), RightFootFlat() },
        
    // Tilt to left, Turn left hip, Turn right hip
    { 300, LeftHipIn(HIP_DELTA), LeftFootUp(FOOT_DELTA), RightHipIn(HIP_DELTA), RightFootDown(FOOT_DELTA) },
    
    // Tilt to left, Feet even
    { 300, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootDown(FOOT_DELTA) },
    
    // Feet flat, Feet even
    { 300, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() }
};


// Shake head anim. Left right quickly to emulate shaking head.
int shakeHeadAnim[][5] = {
    // Metadata. First element is number of frames.
    { 4, 0, 0, 0, 0 },
    
    // Feet flat, Twist left
    { 150, LeftHipOut(HIP_DELTA), LeftFootFlat(), RightHipIn(HIP_DELTA), RightFootFlat() },
    
    // Feet flat, Feet even
    { 150, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() },
    
    // Feet flat, Twist right
    { 150, LeftHipIn(HIP_DELTA), LeftFootFlat(), RightHipOut(HIP_DELTA), RightFootFlat() },
    
    // Feet flat, Feet even
    { 150, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() }    
};


// Wobble anim. Tilt left and right to do a fun wobble.
int wobbleAnim[][5] = {
    // Metadata. First element is number of frames.
    { 4, 0, 0, 0, 0 },
    
    // Tilt left, Feet even
    { 300, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootDown(FOOT_DELTA) },
    
    // Feet flat, Feet even
    { 300, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() },
    
    // Tilt right, Feet even
    { 300, LeftHipCentre(), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) },
    
    // Feet flat, Feet even
    { 300, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() }    
};

// Wobble left anim. Tilt left and back.
int wobbleLeftAnim[][5] = {
    // Metadata. First element is number of frames.
    { 2, 0, 0, 0, 0 },
    
    // Tilt left, Feet even
    { 300, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootDown(FOOT_DELTA) },
    
    // Feet flat, Feet even
    { 300, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() },
};


// Wobble right anim. Tilt right and back.
int wobbleRightAnim[][5] = {
    // Metadata. First element is number of frames.
    { 2, 0, 0, 0, 0 },
    
    // Tilt right, Feet even
    { 300, LeftHipCentre(), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) },
    
    // Feet flat, Feet even
    { 300, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() }    
};


// Tap feet anim. Tap both feet.
int tapFeetAnim[][5] = {
    // Metadata. First element is number of frames.
    { 2, 0, 0, 0, 0 },
    
    // Raise both feet, Feet even
    { 500, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) },
    
    // Feet flat, Feet even
    { 500, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() },
};


// Tap left foot anim.
int tapLeftFootAnim[][5] = {
    // Metadata. First element is number of frames.
    { 2, 0, 0, 0, 0 },
    
    // Raise left foot, Feet even
    { 500, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootFlat() },
    
    // Feet flat, Feet even
    { 500, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() },
};


// Tap right foot anim.
int tapRightFootAnim[][5] = {
    // Metadata. First element is number of frames.
    { 2, 0, 0, 0, 0 },
    
    // Raise right foot, Feet even
    { 500, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootUp(FOOT_DELTA) },
    
    // Feet flat, Feet even
    { 500, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() },
};


// Bounce up and down anim.
int bounceAnim[][5] = {
    // Metadata. First element is number of frames.
    { 2, 0, 0, 0, 0 },
    
    // Raise both feet, Feet even
    { 500, LeftHipCentre(), LeftFootDown(300), RightHipCentre(), RightFootDown(300) },
    
    // Feet flat, Feet even
    { 500, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() },
};


// Shake Legs Animation.
int shakeLegsAnim[][5] = {
    // Metadata. First element is number of frames.
    { 14, 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) },
    
    // Feet flat, Feet even
    { 300, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() },
    
    // Tilt right, Feet even
    { 300, LeftHipCentre(), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) },
    
    // Tilt right, Left hip in
    { 100, LeftHipIn(HIP_DELTA), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) },
    
    // Tilt right, Feet even
    { 100, LeftHipCentre(), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) },
    
    // Tilt right, Left hip out
    { 100, LeftHipOut(HIP_DELTA), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) },
    
    // Tilt right, Feet even
    { 100, LeftHipCentre(), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) },
    
    // Feet flat, Feet even
    { 300, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() }    
};


// Shake Left Leg Animation.
int shakeLeftLegAnim[][5] = {
    // Metadata. First element is number of frames.
    { 12, 0, 0, 0, 0 },
    
    // Tilt right, Feet even
    { 300, LeftHipCentre(), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) },
    
    // Tilt right, Left hip in
    { 100, LeftHipIn(HIP_DELTA), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) },
    
    // Tilt right, Feet even
    { 100, LeftHipCentre(), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) },
    
    // Tilt right, Left hip out
    { 100, LeftHipOut(HIP_DELTA), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) },
    
    // Tilt right, Feet even
    { 100, LeftHipCentre(), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) },
    
    // Tilt right, Left hip in
    { 100, LeftHipIn(HIP_DELTA), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) },
    
    // Tilt right, Feet even
    { 100, LeftHipCentre(), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) },
    
    // Tilt right, Left hip out
    { 100, LeftHipOut(HIP_DELTA), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) },
    
    // Tilt right, Feet even
    { 100, LeftHipCentre(), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) },
    
    // Tilt right, Left hip in
    { 100, LeftHipIn(HIP_DELTA), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) },
    
    // Tilt right, Feet even
    { 100, LeftHipCentre(), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) },
    
    // Feet flat, Feet even
    { 300, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() }    
};


// Shake Right Leg Animation.
int shakeRightLegAnim[][5] = {
    // Metadata. First element is number of frames.
    { 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 from
int  targetFrame;         // Frame we are lerping to
int  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 playing

int  (*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 positions
int  currLeftHip;
int  currLeftFoot;
int  currRightHip;
int  currRightFoot;

// Servo positions at start of current keyframe
int  startLeftHip;
int  startLeftFoot;
int  startRightHip;
int  startRightFoot;


//-------------------------------------------------------------------------------
// Parser Variables
//-------------------------------------------------------------------------------

// Constant delimiter tag chars
const 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("<OK>");
}


// 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: <OK>
            if ((currCmd[0] == 'O') && (currCmd[1] == 'K'))
            {
                softwareSerial.println("<OK>");
            }
            
            // Set Servo: <SV, time, leftHip, leftFoot, rightHip, rightFoot>
            // 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: <ST>, 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: <SI>
            else if ((currCmd[0] == 'S') && (currCmd[1] == 'I'))
            {
                StopAnimImmediate("SI");
            }
            
            // Forward: <FW, #Times>, -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: <BW, #Times>, -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: <LT, #Times>, -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: <RT, #Times>, -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: <SX, #Times>, -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: <BX, #Times>, -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: <WX, #Times>, -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: <WY, #Times>, -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: <WZ, #Times>, -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: <TX, #Times>, -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: <TY, #Times>, -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: <TZ, #Times>, -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: <LX, #Times>, -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: <LY, #Times>, -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: <LZ, #Times>, -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 up
void 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];
}
Shop

Project Hub

Video

Blog

Shop home

3D printing

Single Board Computers

Development kits

Sensors

Internet of things

Robotics

Accessories

Product of the Week

Electromaker Educator

The Electromaker Show

The Electromaker Podcast

Arduino Robot Controlled By Android Smartphone

About the project

Project info

Items used in this project

Hardware components

Software apps and online services

Hand tools and fabrication machines

Story

Details

Schematics, diagrams and documents

Schematics

Code

Arduino code

Credits

mircemk

Leave your feedback...

Shop

Project Hub

Video

Blog

Shop home

3D printing

Single Board Computers

Development kits

Sensors

Internet of things

Robotics

Accessories

Product of the Week

Electromaker Educator

The Electromaker Show

The Electromaker Podcast

Arduino Robot Controlled By Android Smartphone

About the project

Project info

Items used in this project

Hardware components

Software apps and online services

Hand tools and fabrication machines

Story

Details

Schematics, diagrams and documents

Schematics

Code

Arduino code

Credits

mircemk

Leave your feedback...

Related products
