/*
* =============================================================
* 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];
}
Comments