├── ArduinoCode
    ├── MobBob-Control-Bluetooth
    │   └── MobBob-Control-Bluetooth.ino
    ├── MobBob-Control-Bluno
    │   └── MobBob-Control-Bluno.ino
    └── README.txt
└── LICENSE


/ArduinoCode/MobBob-Control-Bluetooth/MobBob-Control-Bluetooth.ino:
--------------------------------------------------------------------------------
   1 | /*
   2 |  * =============================================================
   3 |  *   MobBob Control Program - Software Serial Bluetooth Version
   4 |  *   by Kevin Chan (aka Cevinius)
   5 |  * =============================================================
   6 |  *
   7 |  * This program enables MobBob to be controlled using serial commands. In this version of the code, the
   8 |  * commands are received over a software serial port, with pins defined in the #define near the top.
   9 |  * This means you can use any Arduino compatible board, and plug a bluetooth card into the pins set for
  10 |  * software serial. (As opposed to the other version of this designed for the Bluno board from DFRobot.)
  11 |  *
  12 |  * This program is long and contains 2 main components - a smooth servo animation program and a serial
  13 |  * command parser program.
  14 |  *
  15 |  * Animation System
  16 |  * ================
  17 |  * The animation program is designed to animate servo keyframe arrays smoothly. The code tries to do its
  18 |  * best to be easy to use.
  19 |  *
  20 |  * The animation system will only queue 1 command. i.e. One command can be running,
  21 |  * and one command can be queued up. If you send more commands, they will over-write the queued command.
  22 |  *
  23 |  * The animation system will by default wait to finish the current animation before starting the next. This
  24 |  * means that if the animation data ends with the robot in its base pose, things will join smoothly. To
  25 |  * support this, the animation system also has a feature where an animation can have a "finish sequence"
  26 |  * to put the robot back into the base pose. This feature is used for the walk forward/backward animations.
  27 |  * Those animations have a final sequence which puts the robot back into the base pose.
  28 |  *
  29 |  * When an animation is finished playing, the animation system will output a response string to the Serial port.
  30 |  * This enables the callers to know when the animations they've requested have finished playing. This is useful
  31 |  * for users to sequence animations - waiting for one to finish before starting another.
  32 |  *
  33 |  * The animation code has many variables to enable things to be tweaked. E.g. Update frequency, arduino pins, etc.
  34 |  *
  35 |  * The animation data array format is also designed to be easy to edit by hand.
  36 |  *
  37 |  * Command Parser
  38 |  * ==============
  39 |  * This system parses commands received over serial, and processes them. The commands include one for directly
  40 |  * setting servo positions, as well as commands for triggering pre-defined animations and walks.
  41 |  *
  42 |  * So, users who don't want to worry about the details of walking can just use the pre-defined walks/animations.
  43 |  * And, users who want complete control over the servos (to create new animations on the fly) can do that too.
  44 |  *
  45 |  * As mentioned above, these commands can be used interactively from the Arduino Serial Monitor. They can also be
  46 |  * sent in using Bluetooth LE (when a Bluno is used). The phone app will send the commands over Bluetooth LE to the
  47 |  * Bluno.
  48 |  *
  49 |  * General Commands:
  50 |  * -----------------
  51 |  *   Ready/OK Check: <OK>
  52 |  *     Status check. The response is returned immediately to check if the controller is working.
  53 |  *
  54 |  *   Set Servo: <SV, time, leftHip, leftFoot, rightHip, rightFoot>
  55 |  *                time      - time to tween to specified angles, 0 will immediately jump to angles
  56 |  *                leftHip   - microsecs from centre. -ve is hip in, +ve is hip out 
  57 |  *                leftFoot  - microsecs from flat. -ve is foot down, +ve is foot up
  58 |  *                rightHip  - microsecs from centre. -ve is hip in, +ve is hip out 
  59 |  *                rightFoot - microsecs from flat. -ve is foot down, +ve is foot up
  60 |  *     This command is used to get full control over the servos. You can tween the robot from its
  61 |  *     current pose to the specified pose over the duration specified.
  62 |  *
  63 |  *   Stop/Reset: <ST>
  64 |  *     Stops the robot after the current animation. Can be used to stop animations set to loop
  65 |  *     indefinitely. This can also be used to put the robot into its base pose (standing straight)
  66 |  *
  67 |  *   Stop Immediate: <SI>
  68 |  *     Stops the robot immediately without waiting to complete the current animation. This
  69 |  *     interrupts the robots current animation. Potentially the robot can be mid-animation
  70 |  *     and in an unstable pose, so be careful when using this.
  71 |  *
  72 |  * Standard Walk Commands:
  73 |  * -----------------------
  74 |  *   Forward:    <FW, #Times>, -1 means continuous, 0 or no param is the same as 1 time.
  75 |  *   Backward:   <BW, #Times>, -1 means continuous, 0 or no param is the same as 1 time.
  76 |  *   Turn Left:  <LT, #Times>, -1 means continuous, 0 or no param is the same as 1 time.
  77 |  *   Turn Right: <RT, #Times>, -1 means continuous, 0 or no param is the same as 1 time.
  78 |  *
  79 |  * Fun Animation Commands:
  80 |  * -----------------------
  81 |  *   Shake Head:      <SX, #Times>, -1 means continuous, 0 or no param is the same as 1 time.
  82 |  *
  83 |  *   Bounce:          <BX, #Times>, -1 means continuous, 0 or no param is the same as 1 time.
  84 |  *
  85 |  *   Wobble:          <WX, #Times>, -1 means continuous, 0 or no param is the same as 1 time.
  86 |  *   Wobble Left:     <WY, #Times>, -1 means continuous, 0 or no param is the same as 1 time.
  87 |  *   Wobble Right:    <WZ, #Times>, -1 means continuous, 0 or no param is the same as 1 time.
  88 |  *
  89 |  *   Tap Feet:        <TX, #Times>, -1 means continuous, 0 or no param is the same as 1 time.
  90 |  *   Tap Left Foot:   <TY, #Times>, -1 means continuous, 0 or no param is the same as 1 time.
  91 |  *   Tap Right Foot:  <TZ, #Times>, -1 means continuous, 0 or no param is the same as 1 time.
  92 |  *
  93 |  *   Shake Legs:      <LX, #Times>, -1 means continuous, 0 or no param is the same as 1 time.
  94 |  *   Shake Left Leg:  <LY, #Times>, -1 means continuous, 0 or no param is the same as 1 time.
  95 |  *   Shake Right Leg: <LZ, #Times>, -1 means continuous, 0 or no param is the same as 1 time.
  96 |  *
  97 |  * Also, the code will send a response string back over Serial when commands have finished 
  98 |  * executing. 
  99 |  *
 100 |  * If the command finished normally, the response string is the command code without
 101 |  * parameters. E.g. When it has finished moving forward, it will send the response "<FW>".
 102 |  *
 103 |  * If a command was interrupted with <SI>, the current animation may have been stopped midway.
 104 |  * In this case, the robot could be in a weird mid-way pose, and finishAnims may not have been
 105 |  * played. To let the user know this has happened, the response string will have the 
 106 |  * parameter -1. E.g If a walk was stopped midway using <SI>, the response string would be
 107 |  * <FW,-1> to indicate that the walk has stopped, but it was stopped midway.
 108 |  * (Note: If you use <ST> to stop, that will wait for the current animation cycle to complete
 109 |  * before stopping. So, animations won't get stopped midway in that case.)
 110 |  * 
 111 |  * Because the responses are sent after an animation is complete, the command sender can
 112 |  * look for the response strings to determine when the robot is ready for a new command.
 113 |  * E.g. If you use the command <FW,3>, the response string isn't sent until all 3 steps
 114 |  * (and finish anim) are completed. So, the command sender can wait for the response
 115 |  * string before telling the robot to do the next thing.
 116 |  */
 117 |  
 118 | #include <Servo.h>
 119 | #include <SoftwareSerial.h>
 120 | 
 121 | //----------------------------------------------------------------------------------
 122 | // Speed of serial communication - Set this for your serial (bluetooth) card.
 123 | //----------------------------------------------------------------------------------
 124 | 
 125 | // Serial communication speed with the bluetooth board.
 126 | // Some boards default to 9600. The board I have has a default value of 115200.
 127 | #define SERIAL_SPEED 115200
 128 | 
 129 | // Setup a Software Serial port on these pins.
 130 | const int rxPin = 11; // pin used to receive data
 131 | const int txPin = 12; // pin used to send data
 132 | SoftwareSerial softwareSerial(rxPin, txPin);
 133 | 
 134 | 
 135 | //----------------------------------------------------------------------------------
 136 | // Setup Arduino Pins - Set these for your particular robot.
 137 | //----------------------------------------------------------------------------------
 138 | 
 139 | const int SERVO_LEFT_HIP   = 5;
 140 | const int SERVO_LEFT_FOOT  = 2;
 141 | const int SERVO_RIGHT_HIP  = 3;
 142 | const int SERVO_RIGHT_FOOT = 4;
 143 | 
 144 | // I want this code to be usable on all 4-servo bipeds! (Like Bob, MobBob)
 145 | // I noticed that some builds mount the hip servos facing a different
 146 | // way to how I did MobBob's so, this setting lets you configure the code
 147 | // for either build style.
 148 | // 1  for MobBob style front facing hips (joint towards the front)
 149 | // -1 for Bob style back facing hips (joint towards the back)
 150 | #define FRONT_JOINT_HIPS 1
 151 | 
 152 | 
 153 | //----------------------------------------------------------------------------------
 154 | // Servo Max/Min/Centre Constants - Set these for your particular robot.
 155 | //----------------------------------------------------------------------------------
 156 | 
 157 | const int LEFT_HIP_CENTRE = 1580;
 158 | const int LEFT_HIP_MIN    = LEFT_HIP_CENTRE - 500;
 159 | const int LEFT_HIP_MAX    = LEFT_HIP_CENTRE + 500;
 160 | 
 161 | const int LEFT_FOOT_CENTRE = 1410;
 162 | const int LEFT_FOOT_MIN    = LEFT_FOOT_CENTRE - 500;
 163 | const int LEFT_FOOT_MAX    = LEFT_FOOT_CENTRE + 500;
 164 | 
 165 | const int RIGHT_HIP_CENTRE = 1500;
 166 | const int RIGHT_HIP_MIN    = RIGHT_HIP_CENTRE - 500;
 167 | const int RIGHT_HIP_MAX    = RIGHT_HIP_CENTRE + 500;
 168 | 
 169 | const int RIGHT_FOOT_CENTRE = 1465;
 170 | const int RIGHT_FOOT_MIN    = RIGHT_FOOT_CENTRE - 500;
 171 | const int RIGHT_FOOT_MAX    = RIGHT_FOOT_CENTRE + 500;
 172 | 
 173 | 
 174 | //------------------------------------------------------------------------------
 175 | // Helper functions to help calculate joint values in a more user-friendly way.
 176 | // You can adjust the signs here if the servos are setup in a different way.
 177 | // Updating here means the animation data doesn't need to be modified if the
 178 | // servos are setup differently.
 179 | // (E.g. Original Bob's hip servos are backwards to MobBob's.)
 180 | //
 181 | // (Also, I find it hard to remember the signs to use for each servo since they 
 182 | // are different for left/right hips, and for left/right feet.)
 183 | //------------------------------------------------------------------------------
 184 | 
 185 | 
 186 | int LeftHipCentre()              { return LEFT_HIP_CENTRE; }
 187 | int LeftHipIn(int millisecs)     { return LEFT_HIP_CENTRE + (FRONT_JOINT_HIPS * millisecs); }
 188 | int LeftHipOut(int millisecs)    { return LEFT_HIP_CENTRE - (FRONT_JOINT_HIPS * millisecs); }
 189 | 
 190 | int RightHipCentre()             { return RIGHT_HIP_CENTRE; }
 191 | int RightHipIn(int millisecs)    { return RIGHT_HIP_CENTRE - (FRONT_JOINT_HIPS * millisecs); }
 192 | int RightHipOut(int millisecs)   { return RIGHT_HIP_CENTRE + (FRONT_JOINT_HIPS * millisecs); }
 193 | 
 194 | int LeftFootFlat()               { return LEFT_FOOT_CENTRE; }
 195 | int LeftFootUp(int millisecs)    { return LEFT_FOOT_CENTRE - millisecs; }
 196 | int LeftFootDown(int millisecs)  { return LEFT_FOOT_CENTRE + millisecs; }
 197 | 
 198 | int RightFootFlat()              { return RIGHT_FOOT_CENTRE; }
 199 | int RightFootUp(int millisecs)   { return RIGHT_FOOT_CENTRE + millisecs; }
 200 | int RightFootDown(int millisecs) { return RIGHT_FOOT_CENTRE - millisecs; }
 201 | 
 202 | 
 203 | //----------------------------------------------------------------------------------
 204 | // Keyframe animation data for standard walking gait and other servo animations
 205 | //
 206 | // Format is { <millseconds>, <leftHipMicros>, <leftFootMicrosecs>, <rightHipMicrosecs>, <rightFootMicrosecs> }
 207 | //     milliseconds    - time to tween to to this keyframe's positions. E.g. 500 means it'll take 500ms to go from the
 208 | //                       robot's position at the start of this frame to the position specified in this frame
 209 | //     leftHipMicros   - position of left hip in servo microsecs.
 210 | //     leftFootMicros  - position of left hip in servo microsecs.
 211 | //     rightHipMicros  - position of left hip in servo microsecs.
 212 | //     rightFootMicros - position of left hip in servo microsecs.
 213 | // 
 214 | // The servo micro values, support a special value of -1. If this value is give, it tells
 215 | // the animation code to ignore this servo in this keyframe. i.e. That servo will
 216 | // stay in the position it had at the start of this keyframe.
 217 | //
 218 | // Also, the first element in the animation data arry is special. It is a metadata element.
 219 | // The first element is { <Num Frames>, 0, 0, 0, 0 }, which tells us the number of frames
 220 | // in the animation. So, the first actual keyframe is in animData[1], and the last keyframe
 221 | // is in animData[<Num Frames>]. (Where <Num Frames> is the value in animData[0][0].)
 222 | //----------------------------------------------------------------------------------
 223 | 
 224 | // Constants to make accessing the keyframe arrays more human readable.
 225 | const int TWEEN_TIME_VALUE = 0;
 226 | const int LEFT_HIP_VALUE   = 1;
 227 | const int LEFT_FOOT_VALUE  = 2;
 228 | const int RIGHT_HIP_VALUE  = 3;
 229 | const int RIGHT_FOOT_VALUE = 4;
 230 | 
 231 | 
 232 | // Constants used in the walking gait animation data.
 233 | const int FOOT_DELTA = 150;
 234 | const int HIP_DELTA  = FRONT_JOINT_HIPS * 120;
 235 | 
 236 | 
 237 | // Goes to the default standing straight position. Used by stopAnim().
 238 | int standStraightAnim[][5] = {
 239 |     // Metadata. First element is number of frames.
 240 |     { 1, 0, 0, 0, 0 },
 241 |     
 242 |     // Feet flat, Feet even
 243 |     { 300, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() }
 244 | };
 245 | 
 246 | 
 247 | // Prior to this, get the robot to Feet Flat, Feet Even (i.e. standStraightAnim).
 248 | int walkForwardAnim[][5] = {
 249 |     // Metadata. First element is number of frames.
 250 |     { 8, 0, 0, 0, 0 },
 251 |     
 252 |     // Tilt to left, Feet even
 253 |     { 300, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootDown(FOOT_DELTA) },
 254 |     
 255 |     // Tilt to left, Right foot forward
 256 |     { 300, LeftHipIn(HIP_DELTA), LeftFootUp(FOOT_DELTA), RightHipOut(HIP_DELTA), RightFootDown(FOOT_DELTA) },
 257 |     
 258 |     // Feet flat, Right foot forward
 259 |     { 300, LeftHipIn(HIP_DELTA), LeftFootFlat(), RightHipOut(HIP_DELTA), RightFootFlat() },
 260 |     
 261 |     // Tilt to right, Right foot forward
 262 |     { 300, LeftHipIn(HIP_DELTA), LeftFootDown(FOOT_DELTA), RightHipOut(HIP_DELTA), RightFootUp(FOOT_DELTA) },
 263 |     
 264 |     // Tilt to right, Feet even
 265 |     { 300, LeftHipCentre(), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) },
 266 |     
 267 |     // Tilt to right, Left foot forward
 268 |     { 300, LeftHipOut(HIP_DELTA), LeftFootDown(FOOT_DELTA), RightHipIn(HIP_DELTA), RightFootUp(FOOT_DELTA) },
 269 |     
 270 |     // Feet flat, Left foot forward
 271 |     { 300, LeftHipOut(HIP_DELTA), LeftFootFlat(), RightHipIn(HIP_DELTA), RightFootFlat() },
 272 |     
 273 |     // Tilt to left, Left foot forward
 274 |     { 300, LeftHipOut(HIP_DELTA), LeftFootUp(FOOT_DELTA), RightHipIn(HIP_DELTA), RightFootDown(FOOT_DELTA) }
 275 | };
 276 | 
 277 | 
 278 | // Prior to this, get the robot to Feet Flat, Feet Even (i.e. standStraightAnim).
 279 | int walkBackwardAnim[][5] = {
 280 |     // Metadata. First element is number of frames.
 281 |     { 8, 0, 0, 0, 0 },
 282 |     
 283 |     // Tilt to left, Feet even
 284 |     { 300, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootDown(FOOT_DELTA) },
 285 |     
 286 |     // Tilt to left, Left foot forward
 287 |     { 300, LeftHipOut(HIP_DELTA), LeftFootUp(FOOT_DELTA), RightHipIn(HIP_DELTA), RightFootDown(FOOT_DELTA) },
 288 | 
 289 |     // Feet flat, Left foot forward
 290 |     { 300, LeftHipOut(HIP_DELTA), LeftFootFlat(), RightHipIn(HIP_DELTA), RightFootFlat() },
 291 |         
 292 |     // Tilt to right, Left foot forward
 293 |     { 300, LeftHipOut(HIP_DELTA), LeftFootDown(FOOT_DELTA), RightHipIn(HIP_DELTA), RightFootUp(FOOT_DELTA) },
 294 |     
 295 |     // Tilt to right, Feet even
 296 |     { 300, LeftHipCentre(), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) },
 297 |     
 298 |     // Tilt to right, Right foot forward
 299 |     { 300, LeftHipIn(HIP_DELTA), LeftFootDown(FOOT_DELTA), RightHipOut(HIP_DELTA), RightFootUp(FOOT_DELTA) },
 300 |     
 301 |     // Feet flat, Right foot forward
 302 |     { 300, LeftHipIn(HIP_DELTA), LeftFootFlat(), RightHipOut(HIP_DELTA), RightFootFlat() },
 303 |     
 304 |     // Tilt to left, Right foot forward
 305 |     { 300, LeftHipIn(HIP_DELTA), LeftFootUp(FOOT_DELTA), RightHipOut(HIP_DELTA), RightFootDown(FOOT_DELTA) }
 306 | };
 307 | 
 308 | // Finish walk anim takes the robot from the end of walkForwardAnim/walkBackwardAnim back to standStraightAnim.
 309 | int walkEndAnim[][5] = {
 310 |     // Metadata. First element is number of frames.
 311 |     { 2, 0, 0, 0, 0 },
 312 |     
 313 |     // Tilt to left, Feet even
 314 |     { 300, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootDown(FOOT_DELTA) },
 315 | 
 316 |     // Feet flat, Feet even
 317 |     { 300, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() }
 318 | };
 319 | 
 320 | 
 321 | // Prior to this, get the robot to Feet Flat, Feet Even (i.e. standStraightAnim).
 322 | int turnLeftAnim[][5] = {
 323 |     // Metadata. First element is number of frames.
 324 |     { 6, 0, 0, 0, 0 },
 325 |     
 326 |     // Tilt to left, Feet even
 327 |     { 300, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootDown(FOOT_DELTA) },
 328 |     
 329 |     // Tilt to left, Turn left hip, Turn right hip
 330 |     { 300, LeftHipIn(HIP_DELTA), LeftFootUp(FOOT_DELTA), RightHipIn(HIP_DELTA), RightFootDown(FOOT_DELTA) },
 331 | 
 332 |     // Feet flat, Turn left hip, Turn right hip
 333 |     { 300, LeftHipIn(HIP_DELTA), LeftFootFlat(), RightHipIn(HIP_DELTA), RightFootFlat() },
 334 |         
 335 |     // Tilt to right, Turn left hip, Turn right hip
 336 |     { 300, LeftHipIn(HIP_DELTA), LeftFootDown(FOOT_DELTA), RightHipIn(HIP_DELTA), RightFootUp(FOOT_DELTA) },
 337 |     
 338 |     // Tilt to right, Feet even
 339 |     { 300, LeftHipCentre(), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) },
 340 |     
 341 |     // Feet flat, Feet even
 342 |     { 300, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() }
 343 | };
 344 | 
 345 | 
 346 | // Prior to this, get the robot to Feet Flat, Feet Even (i.e. standStraightAnim).
 347 | int turnRightAnim[][5] = {
 348 |     // Metadata. First element is number of frames.
 349 |     { 6, 0, 0, 0, 0 },
 350 |     
 351 |     // Tilt to right, Feet even
 352 |     { 300, LeftHipCentre(), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) },
 353 |     
 354 |     // Tilt to right, Turn left hip, Turn right hip
 355 |     { 300, LeftHipIn(HIP_DELTA), LeftFootDown(FOOT_DELTA), RightHipIn(HIP_DELTA), RightFootUp(FOOT_DELTA) },
 356 | 
 357 |     // Feet flat, Turn left hip, Turn right hip
 358 |     { 300, LeftHipIn(HIP_DELTA), LeftFootFlat(), RightHipIn(HIP_DELTA), RightFootFlat() },
 359 |         
 360 |     // Tilt to left, Turn left hip, Turn right hip
 361 |     { 300, LeftHipIn(HIP_DELTA), LeftFootUp(FOOT_DELTA), RightHipIn(HIP_DELTA), RightFootDown(FOOT_DELTA) },
 362 |     
 363 |     // Tilt to left, Feet even
 364 |     { 300, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootDown(FOOT_DELTA) },
 365 |     
 366 |     // Feet flat, Feet even
 367 |     { 300, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() }
 368 | };
 369 | 
 370 | 
 371 | // Shake head anim. Left right quickly to emulate shaking head.
 372 | int shakeHeadAnim[][5] = {
 373 |     // Metadata. First element is number of frames.
 374 |     { 4, 0, 0, 0, 0 },
 375 |     
 376 |     // Feet flat, Twist left
 377 |     { 150, LeftHipOut(HIP_DELTA), LeftFootFlat(), RightHipIn(HIP_DELTA), RightFootFlat() },
 378 |     
 379 |     // Feet flat, Feet even
 380 |     { 150, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() },
 381 |     
 382 |     // Feet flat, Twist right
 383 |     { 150, LeftHipIn(HIP_DELTA), LeftFootFlat(), RightHipOut(HIP_DELTA), RightFootFlat() },
 384 |     
 385 |     // Feet flat, Feet even
 386 |     { 150, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() }    
 387 | };
 388 | 
 389 | 
 390 | // Wobble anim. Tilt left and right to do a fun wobble.
 391 | int wobbleAnim[][5] = {
 392 |     // Metadata. First element is number of frames.
 393 |     { 4, 0, 0, 0, 0 },
 394 |     
 395 |     // Tilt left, Feet even
 396 |     { 300, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootDown(FOOT_DELTA) },
 397 |     
 398 |     // Feet flat, Feet even
 399 |     { 300, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() },
 400 |     
 401 |     // Tilt right, Feet even
 402 |     { 300, LeftHipCentre(), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) },
 403 |     
 404 |     // Feet flat, Feet even
 405 |     { 300, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() }    
 406 | };
 407 | 
 408 | // Wobble left anim. Tilt left and back.
 409 | int wobbleLeftAnim[][5] = {
 410 |     // Metadata. First element is number of frames.
 411 |     { 2, 0, 0, 0, 0 },
 412 |     
 413 |     // Tilt left, Feet even
 414 |     { 300, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootDown(FOOT_DELTA) },
 415 |     
 416 |     // Feet flat, Feet even
 417 |     { 300, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() },
 418 | };
 419 | 
 420 | 
 421 | // Wobble right anim. Tilt right and back.
 422 | int wobbleRightAnim[][5] = {
 423 |     // Metadata. First element is number of frames.
 424 |     { 2, 0, 0, 0, 0 },
 425 |     
 426 |     // Tilt right, Feet even
 427 |     { 300, LeftHipCentre(), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) },
 428 |     
 429 |     // Feet flat, Feet even
 430 |     { 300, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() }    
 431 | };
 432 | 
 433 | 
 434 | // Tap feet anim. Tap both feet.
 435 | int tapFeetAnim[][5] = {
 436 |     // Metadata. First element is number of frames.
 437 |     { 2, 0, 0, 0, 0 },
 438 |     
 439 |     // Raise both feet, Feet even
 440 |     { 500, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) },
 441 |     
 442 |     // Feet flat, Feet even
 443 |     { 500, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() },
 444 | };
 445 | 
 446 | 
 447 | // Tap left foot anim.
 448 | int tapLeftFootAnim[][5] = {
 449 |     // Metadata. First element is number of frames.
 450 |     { 2, 0, 0, 0, 0 },
 451 |     
 452 |     // Raise left foot, Feet even
 453 |     { 500, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootFlat() },
 454 |     
 455 |     // Feet flat, Feet even
 456 |     { 500, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() },
 457 | };
 458 | 
 459 | 
 460 | // Tap right foot anim.
 461 | int tapRightFootAnim[][5] = {
 462 |     // Metadata. First element is number of frames.
 463 |     { 2, 0, 0, 0, 0 },
 464 |     
 465 |     // Raise right foot, Feet even
 466 |     { 500, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootUp(FOOT_DELTA) },
 467 |     
 468 |     // Feet flat, Feet even
 469 |     { 500, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() },
 470 | };
 471 | 
 472 | 
 473 | // Bounce up and down anim.
 474 | int bounceAnim[][5] = {
 475 |     // Metadata. First element is number of frames.
 476 |     { 2, 0, 0, 0, 0 },
 477 |     
 478 |     // Raise both feet, Feet even
 479 |     { 500, LeftHipCentre(), LeftFootDown(300), RightHipCentre(), RightFootDown(300) },
 480 |     
 481 |     // Feet flat, Feet even
 482 |     { 500, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() },
 483 | };
 484 | 
 485 | 
 486 | // Shake Legs Animation.
 487 | int shakeLegsAnim[][5] = {
 488 |     // Metadata. First element is number of frames.
 489 |     { 14, 0, 0, 0, 0 },
 490 |     
 491 |     // Tilt left, Feet even
 492 |     { 300, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootDown(FOOT_DELTA) },
 493 |     
 494 |     // Tilt left, Right hip in
 495 |     { 100, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipIn(HIP_DELTA), RightFootDown(FOOT_DELTA) },
 496 |     
 497 |     // Tilt left, Feet even
 498 |     { 100, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootDown(FOOT_DELTA) },
 499 |     
 500 |     // Tilt left, Right hip out
 501 |     { 100, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipOut(HIP_DELTA), RightFootDown(FOOT_DELTA) },
 502 |     
 503 |     // Tilt left, Feet even
 504 |     { 100, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootDown(FOOT_DELTA) },
 505 |     
 506 |     // Tilt left, Right hip in
 507 |     { 100, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipIn(HIP_DELTA), RightFootDown(FOOT_DELTA) },
 508 |     
 509 |     // Tilt left, Feet even
 510 |     { 100, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootDown(FOOT_DELTA) },
 511 |     
 512 |     // Feet flat, Feet even
 513 |     { 300, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() },
 514 |     
 515 |     // Tilt right, Feet even
 516 |     { 300, LeftHipCentre(), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) },
 517 |     
 518 |     // Tilt right, Left hip in
 519 |     { 100, LeftHipIn(HIP_DELTA), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) },
 520 |     
 521 |     // Tilt right, Feet even
 522 |     { 100, LeftHipCentre(), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) },
 523 |     
 524 |     // Tilt right, Left hip out
 525 |     { 100, LeftHipOut(HIP_DELTA), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) },
 526 |     
 527 |     // Tilt right, Feet even
 528 |     { 100, LeftHipCentre(), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) },
 529 |     
 530 |     // Feet flat, Feet even
 531 |     { 300, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() }    
 532 | };
 533 | 
 534 | 
 535 | // Shake Left Leg Animation.
 536 | int shakeLeftLegAnim[][5] = {
 537 |     // Metadata. First element is number of frames.
 538 |     { 12, 0, 0, 0, 0 },
 539 |     
 540 |     // Tilt right, Feet even
 541 |     { 300, LeftHipCentre(), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) },
 542 |     
 543 |     // Tilt right, Left hip in
 544 |     { 100, LeftHipIn(HIP_DELTA), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) },
 545 |     
 546 |     // Tilt right, Feet even
 547 |     { 100, LeftHipCentre(), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) },
 548 |     
 549 |     // Tilt right, Left hip out
 550 |     { 100, LeftHipOut(HIP_DELTA), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) },
 551 |     
 552 |     // Tilt right, Feet even
 553 |     { 100, LeftHipCentre(), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) },
 554 |     
 555 |     // Tilt right, Left hip in
 556 |     { 100, LeftHipIn(HIP_DELTA), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) },
 557 |     
 558 |     // Tilt right, Feet even
 559 |     { 100, LeftHipCentre(), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) },
 560 |     
 561 |     // Tilt right, Left hip out
 562 |     { 100, LeftHipOut(HIP_DELTA), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) },
 563 |     
 564 |     // Tilt right, Feet even
 565 |     { 100, LeftHipCentre(), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) },
 566 |     
 567 |     // Tilt right, Left hip in
 568 |     { 100, LeftHipIn(HIP_DELTA), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) },
 569 |     
 570 |     // Tilt right, Feet even
 571 |     { 100, LeftHipCentre(), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) },
 572 |     
 573 |     // Feet flat, Feet even
 574 |     { 300, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() }    
 575 | };
 576 | 
 577 | 
 578 | // Shake Right Leg Animation.
 579 | int shakeRightLegAnim[][5] = {
 580 |     // Metadata. First element is number of frames.
 581 |     { 12, 0, 0, 0, 0 },
 582 |     
 583 |     // Tilt left, Feet even
 584 |     { 300, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootDown(FOOT_DELTA) },
 585 |     
 586 |     // Tilt left, Right hip in
 587 |     { 100, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipIn(HIP_DELTA), RightFootDown(FOOT_DELTA) },
 588 |     
 589 |     // Tilt left, Feet even
 590 |     { 100, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootDown(FOOT_DELTA) },
 591 |     
 592 |     // Tilt left, Right hip out
 593 |     { 100, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipOut(HIP_DELTA), RightFootDown(FOOT_DELTA) },
 594 |     
 595 |     // Tilt left, Feet even
 596 |     { 100, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootDown(FOOT_DELTA) },
 597 |     
 598 |     // Tilt left, Right hip in
 599 |     { 100, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipIn(HIP_DELTA), RightFootDown(FOOT_DELTA) },
 600 |     
 601 |     // Tilt left, Feet even
 602 |     { 100, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootDown(FOOT_DELTA) },
 603 |     
 604 |     // Tilt left, Right hip out
 605 |     { 100, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipOut(HIP_DELTA), RightFootDown(FOOT_DELTA) },
 606 |     
 607 |     // Tilt left, Feet even
 608 |     { 100, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootDown(FOOT_DELTA) },
 609 |     
 610 |     // Tilt left, Right hip in
 611 |     { 100, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipIn(HIP_DELTA), RightFootDown(FOOT_DELTA) },
 612 |     
 613 |     // Tilt left, Feet even
 614 |     { 100, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootDown(FOOT_DELTA) },
 615 |     
 616 |     // Feet flat, Feet even
 617 |     { 300, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() },    
 618 | };
 619 | 
 620 | 
 621 | //----------------------------------------------------------------------------------
 622 | // Special dynamic animation data for setting/tweening servo positions.
 623 | //----------------------------------------------------------------------------------
 624 | 
 625 | // These are 2 special anim data that we use for the SetServos() function. They have
 626 | // a single frame. Those will change the data in these anim data and play them to 
 627 | // move the servos.
 628 | int setServosAnim1[][5] = {
 629 |     // Metadata. First element is number of frames.
 630 |     { 1, 0, 0, 0, 0 },
 631 |     
 632 |     // Tilt left, Feet even
 633 |     { 0, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() }
 634 | };
 635 | 
 636 | int setServosAnim2[][5] = {
 637 |     // Metadata. First element is number of frames.
 638 |     { 1, 0, 0, 0, 0 },
 639 |     
 640 |     // Tilt left, Feet even
 641 |     { 0, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() }
 642 | };
 643 | 
 644 | 
 645 | //----------------------------------------------------------------------------------
 646 | // Servo Variables
 647 | //----------------------------------------------------------------------------------
 648 | 
 649 | Servo servoLeftHip;
 650 | Servo servoLeftFoot;
 651 | Servo servoRightHip;
 652 | Servo servoRightFoot;
 653 | 
 654 | //----------------------------------------------------------------------------------
 655 | // State variables for playing animations.
 656 | //----------------------------------------------------------------------------------
 657 | 
 658 | // Milliseconds between animation updates.
 659 | const int millisBetweenAnimUpdate = 20;
 660 | 
 661 | // Time when we did the last animation update.
 662 | long timeAtLastAnimUpdate;
 663 | 
 664 | // Related to currently playing anim.
 665 | int  (*currAnim)[5];      // Current animation we're playing.
 666 | int  (*finishAnim)[5];    // Animation to play when the currAnim finishes or is stopped.
 667 | long timeAtStartOfFrame;  // millis() at last keyframe - frame we're lerping from
 668 | int  targetFrame;         // Frame we are lerping to
 669 | int  animNumLoops;        // Number of times to play the animation. -1 means loop forever.
 670 | char animCompleteStr[3] = "--"; // This is a 2 character string. When the anim is complete, 
 671 |                                 // we print out the status as "<" + animComplereStr + ">".
 672 | 
 673 | // Related to anim queue. I.e. Next anim to play.
 674 | bool animInProgress;    // Whether an animation is playing
 675 | 
 676 | int  (*nextAnim)[5];      // This is the next animation to play once the current one is done. 
 677 |                           // i.e. It's like a queue of size 1!
 678 |                           // If curr is non-looping, we play this at the end of the current anim.
 679 |                           // If curr is looping, this starts at the end of the current loop, 
 680 |                           // replacing curr anim.
 681 |                           // If nothing is playing, this starts right away.
 682 |                           
 683 | int  (*nextFinishAnim)[5]; // This is the finish animation for the queued animation.
 684 | 
 685 | int  nextAnimNumLoops;    // Number of times to play the animation. -1 means loop forever.
 686 | 
 687 | char nextAnimCompleteStr[3] = "--"; // This is a 2 character string. When the anim is complete, 
 688 |                                     // we print out the status as "<" + animComplereStr + ">".
 689 | 
 690 | bool interruptInProgressAnim; // Whether to change anim immediately, interrupting the current one.
 691 | 
 692 | 
 693 | // Curr servo positions
 694 | int  currLeftHip;
 695 | int  currLeftFoot;
 696 | int  currRightHip;
 697 | int  currRightFoot;
 698 | 
 699 | // Servo positions at start of current keyframe
 700 | int  startLeftHip;
 701 | int  startLeftFoot;
 702 | int  startRightHip;
 703 | int  startRightFoot;
 704 | 
 705 | 
 706 | //-------------------------------------------------------------------------------
 707 | // Parser Variables
 708 | //-------------------------------------------------------------------------------
 709 | 
 710 | // Constant delimiter tag chars
 711 | const char START_CHAR = '<';
 712 | const char END_CHAR   = '>';
 713 | const char SEP_CHAR   = ',';
 714 | 
 715 | // Constants and a variable for the parser state.
 716 | const int PARSER_WAITING = 0; // Waiting for '<' to start parsing.
 717 | const int PARSER_COMMAND = 1; // Reading the command string.
 718 | const int PARSER_PARAM1  = 2; // Reading param 1.
 719 | const int PARSER_PARAM2  = 3; // Reading param 2.
 720 | const int PARSER_PARAM3  = 4; // Reading param 3.
 721 | const int PARSER_PARAM4  = 5; // Reading param 3.
 722 | const int PARSER_PARAM5  = 6; // Reading param 3.
 723 | const int PARSER_EXECUTE = 7; // Finished parsing a command, so execute it.
 724 | 
 725 | // Current parser state.
 726 | int currParserState = PARSER_WAITING; 
 727 | 
 728 | // String for storing the command. 2 chars for the command and 1 char for '\0'.
 729 | // We store the command here as we're parsing.
 730 | char currCmd[3] = "--";
 731 | 
 732 | // For tracking which letter we are in the command.
 733 | int currCmdIndex;
 734 | 
 735 | // Max command length.
 736 | const int CMD_LENGTH = 2;
 737 | 
 738 | 
 739 | // Current param values. Store them here after we parse them.
 740 | int currParam1Val;
 741 | int currParam2Val;
 742 | int currParam3Val;
 743 | int currParam4Val;
 744 | int currParam5Val;
 745 | 
 746 | // Variable for tracking which digit we're parsing in a param.
 747 | // We use this to convert the single digits back into a decimal value.
 748 | int currParamIndex;
 749 | 
 750 | // Whether the current param is negative.
 751 | boolean currParamNegative;
 752 | 
 753 | // Max parameter length. Stop parsing if it exceeds this.
 754 | const int MAX_PARAM_LENGTH = 6;
 755 | 
 756 | 
 757 | //===============================================================================
 758 | // Arduino setup() and loop().
 759 | //===============================================================================
 760 | 
 761 | void setup() 
 762 | {
 763 |     // Setup the main serial port
 764 |     softwareSerial.begin(SERIAL_SPEED);
 765 |     
 766 |     // Setup the Servos
 767 |     servoLeftHip.attach(  SERVO_LEFT_HIP,   LEFT_HIP_MIN,   LEFT_HIP_MAX);
 768 |     servoLeftFoot.attach( SERVO_LEFT_FOOT,  LEFT_FOOT_MIN,  LEFT_FOOT_MAX);
 769 |     servoRightHip.attach( SERVO_RIGHT_HIP,  RIGHT_HIP_MIN,  RIGHT_HIP_MAX);
 770 |     servoRightFoot.attach(SERVO_RIGHT_FOOT, RIGHT_FOOT_MIN, RIGHT_FOOT_MAX);
 771 | 
 772 |     // Set things up for the parser.
 773 |     setup_Parser();
 774 |     
 775 |     // Set things up for the animation code.
 776 |     setup_Animation();
 777 | }
 778 | 
 779 | void loop() 
 780 | {
 781 |     // Update the parser.
 782 |     loop_Parser();
 783 |     
 784 |     // Update the animation.
 785 |     loop_Animation();
 786 | }
 787 | 
 788 | 
 789 | //===============================================================================
 790 | // Related to the parser
 791 | //===============================================================================
 792 | 
 793 | // Sets up the parser stuff. Called in setup(). Should not be called elsewhere.
 794 | void setup_Parser()
 795 | {
 796 |     // Wait for first command.
 797 |     currParserState = PARSER_WAITING;
 798 |     
 799 |     // Print this response to say we've booted and are ready.
 800 |     softwareSerial.println("<OK>");
 801 | }
 802 | 
 803 | 
 804 | // Loop() for the parser stuff. Called in loop(). Should not be called elsewhere.
 805 | void loop_Parser()
 806 | {
 807 |     //---------------------------------------------------------
 808 |     // PARSER
 809 |     //
 810 |     // If there is data, parse it and process it.
 811 |     //---------------------------------------------------------
 812 |     
 813 |     // Read from pin serial port and write it out on USB port.
 814 |     if (softwareSerial.available() > 0)
 815 |     {
 816 |         char c = softwareSerial.read();
 817 |     
 818 |         // If we're in WAITING state, look for the START_CHAR.
 819 |         if (currParserState == PARSER_WAITING)
 820 |         {
 821 |             // If it's the START_CHAR, move out of this state...
 822 |             if (c == START_CHAR)
 823 |             {
 824 |                 // Start parsing the command.
 825 |                 currParserState = PARSER_COMMAND;
 826 |         
 827 |                 // Reset thing ready for parsing
 828 |                 currCmdIndex = 0;
 829 |                 currCmd[0] = '-';
 830 |                 currCmd[1] = '-';
 831 |                 currParam1Val = 0;
 832 |                 currParam2Val = 0;
 833 |                 currParam3Val = 0;
 834 |                 currParam4Val = 0;
 835 |                 currParam5Val = 0;
 836 |             }
 837 |       
 838 |             // Otherwise, stay in this state.
 839 |         }
 840 |     
 841 |         // In the state to look for the command.
 842 |         else if (currParserState == PARSER_COMMAND)
 843 |         {
 844 |             // Else if it's a separator, parse parameter 1. But make sure it's not
 845 |             // empty, or else it's a parse error.
 846 |             if (c == SEP_CHAR)
 847 |             {
 848 |                 if (currCmdIndex == CMD_LENGTH)
 849 |                 {
 850 |                     currParserState = PARSER_PARAM1;
 851 |                     currParamIndex = 0;
 852 |                     currParamNegative = false;
 853 |                 }
 854 |                 else
 855 |                 {
 856 |                     currParserState = PARSER_WAITING;
 857 |                 }
 858 |             }
 859 |       
 860 |             // Else if it's the end char, there are no parameters, so we're ready to
 861 |             // process. But make sure it's not empty. Otherwise, it's a parse error.
 862 |             else if (c == END_CHAR)
 863 |             {
 864 |                 if (currCmdIndex == CMD_LENGTH)
 865 |                 {
 866 |                     currParserState = PARSER_EXECUTE;
 867 |                 }
 868 |                 else
 869 |                 {
 870 |                     currParserState = PARSER_WAITING;
 871 |                 }
 872 |             }
 873 |       
 874 |             // If we've got too many letters here, we have a parse error,
 875 |             // so abandon and go back to PARSER_WAITING
 876 |             else if ( (currCmdIndex >= CMD_LENGTH) || (c < 'A') || (c > 'Z') )
 877 |             {
 878 |                 currParserState = PARSER_WAITING;
 879 |             }
 880 |       
 881 |             // Store the current character.
 882 |             else
 883 |             {
 884 |                 currCmd[currCmdIndex] = c;
 885 |                 currCmdIndex++;
 886 |             }
 887 |         }
 888 |     
 889 |         // In the state to parse param 1.
 890 |         else if (currParserState == PARSER_PARAM1)
 891 |         {
 892 |             // Else if it's a separator, parse parameter 1.
 893 |             if (c == SEP_CHAR)
 894 |             {
 895 |                 if (currParamNegative)
 896 |                 {
 897 |                     currParam1Val = -1 * currParam1Val;
 898 |                 }
 899 | 
 900 |                 currParserState = PARSER_PARAM2;
 901 |                 currParamIndex = 0;
 902 |                 currParamNegative = false;
 903 |             }
 904 |       
 905 |             // Else if it's the end char, there are no parameters, so we're ready to
 906 |             // process.
 907 |             else if (c == END_CHAR)
 908 |             {
 909 |                 if (currParamNegative)
 910 |                 {
 911 |                     currParam1Val = -1 * currParam1Val;
 912 |                 }
 913 | 
 914 |                 currParserState = PARSER_EXECUTE;
 915 |             }
 916 |       
 917 |             // Check for negative at the start.
 918 |             else if ( (currParamIndex == 0) && (c == '-') )
 919 |             {
 920 |                 currParamNegative = true;
 921 |                 currParamIndex++;
 922 |             }
 923 |             
 924 |             // If it's too long, or the character is not a digit, then it's
 925 |             // a parse error, so abandon and go back to PARSER_WAITING.
 926 |             else if ( (currParamIndex >= MAX_PARAM_LENGTH) || (c < '0') || (c > '9') )
 927 |             {
 928 |                 currParserState = PARSER_WAITING;
 929 |             }
 930 | 
 931 |             // It's a valid character, so process it.
 932 |             else
 933 |             {
 934 |                 // Shift existing value across and add new digit at the bottom.
 935 |                 int currDigitVal = c - '0';
 936 |                 currParam1Val = (currParam1Val * 10) + currDigitVal;
 937 |                 currParamIndex++;
 938 |             }
 939 | 
 940 |         }
 941 |     
 942 |         // In the state to parse param 2.
 943 |         else if (currParserState == PARSER_PARAM2)
 944 |         {
 945 |             // Else if it's a separator, parse parameter 2.
 946 |             if (c == SEP_CHAR)
 947 |             {
 948 |                 if (currParamNegative)
 949 |                 {
 950 |                     currParam2Val = -1 * currParam2Val;
 951 |                 }
 952 | 
 953 |                 currParserState = PARSER_PARAM3;
 954 |                 currParamIndex = 0;
 955 |                 currParamNegative = false;
 956 |             }
 957 |       
 958 |             // Else if it's the end char, there are no parameters, so we're ready to
 959 |             // process.
 960 |             else if (c == END_CHAR)
 961 |             {
 962 |                 if (currParamNegative)
 963 |                 {
 964 |                     currParam2Val = -1 * currParam2Val;
 965 |                 }
 966 | 
 967 |                 currParserState = PARSER_EXECUTE;
 968 |             }
 969 |       
 970 |             // Check for negative at the start.
 971 |             else if ( (currParamIndex == 0) && (c == '-') )
 972 |             {
 973 |                 currParamNegative = true;
 974 |                 currParamIndex++;
 975 |             }
 976 |             
 977 |             // If it's too long, or the character is not a digit, then it's
 978 |             // a parse error, so abandon and go back to PARSER_WAITING.
 979 |             else if ( (currParamIndex >= MAX_PARAM_LENGTH) || (c < '0') || (c > '9') )
 980 |             {
 981 |                 currParserState = PARSER_WAITING;
 982 |             }
 983 | 
 984 |             // It's a valid character, so process it.
 985 |             else
 986 |             {
 987 |                 // Shift existing value across and add new digit at the bottom.
 988 |                 int currDigitVal = c - '0';
 989 |                 currParam2Val = (currParam2Val * 10) + currDigitVal;
 990 |                 currParamIndex++;
 991 |             }
 992 | 
 993 |         }
 994 |     
 995 |         // In the state to parse param 3.
 996 |         else if (currParserState == PARSER_PARAM3)
 997 |         {
 998 |             // Else if it's a separator, parse parameter 2.
 999 |             if (c == SEP_CHAR)
1000 |             {
1001 |                 if (currParamNegative)
1002 |                 {
1003 |                     currParam3Val = -1 * currParam3Val;
1004 |                 }
1005 | 
1006 |                 currParserState = PARSER_PARAM4;
1007 |                 currParamIndex = 0;
1008 |                 currParamNegative = false;
1009 |             }
1010 |       
1011 |             // Else if it's the end char, there are no parameters, so we're ready to
1012 |             // process.
1013 |             else if (c == END_CHAR)
1014 |             {
1015 |                 if (currParamNegative)
1016 |                 {
1017 |                     currParam3Val = -1 * currParam3Val;
1018 |                 }
1019 | 
1020 |                 currParserState = PARSER_EXECUTE;
1021 |             }
1022 |       
1023 |             // Check for negative at the start.
1024 |             else if ( (currParamIndex == 0) && (c == '-') )
1025 |             {
1026 |                 currParamNegative = true;
1027 |                 currParamIndex++;
1028 |             }
1029 |             
1030 |             // If it's too long, or the character is not a digit, then it's
1031 |             // a parse error, so abandon and go back to PARSER_WAITING.
1032 |             else if ( (currParamIndex >= MAX_PARAM_LENGTH) || (c < '0') || (c > '9') )
1033 |             {
1034 |                 currParserState = PARSER_WAITING;
1035 |             }
1036 | 
1037 |             // It's a valid character, so process it.
1038 |             else
1039 |             {
1040 |                 // Shift existing value across and add new digit at the bottom.
1041 |                 int currDigitVal = c - '0';
1042 |                 currParam3Val = (currParam3Val * 10) + currDigitVal;
1043 |                 currParamIndex++;
1044 |             }
1045 | 
1046 |         }
1047 |     
1048 |         // In the state to parse param 4.
1049 |         else if (currParserState == PARSER_PARAM4)
1050 |         {
1051 |             // Else if it's a separator, parse parameter 2.
1052 |             if (c == SEP_CHAR)
1053 |             {
1054 |                 if (currParamNegative)
1055 |                 {
1056 |                     currParam4Val = -1 * currParam4Val;
1057 |                 }
1058 | 
1059 |                 currParserState = PARSER_PARAM5;
1060 |                 currParamIndex = 0;
1061 |                 currParamNegative = false;
1062 |             }
1063 |       
1064 |             // Else if it's the end char, there are no parameters, so we're ready to
1065 |             // process.
1066 |             else if (c == END_CHAR)
1067 |             {
1068 |                 if (currParamNegative)
1069 |                 {
1070 |                     currParam4Val = -1 * currParam4Val;
1071 |                 }
1072 | 
1073 |                 currParserState = PARSER_EXECUTE;
1074 |             }
1075 |       
1076 |             // Check for negative at the start.
1077 |             else if ( (currParamIndex == 0) && (c == '-') )
1078 |             {
1079 |                 currParamNegative = true;
1080 |                 currParamIndex++;
1081 |             }
1082 |             
1083 |             // If it's too long, or the character is not a digit, then it's
1084 |             // a parse error, so abandon and go back to PARSER_WAITING.
1085 |             else if ( (currParamIndex >= MAX_PARAM_LENGTH) || (c < '0') || (c > '9') )
1086 |             {
1087 |                 currParserState = PARSER_WAITING;
1088 |             }
1089 | 
1090 |             // It's a valid character, so process it.
1091 |             else
1092 |             {
1093 |                 // Shift existing value across and add new digit at the bottom.
1094 |                 int currDigitVal = c - '0';
1095 |                 currParam4Val = (currParam4Val * 10) + currDigitVal;
1096 |                 currParamIndex++;
1097 |             }
1098 | 
1099 |         }
1100 |             // In the state to parse param 5.
1101 |         else if (currParserState == PARSER_PARAM5)
1102 |         {
1103 |             // If it's the end char, there are no parameters, so we're ready to
1104 |             // process.
1105 |             if (c == END_CHAR)
1106 |             {
1107 |                 if (currParamNegative)
1108 |                 {
1109 |                     currParam5Val = -1 * currParam5Val;
1110 |                 }
1111 |                 currParserState = PARSER_EXECUTE;
1112 |             }
1113 |       
1114 |             // Check for negative at the start.
1115 |             else if ( (currParamIndex == 0) && (c == '-') )
1116 |             {
1117 |                 currParamNegative = true;
1118 |                 currParamIndex++;
1119 |             }
1120 |             
1121 |             // If it's too long, or the character is not a digit, then it's
1122 |             // a parse error, so abandon and go back to PARSER_WAITING.
1123 |             else if ( (currParamIndex >= MAX_PARAM_LENGTH) || (c < '0') || (c > '9') )
1124 |             {
1125 |                 currParserState = PARSER_WAITING;
1126 |             }
1127 | 
1128 |             // It's a valid character, so process it.
1129 |             else
1130 |             {
1131 |                 // Shift existing value across and add new digit at the bottom.
1132 |                 int currDigitVal = c - '0';
1133 |                 currParam5Val = (currParam5Val * 10) + currDigitVal;
1134 |                 currParamIndex++;
1135 |             }
1136 | 
1137 |         }
1138 |     
1139 |         
1140 |         //---------------------------------------------------------
1141 |         // PARSER CODE HANDLER (Still part of Parser, but section that
1142 |         // processes completed commands)
1143 |         //
1144 |         // If the most recently read char completes a command,
1145 |         // then process the command, and clear the state to
1146 |         // go back to looking for a new command.
1147 |         //
1148 |         // The parsed items are stored in:
1149 |         //    currCmd, currParam1Val, currParam2Val, currParam3Val, 
1150 |         //             currParam4Val, currParam5Val
1151 |         //---------------------------------------------------------
1152 |     
1153 |         if (currParserState == PARSER_EXECUTE)
1154 |         {
1155 |             // Ready/OK Check: <OK>
1156 |             if ((currCmd[0] == 'O') && (currCmd[1] == 'K'))
1157 |             {
1158 |                 softwareSerial.println("<OK>");
1159 |             }
1160 |             
1161 |             // Set Servo: <SV, time, leftHip, leftFoot, rightHip, rightFoot>
1162 |             // time      - time to tween to specified angles
1163 |             // leftHip   - microsecs from centre. -ve is hip in, +ve is hip out 
1164 |             // leftFoot  - microsecs from flat. -ve is foot down, +ve is foot up
1165 |             // rightHip  - microsecs from centre. -ve is hip in, +ve is hip out 
1166 |             // rightFoot - microsecs from flat. -ve is foot down, +ve is foot up
1167 |             else if ((currCmd[0] == 'S') && (currCmd[1] == 'V'))
1168 |             {
1169 |                 int tweenTime = currParam1Val;
1170 |                 if (currParam1Val < 0)
1171 |                 {
1172 |                     tweenTime = 0;
1173 |                 }
1174 |                 SetServos(tweenTime, currParam2Val, currParam3Val, currParam4Val, currParam5Val, "SV");
1175 |             }
1176 |             
1177 |             // Stop/Reset: <ST>, Stops current anim. Also can be used to put robot into reset position.
1178 |             else if ((currCmd[0] == 'S') && (currCmd[1] == 'T'))
1179 |             {
1180 |                 StopAnim("ST");
1181 |             }
1182 |             
1183 |             // Stop Immediate: <SI>
1184 |             else if ((currCmd[0] == 'S') && (currCmd[1] == 'I'))
1185 |             {
1186 |                 StopAnimImmediate("SI");
1187 |             }
1188 |             
1189 |             // Forward: <FW, #Times>, -1 means continuous, 0 or no param is the same as 1 time.
1190 |             else if ((currCmd[0] == 'F') && (currCmd[1] == 'W'))
1191 |             {
1192 |                 int numTimes = currParam1Val;
1193 |                 if (currParam1Val < 0)
1194 |                 {
1195 |                     numTimes = -1;
1196 |                 }
1197 |                 
1198 |                 PlayAnimNumTimes(walkForwardAnim, walkEndAnim, numTimes, "FW");
1199 |             }
1200 |             
1201 |             // Backward: <BW, #Times>, -1 means continuous, 0 or no param is the same as 1 time.
1202 |             else if ((currCmd[0] == 'B') && (currCmd[1] == 'W'))
1203 |             {
1204 |                 int numTimes = currParam1Val;
1205 |                 if (currParam1Val < 0)
1206 |                 {
1207 |                     numTimes = -1;
1208 |                 }
1209 |                 
1210 |                 PlayAnimNumTimes(walkBackwardAnim, walkEndAnim, numTimes, "BW");
1211 |             }
1212 |             
1213 |             // Turn Left: <LT, #Times>, -1 means continuous, 0 or no param is the same as 1 time.
1214 |             else if ((currCmd[0] == 'L') && (currCmd[1] == 'T'))
1215 |             {
1216 |                 int numTimes = currParam1Val;
1217 |                 if (currParam1Val < 0)
1218 |                 {
1219 |                     numTimes = -1;
1220 |                 }
1221 |                 
1222 |                 PlayAnimNumTimes(turnLeftAnim, NULL, numTimes, "LT");
1223 |             }
1224 |             
1225 |             // Turn Right: <RT, #Times>, -1 means continuous, 0 or no param is the same as 1 time.
1226 |             else if ((currCmd[0] == 'R') && (currCmd[1] == 'T'))
1227 |             {
1228 |                 int numTimes = currParam1Val;
1229 |                 if (currParam1Val < 0)
1230 |                 {
1231 |                     numTimes = -1;
1232 |                 }
1233 |                 
1234 |                 PlayAnimNumTimes(turnRightAnim, NULL, numTimes, "RT");
1235 |             }
1236 |             
1237 |             // Shake Head: <SX, #Times>, -1 means continuous, 0 or no param is the same as 1 time.
1238 |             else if ((currCmd[0] == 'S') && (currCmd[1] == 'X'))
1239 |             {
1240 |                 int numTimes = currParam1Val;
1241 |                 if (currParam1Val < 0)
1242 |                 {
1243 |                     numTimes = -1;
1244 |                 }
1245 |                 
1246 |                 PlayAnimNumTimes(shakeHeadAnim, NULL, numTimes, "SX");
1247 |             }
1248 |             
1249 |             // Bounce: <BX, #Times>, -1 means continuous, 0 or no param is the same as 1 time.
1250 |             else if ((currCmd[0] == 'B') && (currCmd[1] == 'X'))
1251 |             {
1252 |                 int numTimes = currParam1Val;
1253 |                 if (currParam1Val < 0)
1254 |                 {
1255 |                     numTimes = -1;
1256 |                 }
1257 |                 
1258 |                 PlayAnimNumTimes(bounceAnim, NULL, numTimes, "BX");
1259 |             }
1260 |             
1261 |             // Wobble: <WX, #Times>, -1 means continuous, 0 or no param is the same as 1 time.
1262 |             else if ((currCmd[0] == 'W') && (currCmd[1] == 'X'))
1263 |             {
1264 |                 int numTimes = currParam1Val;
1265 |                 if (currParam1Val < 0)
1266 |                 {
1267 |                     numTimes = -1;
1268 |                 }
1269 |                 
1270 |                 PlayAnimNumTimes(wobbleAnim, NULL, numTimes, "WX");
1271 |             }
1272 |             
1273 |             // Wobble Left: <WY, #Times>, -1 means continuous, 0 or no param is the same as 1 time.
1274 |             else if ((currCmd[0] == 'W') && (currCmd[1] == 'Y'))
1275 |             {
1276 |                 int numTimes = currParam1Val;
1277 |                 if (currParam1Val < 0)
1278 |                 {
1279 |                     numTimes = -1;
1280 |                 }
1281 |                 
1282 |                 PlayAnimNumTimes(wobbleLeftAnim, NULL, numTimes, "WY");
1283 |             }
1284 |             
1285 |             // Wobble Right: <WZ, #Times>, -1 means continuous, 0 or no param is the same as 1 time.
1286 |             else if ((currCmd[0] == 'W') && (currCmd[1] == 'Z'))
1287 |             {
1288 |                 int numTimes = currParam1Val;
1289 |                 if (currParam1Val < 0)
1290 |                 {
1291 |                     numTimes = -1;
1292 |                 }
1293 |                 
1294 |                 PlayAnimNumTimes(wobbleRightAnim, NULL, numTimes, "WZ");
1295 |             }
1296 |             
1297 |             // Tap Feet: <TX, #Times>, -1 means continuous, 0 or no param is the same as 1 time.
1298 |             else if ((currCmd[0] == 'T') && (currCmd[1] == 'X'))
1299 |             {
1300 |                 int numTimes = currParam1Val;
1301 |                 if (currParam1Val < 0)
1302 |                 {
1303 |                     numTimes = -1;
1304 |                 }
1305 |                 
1306 |                 PlayAnimNumTimes(tapFeetAnim, NULL, numTimes, "TX");
1307 |             }
1308 |             
1309 |             // Tap Left Foot: <TY, #Times>, -1 means continuous, 0 or no param is the same as 1 time.
1310 |             else if ((currCmd[0] == 'T') && (currCmd[1] == 'Y'))
1311 |             {
1312 |                 int numTimes = currParam1Val;
1313 |                 if (currParam1Val < 0)
1314 |                 {
1315 |                     numTimes = -1;
1316 |                 }
1317 |                 
1318 |                 PlayAnimNumTimes(tapLeftFootAnim, NULL, numTimes, "TY");
1319 |             }
1320 |             
1321 |             // Tap Right Foot: <TZ, #Times>, -1 means continuous, 0 or no param is the same as 1 time.
1322 |             else if ((currCmd[0] == 'T') && (currCmd[1] == 'Z'))
1323 |             {
1324 |                 int numTimes = currParam1Val;
1325 |                 if (currParam1Val < 0)
1326 |                 {
1327 |                     numTimes = -1;
1328 |                 }
1329 |                 
1330 |                 PlayAnimNumTimes(tapRightFootAnim, NULL, numTimes, "TZ");
1331 |             }
1332 |             
1333 |             // Shake Legs: <LX, #Times>, -1 means continuous, 0 or no param is the same as 1 time.
1334 |             else if ((currCmd[0] == 'L') && (currCmd[1] == 'X'))
1335 |             {
1336 |                 int numTimes = currParam1Val;
1337 |                 if (currParam1Val < 0)
1338 |                 {
1339 |                     numTimes = -1;
1340 |                 }
1341 |                 
1342 |                 PlayAnimNumTimes(shakeLegsAnim, NULL, numTimes, "LX");
1343 |             }
1344 |             
1345 |             // Shake Left Leg: <LY, #Times>, -1 means continuous, 0 or no param is the same as 1 time.
1346 |             else if ((currCmd[0] == 'L') && (currCmd[1] == 'Y'))
1347 |             {
1348 |                 int numTimes = currParam1Val;
1349 |                 if (currParam1Val < 0)
1350 |                 {
1351 |                     numTimes = -1;
1352 |                 }
1353 |                 
1354 |                 PlayAnimNumTimes(shakeLeftLegAnim, NULL, numTimes, "LY");
1355 |             }
1356 |             
1357 |             // Shake Right Leg: <LZ, #Times>, -1 means continuous, 0 or no param is the same as 1 time.
1358 |             else if ((currCmd[0] == 'L') && (currCmd[1] == 'Z'))
1359 |             {
1360 |                 int numTimes = currParam1Val;
1361 |                 if (currParam1Val < 0)
1362 |                 {
1363 |                     numTimes = -1;
1364 |                 }
1365 |                 
1366 |                 PlayAnimNumTimes(shakeRightLegAnim, NULL, numTimes, "LZ");
1367 |             }
1368 |             
1369 |             //--------------------------------------------------
1370 |             // Clear the state and wait for the next command!
1371 |             // This must be done!
1372 |             //--------------------------------------------------
1373 |             currParserState = PARSER_WAITING;
1374 |         }
1375 |     }
1376 | }
1377 | 
1378 | 
1379 | //===============================================================================
1380 | // Related to playing servo animations.
1381 | //===============================================================================
1382 | 
1383 | // Call this to play the given animation once. Pass in NULL if there is no finishAnim.
1384 | void PlayAnim(int animToPlay[][5], int finishAnim[][5], const char *completeStr)
1385 | {
1386 |     // Put this in the queue.
1387 |     PlayAnimNumTimes(animToPlay, finishAnim, 1, completeStr);
1388 | }
1389 | 
1390 | // Call this to loop the given animation. Pass in NULL if there is no finishAnim.
1391 | void LoopAnim(int animToPlay[][5], int finishAnim[][5], const char *completeStr)
1392 | {
1393 |     // Put this in the queue.
1394 |     PlayAnimNumTimes(animToPlay, finishAnim, -1, completeStr);
1395 | }
1396 | 
1397 | // Call this to play the given animation the specified number of times. 
1398 | // -1 number of times will make it loop forever.
1399 | // Pass in NULL if there is no finishAnim.
1400 | void PlayAnimNumTimes(int animToPlay[][5], int finishAnim[][5], int numTimes, const char *completeStr)
1401 | {
1402 |     // Put this in the queue.
1403 |     nextAnim         = animToPlay;
1404 |     nextFinishAnim   = finishAnim;
1405 |     nextAnimNumLoops = numTimes;
1406 | 
1407 |     // Save the completeStr
1408 |     if (completeStr == NULL)
1409 |     {
1410 |         nextAnimCompleteStr[0] = '-';
1411 |         nextAnimCompleteStr[1] = '-';
1412 |     }
1413 |     else
1414 |     {
1415 |         nextAnimCompleteStr[0] = completeStr[0];
1416 |         nextAnimCompleteStr[1] = completeStr[1];
1417 |     }
1418 | }
1419 | 
1420 | // Stop after the current animation.
1421 | void StopAnim(const char *completeStr)
1422 | {
1423 |     // Put this in the queue.
1424 |     PlayAnimNumTimes(standStraightAnim, NULL, 1, completeStr);
1425 | }
1426 | 
1427 | // Stop immediately and lerp robot to zero position, interrupting 
1428 | // any animation that is in progress.
1429 | void StopAnimImmediate(const char *completeStr)
1430 | {
1431 |     // Put this in the queue.
1432 |     interruptInProgressAnim = true;
1433 |     PlayAnimNumTimes(standStraightAnim, NULL, 1, completeStr);
1434 | }
1435 | 
1436 | // Moves servos to the specified positions. Time 0 will make it immediate. Otherwise,
1437 | // it'll tween it over a specified time.
1438 | // For positions, 0 means centered.
1439 | // For hips, -ve is hip left, +ve is hip right
1440 | // For feet, -ve is foot down, +ve is foot up
1441 | void SetServos(int tweenTime, int leftHip, int leftFoot, int rightHip, int rightFoot, const char* completeStr)
1442 | {
1443 |     // Save the completeStr
1444 |     if (completeStr == NULL)
1445 |     {
1446 |         nextAnimCompleteStr[0] = '-';
1447 |         nextAnimCompleteStr[1] = '-';
1448 |     }
1449 |     else
1450 |     {
1451 |         nextAnimCompleteStr[0] = completeStr[0];
1452 |         nextAnimCompleteStr[1] = completeStr[1];
1453 |     }
1454 |     
1455 |     // Decide which tween data we use. We don't want to over-write the one that is
1456 |     // in progress. We have and reuse these to keep memory allocation fixed.
1457 |     int (*tweenServoData)[5];
1458 |     if (currAnim != setServosAnim1)
1459 |     {
1460 |         tweenServoData = setServosAnim1;
1461 |     }
1462 |     else
1463 |     {
1464 |         tweenServoData = setServosAnim2;
1465 |     }
1466 |     
1467 |     // Set the tween information into the animation data.
1468 |     tweenServoData[1][TWEEN_TIME_VALUE] = tweenTime;
1469 |     tweenServoData[1][LEFT_HIP_VALUE]   = LeftHipIn(leftHip);
1470 |     tweenServoData[1][LEFT_FOOT_VALUE]  = LeftFootUp(leftFoot);
1471 |     tweenServoData[1][RIGHT_HIP_VALUE]  = RightHipIn(rightHip);
1472 |     tweenServoData[1][RIGHT_FOOT_VALUE] = RightFootUp(rightFoot);
1473 |     
1474 |     // Queue this tween to be played next.
1475 |     PlayAnim(tweenServoData, NULL, completeStr);
1476 | }
1477 | 
1478 | 
1479 | // Set up variables for animation. This is called in setup(). Should be not called by anywhere else.
1480 | void setup_Animation()
1481 | {
1482 |     // Set the servos to the feet flat, feet even position.
1483 |     currLeftHip   = LEFT_HIP_CENTRE;
1484 |     currLeftFoot  = LEFT_FOOT_CENTRE;
1485 |     currRightHip  = RIGHT_HIP_CENTRE;
1486 |     currRightFoot = RIGHT_FOOT_CENTRE;
1487 |     UpdateServos();
1488 |     
1489 |     // Set the "start" positions to the current ones. So, when
1490 |     // we pay the next anim, we will tween from the current positions.
1491 |     startLeftHip   = currLeftHip;
1492 |     startLeftFoot  = currLeftFoot;
1493 |     startRightHip  = currRightHip;
1494 |     startRightFoot = currRightFoot;
1495 |     
1496 |     // No animation is playing yet, and nothing in the queue yet.
1497 |     timeAtLastAnimUpdate    = millis();
1498 |     animInProgress          = false;
1499 |     interruptInProgressAnim = false;
1500 |     currAnim       = NULL;
1501 |     finishAnim     = NULL;
1502 |     nextAnim       = NULL;
1503 |     nextFinishAnim = NULL;
1504 | }
1505 | 
1506 | // Loop function for processing animation. This is called in every loop(). Should be be called by anywhere else.
1507 | //
1508 | // NOTE: The way looping animations work is that they basically add themselves back to the queue
1509 | //       when a cycle is done, and if there's nothing already queued up! This way, looping animations
1510 | //       work in a similar way to single-play animations, and fits into the queueing system.
1511 | void loop_Animation()
1512 | {
1513 |     // Get the time at the start of this frame.
1514 |     long currTime = millis();
1515 | 
1516 |     //--------------------------------------------------------------------------------------
1517 |     // Decide if we want to perform the animation update. We don't execute this every frame.
1518 |     //--------------------------------------------------------------------------------------
1519 |     
1520 |     if (timeAtLastAnimUpdate + millisBetweenAnimUpdate > currTime)
1521 |     {
1522 |         // Not yet time to do an anim update, so jump out.
1523 |         return;
1524 |     }
1525 |     else
1526 |     {
1527 |         // We reset the timer, and then proceed below to handle the current anim update.
1528 |         timeAtLastAnimUpdate = currTime;
1529 |     }
1530 |     
1531 |     //--------------------------------------------------------------------------------------
1532 |     // Decide if we need to setup and start a new animation. We do if there's no anim 
1533 |     // playing or we've been asked to interrupt the anim.
1534 |     //--------------------------------------------------------------------------------------
1535 |     
1536 |     if ( (nextAnim != NULL) &&  (!animInProgress || interruptInProgressAnim) )
1537 |     {
1538 |         // If this was an interrupt, we also set the "start" servo positions
1539 |         // to the current ones. This way, the animation system will tween from the
1540 |         // current positions.
1541 |         if (interruptInProgressAnim)
1542 |         {
1543 |             // This is the place to notify someone of an animation finishing after getting interrupted
1544 |             // Print the command string we just finished. -1 parameter indicates it was interrupted.
1545 |             softwareSerial.print("<");
1546 |             softwareSerial.print(animCompleteStr);
1547 |             softwareSerial.println(",-1>");
1548 |             
1549 |             // Set the "start" positions to the current ones. So, when
1550 |             // we pay the next anim, we will tween from the current positions.
1551 |             startLeftHip   = currLeftHip;
1552 |             startLeftFoot  = currLeftFoot;
1553 |             startRightHip  = currRightHip;
1554 |             startRightFoot = currRightFoot;
1555 |             
1556 |             // We've handled any interrupt request, so clear the flag.
1557 |             interruptInProgressAnim = false;
1558 |         }
1559 |         
1560 |         // Store the animation we are now playing.
1561 |         currAnim           = nextAnim;
1562 |         finishAnim         = nextFinishAnim;
1563 |         animCompleteStr[0] = nextAnimCompleteStr[0];
1564 |         animCompleteStr[1] = nextAnimCompleteStr[1];
1565 | 
1566 |         nextAnim               = NULL; // Queue is cleared.
1567 |         nextFinishAnim         = NULL;
1568 |         nextAnimCompleteStr[0] = '-';
1569 |         nextAnimCompleteStr[1] = '-';
1570 |         
1571 |         // Record the number of times to play the animation.
1572 |         animNumLoops = nextAnimNumLoops;
1573 |         
1574 |         // Treat current time as start of frame for the initial lerp to the first frame.
1575 |         timeAtStartOfFrame = currTime;
1576 |         
1577 |         // Set the frame counters.
1578 |         targetFrame = 1; // First frame we are lerping to. Index 0 is metadata, so skip.
1579 |         
1580 |         // An animation is now in progress
1581 |         animInProgress = true;
1582 |     }
1583 | 
1584 |     //--------------------------------------------------------------------------------------
1585 |     // If we are currently playing an animation, then update the animation state and the
1586 |     // servo positions.
1587 |     //--------------------------------------------------------------------------------------
1588 |     
1589 |     if (animInProgress)
1590 |     {
1591 |         // Determine if we need to switch to the next frame.
1592 |         int timeInCurrFrame = currTime - timeAtStartOfFrame;
1593 |         if (timeInCurrFrame > currAnim[targetFrame][TWEEN_TIME_VALUE])
1594 |         {
1595 |             // Set the servo positions to the targetFrame's values.
1596 |             // We only set this if the value is > 0. -ve values means that
1597 |             // the current target keyframe did not alter that servos position.
1598 |             if (currAnim[targetFrame][LEFT_HIP_VALUE] >= 0)
1599 |             {
1600 |                 currLeftHip = currAnim[targetFrame][LEFT_HIP_VALUE];
1601 |             }
1602 |             if (currAnim[targetFrame][LEFT_FOOT_VALUE] >= 0)
1603 |             {
1604 |                 currLeftFoot = currAnim[targetFrame][LEFT_FOOT_VALUE];
1605 |             }
1606 |             if (currAnim[targetFrame][RIGHT_HIP_VALUE] >= 0)
1607 |             {
1608 |                 currRightHip = currAnim[targetFrame][RIGHT_HIP_VALUE];
1609 |             }
1610 |             if (currAnim[targetFrame][RIGHT_FOOT_VALUE] >= 0)
1611 |             {
1612 |                 currRightFoot = currAnim[targetFrame][RIGHT_FOOT_VALUE];
1613 |             }
1614 |             UpdateServos();
1615 |             
1616 |             // These current values are now the start of frame values.
1617 |             startLeftHip   = currLeftHip;
1618 |             startLeftFoot  = currLeftFoot;
1619 |             startRightHip  = currRightHip;
1620 |             startRightFoot = currRightFoot;
1621 |             
1622 |             // Now, we try to move to the next frame.
1623 |             // - If there is a next frame, set that as the new target, and proceed.
1624 |             // - If there's no next frame, but it's looping, we re-add this animation
1625 |             //   to the queue.
1626 |             // - If there's no next frame, and this is not looping, we stop animating.
1627 |             // (Remember that targetFrame is 1-based since the first element of the animation
1628 |             // data array is metadata)
1629 |             
1630 |             // Increment targetFrame, and reset time in the current frame.
1631 |             targetFrame++;
1632 |             timeAtStartOfFrame = currTime;
1633 |             
1634 |             // If there is no next frame, we stop this current animation.
1635 |             // If it is looping, then we re-queue the current animation if the queue is empty.
1636 |             if (targetFrame > NumOfFrames(currAnim))
1637 |             {
1638 |                 // Stop the current animation.
1639 |                 animInProgress = false;
1640 |                 
1641 |                 // If we're looping forever, and there's no next anim, re-queue the 
1642 |                 // animation if the queue is empty.
1643 |                 if ((animNumLoops < 0) && (nextAnim == NULL))
1644 |                 {
1645 |                     LoopAnim(currAnim, finishAnim, animCompleteStr);
1646 |                 }
1647 |                 
1648 |                 // If we're looping forever, and there is something in the queue, then
1649 |                 // finish the animation and proceed.
1650 |                 else if ((animNumLoops < 0) && (nextAnim != NULL))
1651 |                 {
1652 |                     if (finishAnim != NULL)
1653 |                     {
1654 |                         // Switch to the finish anim.
1655 |                         currAnim       = finishAnim;
1656 |                         finishAnim     = NULL;
1657 |                         
1658 |                         // Record the number of times to play the animation.
1659 |                         animNumLoops = 1;
1660 |                         
1661 |                         // Treat current time as start of frame for the initial lerp to the first frame.
1662 |                         timeAtStartOfFrame = currTime;
1663 |                         
1664 |                         // Set the frame counters.
1665 |                         targetFrame = 1; // First frame we are lerping to. Index 0 is metadata, so skip.
1666 |                         
1667 |                         // An animation is now in progress
1668 |                         animInProgress = true;
1669 |                     }
1670 |                     else
1671 |                     {
1672 |                         // We've stopped, so can notify if needed.
1673 |                         // Print the command string we just finished.
1674 |                         softwareSerial.print("<");
1675 |                         softwareSerial.print(animCompleteStr);
1676 |                         softwareSerial.println(">");
1677 |                     }
1678 |                 }
1679 |                 
1680 |                 // If we're looping a limited number of times, and there's no next anim,
1681 |                 // re-queue the animation if the queue is empty.
1682 |                 else if ((animNumLoops > 1) && (nextAnim == NULL))
1683 |                 {
1684 |                     PlayAnimNumTimes(currAnim, finishAnim, animNumLoops-1, animCompleteStr);
1685 |                 }
1686 |                 
1687 |                 // In this case, numAnimLoops is 1, this is the last loop through, so
1688 |                 // we're done. We play the finishAnim first if needed.
1689 |                 else
1690 |                 {
1691 |                     // If there is a finish animation, switch to that animation.
1692 |                     if (finishAnim != NULL)
1693 |                     {
1694 |                         // Switch to the finish anim.
1695 |                         currAnim       = finishAnim;
1696 |                         finishAnim     = NULL;
1697 |                         
1698 |                         // Record the number of times to play the animation.
1699 |                         animNumLoops = 1;
1700 |                         
1701 |                         // Treat current time as start of frame for the initial lerp to the first frame.
1702 |                         timeAtStartOfFrame = currTime;
1703 |                         
1704 |                         // Set the frame counters.
1705 |                         targetFrame = 1; // First frame we are lerping to. Index 0 is metadata, so skip.
1706 |                         
1707 |                         // An animation is now in progress
1708 |                         animInProgress = true;
1709 |                     }
1710 |                     
1711 |                     // Otherwise, we're done! We've played the finishAnim if there was one.
1712 |                     else
1713 |                     {
1714 |                         // Print the command string we just finished.
1715 |                         softwareSerial.print("<");
1716 |                         softwareSerial.print(animCompleteStr);
1717 |                         softwareSerial.println(">");
1718 |                     }
1719 |                 }
1720 |             }
1721 |         }
1722 |         
1723 |         // If we're still animating (i.e. the previous check didn't find that
1724 |         // we've finished the current animation), then proceed.
1725 |         if (animInProgress)
1726 |         {
1727 |             // Set the servos per data in the current frame. We only update the servos that have target
1728 |             // microsecond values > 0. This is to support the feature where we leave a servo at its
1729 |             // existing position if an animation data item is -1.
1730 |             float frameTimeFraction = (currTime - timeAtStartOfFrame) / ((float) currAnim[targetFrame][TWEEN_TIME_VALUE]);
1731 |             
1732 |             if (currAnim[targetFrame][LEFT_HIP_VALUE] >= 0)
1733 |             {
1734 |                 currLeftHip = startLeftHip + ((currAnim[targetFrame][LEFT_HIP_VALUE] - startLeftHip) * frameTimeFraction);
1735 |             }
1736 |             
1737 |             if (currAnim[targetFrame][LEFT_FOOT_VALUE] >= 0)
1738 |             {
1739 |                 currLeftFoot = startLeftFoot + ((currAnim[targetFrame][LEFT_FOOT_VALUE] - startLeftFoot)  * frameTimeFraction);
1740 |             }
1741 |             
1742 |             if (currAnim[targetFrame][RIGHT_HIP_VALUE] >= 0)
1743 |             {
1744 |                 currRightHip = startRightHip  + ((currAnim[targetFrame][RIGHT_HIP_VALUE] - startRightHip) * frameTimeFraction);
1745 |             }
1746 |             
1747 |             if (currAnim[targetFrame][RIGHT_FOOT_VALUE] >= 0)
1748 |             {
1749 |                 currRightFoot = startRightFoot + ((currAnim[targetFrame][RIGHT_FOOT_VALUE] - startRightFoot) * frameTimeFraction);
1750 |             }
1751 |             
1752 |             UpdateServos();
1753 |         }
1754 |     }
1755 | }
1756 | 
1757 | 
1758 | // Move all the servo to the positions set in the curr... variables.
1759 | // In the code, we update those variables and then call this to set the servos.
1760 | void UpdateServos()
1761 | {
1762 |     servoLeftHip.writeMicroseconds(currLeftHip);
1763 |     servoLeftFoot.writeMicroseconds(currLeftFoot);
1764 |     servoRightHip.writeMicroseconds(currRightHip);
1765 |     servoRightFoot.writeMicroseconds(currRightFoot);
1766 | }
1767 | 
1768 | 
1769 | // Return the number of frames in the given animation data.
1770 | // Have this helper function to avoid the "magic number" reference of animData[0][0].
1771 | int NumOfFrames(int animData[][5])
1772 | {
1773 |     return animData[0][0];
1774 | }
1775 | 
1776 | 
1777 | 
1778 | 
1779 | 


--------------------------------------------------------------------------------
/ArduinoCode/MobBob-Control-Bluno/MobBob-Control-Bluno.ino:
--------------------------------------------------------------------------------
   1 | /*
   2 |  * ======================================
   3 |  *   MobBob Control Program
   4 |  *   by Kevin Chan (aka Cevinius)
   5 |  * ======================================
   6 |  *
   7 |  * This program enables MobBob to be controlled using serial commands. On most boards, this means you can
   8 |  * interactively send commands to MobBob using the Arduino Serial Monitor. On the Bluno (which is what I'm
   9 |  * using), the commands can be sent over Bluetooth LE.
  10 |  *
  11 |  * This program is long and contains 2 main components - a smooth servo animation program and a serial
  12 |  * command parser program.
  13 |  *
  14 |  * Animation System
  15 |  * ================
  16 |  * The animation program is designed to animate servo keyframe arrays smoothly. The code tries to do its
  17 |  * best to be easy to use.
  18 |  *
  19 |  * The animation system will only queue 1 command. i.e. One command can be running,
  20 |  * and one command can be queued up. If you send more commands, they will over-write the queued command.
  21 |  *
  22 |  * The animation system will by default wait to finish the current animation before starting the next. This
  23 |  * means that if the animation data ends with the robot in its base pose, things will join smoothly. To
  24 |  * support this, the animation system also has a feature where an animation can have a "finish sequence"
  25 |  * to put the robot back into the base pose. This feature is used for the walk forward/backward animations.
  26 |  * Those animations have a final sequence which puts the robot back into the base pose.
  27 |  *
  28 |  * When an animation is finished playing, the animation system will output a response string to the Serial port.
  29 |  * This enables the callers to know when the animations they've requested have finished playing. This is useful
  30 |  * for users to sequence animations - waiting for one to finish before starting another.
  31 |  *
  32 |  * The animation code has many variables to enable things to be tweaked. E.g. Update frequency, arduino pins, etc.
  33 |  *
  34 |  * The animation data array format is also designed to be easy to edit by hand.
  35 |  *
  36 |  * Command Parser
  37 |  * ==============
  38 |  * This system parses commands received over serial, and processes them. The commands include one for directly
  39 |  * setting servo positions, as well as commands for triggering pre-defined animations and walks.
  40 |  *
  41 |  * So, users who don't want to worry about the details of walking can just use the pre-defined walks/animations.
  42 |  * And, users who want complete control over the servos (to create new animations on the fly) can do that too.
  43 |  *
  44 |  * As mentioned above, these commands can be used interactively from the Arduino Serial Monitor. They can also be
  45 |  * sent in using Bluetooth LE (when a Bluno is used). The phone app will send the commands over Bluetooth LE to the
  46 |  * Bluno.
  47 |  *
  48 |  * General Commands:
  49 |  * -----------------
  50 |  *   Ready/OK Check: <OK>
  51 |  *     Status check. The response is returned immediately to check if the controller is working.
  52 |  *
  53 |  *   Set Servo: <SV, time, leftHip, leftFoot, rightHip, rightFoot>
  54 |  *                time      - time to tween to specified angles, 0 will immediately jump to angles
  55 |  *                leftHip   - microsecs from centre. -ve is hip in, +ve is hip out 
  56 |  *                leftFoot  - microsecs from flat. -ve is foot down, +ve is foot up
  57 |  *                rightHip  - microsecs from centre. -ve is hip in, +ve is hip out 
  58 |  *                rightFoot - microsecs from flat. -ve is foot down, +ve is foot up
  59 |  *     This command is used to get full control over the servos. You can tween the robot from its
  60 |  *     current pose to the specified pose over the duration specified.
  61 |  *
  62 |  *   Stop/Reset: <ST>
  63 |  *     Stops the robot after the current animation. Can be used to stop animations set to loop
  64 |  *     indefinitely. This can also be used to put the robot into its base pose (standing straight)
  65 |  *
  66 |  *   Stop Immediate: <SI>
  67 |  *     Stops the robot immediately without waiting to complete the current animation. This
  68 |  *     interrupts the robots current animation. Potentially the robot can be mid-animation
  69 |  *     and in an unstable pose, so be careful when using this.
  70 |  *
  71 |  * Standard Walk Commands:
  72 |  * -----------------------
  73 |  *   Forward:    <FW, #Times>, -1 means continuous, 0 or no param is the same as 1 time.
  74 |  *   Backward:   <BW, #Times>, -1 means continuous, 0 or no param is the same as 1 time.
  75 |  *   Turn Left:  <LT, #Times>, -1 means continuous, 0 or no param is the same as 1 time.
  76 |  *   Turn Right: <RT, #Times>, -1 means continuous, 0 or no param is the same as 1 time.
  77 |  *
  78 |  * Fun Animation Commands:
  79 |  * -----------------------
  80 |  *   Shake Head:      <SX, #Times>, -1 means continuous, 0 or no param is the same as 1 time.
  81 |  *
  82 |  *   Bounce:          <BX, #Times>, -1 means continuous, 0 or no param is the same as 1 time.
  83 |  *
  84 |  *   Wobble:          <WX, #Times>, -1 means continuous, 0 or no param is the same as 1 time.
  85 |  *   Wobble Left:     <WY, #Times>, -1 means continuous, 0 or no param is the same as 1 time.
  86 |  *   Wobble Right:    <WZ, #Times>, -1 means continuous, 0 or no param is the same as 1 time.
  87 |  *
  88 |  *   Tap Feet:        <TX, #Times>, -1 means continuous, 0 or no param is the same as 1 time.
  89 |  *   Tap Left Foot:   <TY, #Times>, -1 means continuous, 0 or no param is the same as 1 time.
  90 |  *   Tap Right Foot:  <TZ, #Times>, -1 means continuous, 0 or no param is the same as 1 time.
  91 |  *
  92 |  *   Shake Legs:      <LX, #Times>, -1 means continuous, 0 or no param is the same as 1 time.
  93 |  *   Shake Left Leg:  <LY, #Times>, -1 means continuous, 0 or no param is the same as 1 time.
  94 |  *   Shake Right Leg: <LZ, #Times>, -1 means continuous, 0 or no param is the same as 1 time.
  95 |  *
  96 |  * Also, the code will send a response string back over Serial when commands have finished 
  97 |  * executing. 
  98 |  *
  99 |  * If the command finished normally, the response string is the command code without
 100 |  * parameters. E.g. When it has finished moving forward, it will send the response "<FW>".
 101 |  *
 102 |  * If a command was interrupted with <SI>, the current animation may have been stopped midway.
 103 |  * In this case, the robot could be in a weird mid-way pose, and finishAnims may not have been
 104 |  * played. To let the user know this has happened, the response string will have the 
 105 |  * parameter -1. E.g If a walk was stopped midway using <SI>, the response string would be
 106 |  * <FW,-1> to indicate that the walk has stopped, but it was stopped midway.
 107 |  * (Note: If you use <ST> to stop, that will wait for the current animation cycle to complete
 108 |  * before stopping. So, animations won't get stopped midway in that case.)
 109 |  * 
 110 |  * Because the responses are sent after an animation is complete, the command sender can
 111 |  * look for the response strings to determine when the robot is ready for a new command.
 112 |  * E.g. If you use the command <FW,3>, the response string isn't sent until all 3 steps
 113 |  * (and finish anim) are completed. So, the command sender can wait for the response
 114 |  * string before telling the robot to do the next thing.
 115 |  */
 116 |  
 117 | #include <Servo.h>
 118 | 
 119 | //----------------------------------------------------------------------------------
 120 | // Speed of serial communication - Set this for your serial (bluetooth) card.
 121 | //----------------------------------------------------------------------------------
 122 | 
 123 | // Serial communication speed with the Bluno. Default is 115200.
 124 | #define SERIAL_SPEED 115200
 125 | 
 126 | 
 127 | //----------------------------------------------------------------------------------
 128 | // Setup Arduino Pins - Set these for your particular robot.
 129 | //----------------------------------------------------------------------------------
 130 | 
 131 | const int SERVO_LEFT_HIP   = 5;
 132 | const int SERVO_LEFT_FOOT  = 4;
 133 | const int SERVO_RIGHT_HIP  = 3;
 134 | const int SERVO_RIGHT_FOOT = 2;
 135 | 
 136 | 
 137 | // I want this code to be usable on all 4-servo bipeds! (Like Bob, MobBob)
 138 | // I noticed that some builds mount the hip servos facing a different
 139 | // way to how I did MobBob's so, this setting lets you configure the code
 140 | // for either build style.
 141 | // 1  for MobBob style front facing hips (joint towards the front)
 142 | // -1 for Bob style back facing hips (joint towards the back)
 143 | #define FRONT_JOINT_HIPS 1
 144 | 
 145 | 
 146 | //----------------------------------------------------------------------------------
 147 | // Servo Max/Min/Centre Constants - Set these for your particular robot.
 148 | //----------------------------------------------------------------------------------
 149 | 
 150 | const int LEFT_HIP_CENTRE = 1430;
 151 | const int LEFT_HIP_MIN    = LEFT_HIP_CENTRE - 500;
 152 | const int LEFT_HIP_MAX    = LEFT_HIP_CENTRE + 500;
 153 | 
 154 | const int LEFT_FOOT_CENTRE = 1420;
 155 | const int LEFT_FOOT_MIN    = LEFT_FOOT_CENTRE - 550;
 156 | const int LEFT_FOOT_MAX    = LEFT_FOOT_CENTRE + 500;
 157 | 
 158 | const int RIGHT_HIP_CENTRE = 1550;
 159 | const int RIGHT_HIP_MIN    = RIGHT_HIP_CENTRE - 500;
 160 | const int RIGHT_HIP_MAX    = RIGHT_HIP_CENTRE + 500;
 161 | 
 162 | const int RIGHT_FOOT_CENTRE = 1400;
 163 | const int RIGHT_FOOT_MIN    = RIGHT_FOOT_CENTRE - 550;
 164 | const int RIGHT_FOOT_MAX    = RIGHT_FOOT_CENTRE + 500;
 165 | 
 166 | 
 167 | //------------------------------------------------------------------------------
 168 | // Helper functions to help calculate joint values in a more user-friendly way.
 169 | // You can adjust the signs here if the servos are setup in a different way.
 170 | // Updating here means the animation data doesn't need to be modified if the
 171 | // servos are setup differently.
 172 | // (E.g. Original Bob's hip servos are backwards to MobBob's.)
 173 | //
 174 | // (Also, I find it hard to remember the signs to use for each servo since they 
 175 | // are different for left/right hips, and for left/right feet.)
 176 | //------------------------------------------------------------------------------
 177 | 
 178 | 
 179 | int LeftHipCentre()              { return LEFT_HIP_CENTRE; }
 180 | int LeftHipIn(int millisecs)     { return LEFT_HIP_CENTRE + (FRONT_JOINT_HIPS * millisecs); }
 181 | int LeftHipOut(int millisecs)    { return LEFT_HIP_CENTRE - (FRONT_JOINT_HIPS * millisecs); }
 182 | 
 183 | int RightHipCentre()             { return RIGHT_HIP_CENTRE; }
 184 | int RightHipIn(int millisecs)    { return RIGHT_HIP_CENTRE - (FRONT_JOINT_HIPS * millisecs); }
 185 | int RightHipOut(int millisecs)   { return RIGHT_HIP_CENTRE + (FRONT_JOINT_HIPS * millisecs); }
 186 | 
 187 | int LeftFootFlat()               { return LEFT_FOOT_CENTRE; }
 188 | int LeftFootUp(int millisecs)    { return LEFT_FOOT_CENTRE - millisecs; }
 189 | int LeftFootDown(int millisecs)  { return LEFT_FOOT_CENTRE + millisecs; }
 190 | 
 191 | int RightFootFlat()              { return RIGHT_FOOT_CENTRE; }
 192 | int RightFootUp(int millisecs)   { return RIGHT_FOOT_CENTRE + millisecs; }
 193 | int RightFootDown(int millisecs) { return RIGHT_FOOT_CENTRE - millisecs; }
 194 | 
 195 | 
 196 | //----------------------------------------------------------------------------------
 197 | // Keyframe animation data for standard walking gait and other servo animations
 198 | //
 199 | // Format is { <millseconds>, <leftHipMicros>, <leftFootMicrosecs>, <rightHipMicrosecs>, <rightFootMicrosecs> }
 200 | //     milliseconds    - time to tween to to this keyframe's positions. E.g. 500 means it'll take 500ms to go from the
 201 | //                       robot's position at the start of this frame to the position specified in this frame
 202 | //     leftHipMicros   - position of left hip in servo microsecs.
 203 | //     leftFootMicros  - position of left hip in servo microsecs.
 204 | //     rightHipMicros  - position of left hip in servo microsecs.
 205 | //     rightFootMicros - position of left hip in servo microsecs.
 206 | // 
 207 | // The servo micro values, support a special value of -1. If this value is give, it tells
 208 | // the animation code to ignore this servo in this keyframe. i.e. That servo will
 209 | // stay in the position it had at the start of this keyframe.
 210 | //
 211 | // Also, the first element in the animation data arry is special. It is a metadata element.
 212 | // The first element is { <Num Frames>, 0, 0, 0, 0 }, which tells us the number of frames
 213 | // in the animation. So, the first actual keyframe is in animData[1], and the last keyframe
 214 | // is in animData[<Num Frames>]. (Where <Num Frames> is the value in animData[0][0].)
 215 | //----------------------------------------------------------------------------------
 216 | 
 217 | // Constants to make accessing the keyframe arrays more human readable.
 218 | const int TWEEN_TIME_VALUE = 0;
 219 | const int LEFT_HIP_VALUE   = 1;
 220 | const int LEFT_FOOT_VALUE  = 2;
 221 | const int RIGHT_HIP_VALUE  = 3;
 222 | const int RIGHT_FOOT_VALUE = 4;
 223 | 
 224 | 
 225 | // Constants used in the walking gait animation data.
 226 | const int FOOT_DELTA = 150;
 227 | const int HIP_DELTA  = FRONT_JOINT_HIPS * 150;
 228 | 
 229 | 
 230 | // Goes to the default standing straight position. Used by stopAnim().
 231 | int standStraightAnim[][5] = {
 232 |     // Metadata. First element is number of frames.
 233 |     { 1, 0, 0, 0, 0 },
 234 |     
 235 |     // Feet flat, Feet even
 236 |     { 300, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() }
 237 | };
 238 | 
 239 | 
 240 | // Prior to this, get the robot to Feet Flat, Feet Even (i.e. standStraightAnim).
 241 | int walkForwardAnim[][5] = {
 242 |     // Metadata. First element is number of frames.
 243 |     { 8, 0, 0, 0, 0 },
 244 |     
 245 |     // Tilt to left, Feet even
 246 |     { 300, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootDown(FOOT_DELTA) },
 247 |     
 248 |     // Tilt to left, Right foot forward
 249 |     { 300, LeftHipIn(HIP_DELTA), LeftFootUp(FOOT_DELTA), RightHipOut(HIP_DELTA), RightFootDown(FOOT_DELTA) },
 250 |     
 251 |     // Feet flat, Right foot forward
 252 |     { 300, LeftHipIn(HIP_DELTA), LeftFootFlat(), RightHipOut(HIP_DELTA), RightFootFlat() },
 253 |     
 254 |     // Tilt to right, Right foot forward
 255 |     { 300, LeftHipIn(HIP_DELTA), LeftFootDown(FOOT_DELTA), RightHipOut(HIP_DELTA), RightFootUp(FOOT_DELTA) },
 256 |     
 257 |     // Tilt to right, Feet even
 258 |     { 300, LeftHipCentre(), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) },
 259 |     
 260 |     // Tilt to right, Left foot forward
 261 |     { 300, LeftHipOut(HIP_DELTA), LeftFootDown(FOOT_DELTA), RightHipIn(HIP_DELTA), RightFootUp(FOOT_DELTA) },
 262 |     
 263 |     // Feet flat, Left foot forward
 264 |     { 300, LeftHipOut(HIP_DELTA), LeftFootFlat(), RightHipIn(HIP_DELTA), RightFootFlat() },
 265 |     
 266 |     // Tilt to left, Left foot forward
 267 |     { 300, LeftHipOut(HIP_DELTA), LeftFootUp(FOOT_DELTA), RightHipIn(HIP_DELTA), RightFootDown(FOOT_DELTA) }
 268 | };
 269 | 
 270 | 
 271 | // Prior to this, get the robot to Feet Flat, Feet Even (i.e. standStraightAnim).
 272 | int walkBackwardAnim[][5] = {
 273 |     // Metadata. First element is number of frames.
 274 |     { 8, 0, 0, 0, 0 },
 275 |     
 276 |     // Tilt to left, Feet even
 277 |     { 300, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootDown(FOOT_DELTA) },
 278 |     
 279 |     // Tilt to left, Left foot forward
 280 |     { 300, LeftHipOut(HIP_DELTA), LeftFootUp(FOOT_DELTA), RightHipIn(HIP_DELTA), RightFootDown(FOOT_DELTA) },
 281 | 
 282 |     // Feet flat, Left foot forward
 283 |     { 300, LeftHipOut(HIP_DELTA), LeftFootFlat(), RightHipIn(HIP_DELTA), RightFootFlat() },
 284 |         
 285 |     // Tilt to right, Left foot forward
 286 |     { 300, LeftHipOut(HIP_DELTA), LeftFootDown(FOOT_DELTA), RightHipIn(HIP_DELTA), RightFootUp(FOOT_DELTA) },
 287 |     
 288 |     // Tilt to right, Feet even
 289 |     { 300, LeftHipCentre(), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) },
 290 |     
 291 |     // Tilt to right, Right foot forward
 292 |     { 300, LeftHipIn(HIP_DELTA), LeftFootDown(FOOT_DELTA), RightHipOut(HIP_DELTA), RightFootUp(FOOT_DELTA) },
 293 |     
 294 |     // Feet flat, Right foot forward
 295 |     { 300, LeftHipIn(HIP_DELTA), LeftFootFlat(), RightHipOut(HIP_DELTA), RightFootFlat() },
 296 |     
 297 |     // Tilt to left, Right foot forward
 298 |     { 300, LeftHipIn(HIP_DELTA), LeftFootUp(FOOT_DELTA), RightHipOut(HIP_DELTA), RightFootDown(FOOT_DELTA) }
 299 | };
 300 | 
 301 | // Finish walk anim takes the robot from the end of walkForwardAnim/walkBackwardAnim back to standStraightAnim.
 302 | int walkEndAnim[][5] = {
 303 |     // Metadata. First element is number of frames.
 304 |     { 2, 0, 0, 0, 0 },
 305 |     
 306 |     // Tilt to left, Feet even
 307 |     { 300, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootDown(FOOT_DELTA) },
 308 | 
 309 |     // Feet flat, Feet even
 310 |     { 300, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() }
 311 | };
 312 | 
 313 | 
 314 | // Prior to this, get the robot to Feet Flat, Feet Even (i.e. standStraightAnim).
 315 | int turnLeftAnim[][5] = {
 316 |     // Metadata. First element is number of frames.
 317 |     { 6, 0, 0, 0, 0 },
 318 |     
 319 |     // Tilt to left, Feet even
 320 |     { 300, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootDown(FOOT_DELTA) },
 321 |     
 322 |     // Tilt to left, Turn left hip, Turn right hip
 323 |     { 300, LeftHipIn(HIP_DELTA), LeftFootUp(FOOT_DELTA), RightHipIn(HIP_DELTA), RightFootDown(FOOT_DELTA) },
 324 | 
 325 |     // Feet flat, Turn left hip, Turn right hip
 326 |     { 300, LeftHipIn(HIP_DELTA), LeftFootFlat(), RightHipIn(HIP_DELTA), RightFootFlat() },
 327 |         
 328 |     // Tilt to right, Turn left hip, Turn right hip
 329 |     { 300, LeftHipIn(HIP_DELTA), LeftFootDown(FOOT_DELTA), RightHipIn(HIP_DELTA), RightFootUp(FOOT_DELTA) },
 330 |     
 331 |     // Tilt to right, Feet even
 332 |     { 300, LeftHipCentre(), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) },
 333 |     
 334 |     // Feet flat, Feet even
 335 |     { 300, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() }
 336 | };
 337 | 
 338 | 
 339 | // Prior to this, get the robot to Feet Flat, Feet Even (i.e. standStraightAnim).
 340 | int turnRightAnim[][5] = {
 341 |     // Metadata. First element is number of frames.
 342 |     { 6, 0, 0, 0, 0 },
 343 |     
 344 |     // Tilt to right, Feet even
 345 |     { 300, LeftHipCentre(), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) },
 346 |     
 347 |     // Tilt to right, Turn left hip, Turn right hip
 348 |     { 300, LeftHipIn(HIP_DELTA), LeftFootDown(FOOT_DELTA), RightHipIn(HIP_DELTA), RightFootUp(FOOT_DELTA) },
 349 | 
 350 |     // Feet flat, Turn left hip, Turn right hip
 351 |     { 300, LeftHipIn(HIP_DELTA), LeftFootFlat(), RightHipIn(HIP_DELTA), RightFootFlat() },
 352 |         
 353 |     // Tilt to left, Turn left hip, Turn right hip
 354 |     { 300, LeftHipIn(HIP_DELTA), LeftFootUp(FOOT_DELTA), RightHipIn(HIP_DELTA), RightFootDown(FOOT_DELTA) },
 355 |     
 356 |     // Tilt to left, Feet even
 357 |     { 300, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootDown(FOOT_DELTA) },
 358 |     
 359 |     // Feet flat, Feet even
 360 |     { 300, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() }
 361 | };
 362 | 
 363 | 
 364 | // Shake head anim. Left right quickly to emulate shaking head.
 365 | int shakeHeadAnim[][5] = {
 366 |     // Metadata. First element is number of frames.
 367 |     { 4, 0, 0, 0, 0 },
 368 |     
 369 |     // Feet flat, Twist left
 370 |     { 150, LeftHipOut(HIP_DELTA), LeftFootFlat(), RightHipIn(HIP_DELTA), RightFootFlat() },
 371 |     
 372 |     // Feet flat, Feet even
 373 |     { 150, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() },
 374 |     
 375 |     // Feet flat, Twist right
 376 |     { 150, LeftHipIn(HIP_DELTA), LeftFootFlat(), RightHipOut(HIP_DELTA), RightFootFlat() },
 377 |     
 378 |     // Feet flat, Feet even
 379 |     { 150, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() }    
 380 | };
 381 | 
 382 | 
 383 | // Wobble anim. Tilt left and right to do a fun wobble.
 384 | int wobbleAnim[][5] = {
 385 |     // Metadata. First element is number of frames.
 386 |     { 4, 0, 0, 0, 0 },
 387 |     
 388 |     // Tilt left, Feet even
 389 |     { 300, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootDown(FOOT_DELTA) },
 390 |     
 391 |     // Feet flat, Feet even
 392 |     { 300, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() },
 393 |     
 394 |     // Tilt right, Feet even
 395 |     { 300, LeftHipCentre(), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) },
 396 |     
 397 |     // Feet flat, Feet even
 398 |     { 300, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() }    
 399 | };
 400 | 
 401 | // Wobble left anim. Tilt left and back.
 402 | int wobbleLeftAnim[][5] = {
 403 |     // Metadata. First element is number of frames.
 404 |     { 2, 0, 0, 0, 0 },
 405 |     
 406 |     // Tilt left, Feet even
 407 |     { 300, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootDown(FOOT_DELTA) },
 408 |     
 409 |     // Feet flat, Feet even
 410 |     { 300, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() },
 411 | };
 412 | 
 413 | 
 414 | // Wobble right anim. Tilt right and back.
 415 | int wobbleRightAnim[][5] = {
 416 |     // Metadata. First element is number of frames.
 417 |     { 2, 0, 0, 0, 0 },
 418 |     
 419 |     // Tilt right, Feet even
 420 |     { 300, LeftHipCentre(), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) },
 421 |     
 422 |     // Feet flat, Feet even
 423 |     { 300, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() }    
 424 | };
 425 | 
 426 | 
 427 | // Tap feet anim. Tap both feet.
 428 | int tapFeetAnim[][5] = {
 429 |     // Metadata. First element is number of frames.
 430 |     { 2, 0, 0, 0, 0 },
 431 |     
 432 |     // Raise both feet, Feet even
 433 |     { 500, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) },
 434 |     
 435 |     // Feet flat, Feet even
 436 |     { 500, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() },
 437 | };
 438 | 
 439 | 
 440 | // Tap left foot anim.
 441 | int tapLeftFootAnim[][5] = {
 442 |     // Metadata. First element is number of frames.
 443 |     { 2, 0, 0, 0, 0 },
 444 |     
 445 |     // Raise left foot, Feet even
 446 |     { 500, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootFlat() },
 447 |     
 448 |     // Feet flat, Feet even
 449 |     { 500, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() },
 450 | };
 451 | 
 452 | 
 453 | // Tap right foot anim.
 454 | int tapRightFootAnim[][5] = {
 455 |     // Metadata. First element is number of frames.
 456 |     { 2, 0, 0, 0, 0 },
 457 |     
 458 |     // Raise right foot, Feet even
 459 |     { 500, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootUp(FOOT_DELTA) },
 460 |     
 461 |     // Feet flat, Feet even
 462 |     { 500, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() },
 463 | };
 464 | 
 465 | 
 466 | // Bounce up and down anim.
 467 | int bounceAnim[][5] = {
 468 |     // Metadata. First element is number of frames.
 469 |     { 2, 0, 0, 0, 0 },
 470 |     
 471 |     // Raise both feet, Feet even
 472 |     { 500, LeftHipCentre(), LeftFootDown(300), RightHipCentre(), RightFootDown(300) },
 473 |     
 474 |     // Feet flat, Feet even
 475 |     { 500, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() },
 476 | };
 477 | 
 478 | 
 479 | // Shake Legs Animation.
 480 | int shakeLegsAnim[][5] = {
 481 |     // Metadata. First element is number of frames.
 482 |     { 14, 0, 0, 0, 0 },
 483 |     
 484 |     // Tilt left, Feet even
 485 |     { 300, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootDown(FOOT_DELTA) },
 486 |     
 487 |     // Tilt left, Right hip in
 488 |     { 100, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipIn(HIP_DELTA), RightFootDown(FOOT_DELTA) },
 489 |     
 490 |     // Tilt left, Feet even
 491 |     { 100, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootDown(FOOT_DELTA) },
 492 |     
 493 |     // Tilt left, Right hip out
 494 |     { 100, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipOut(HIP_DELTA), RightFootDown(FOOT_DELTA) },
 495 |     
 496 |     // Tilt left, Feet even
 497 |     { 100, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootDown(FOOT_DELTA) },
 498 |     
 499 |     // Tilt left, Right hip in
 500 |     { 100, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipIn(HIP_DELTA), RightFootDown(FOOT_DELTA) },
 501 |     
 502 |     // Tilt left, Feet even
 503 |     { 100, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootDown(FOOT_DELTA) },
 504 |     
 505 |     // Feet flat, Feet even
 506 |     { 300, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() },
 507 |     
 508 |     // Tilt right, Feet even
 509 |     { 300, LeftHipCentre(), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) },
 510 |     
 511 |     // Tilt right, Left hip in
 512 |     { 100, LeftHipIn(HIP_DELTA), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) },
 513 |     
 514 |     // Tilt right, Feet even
 515 |     { 100, LeftHipCentre(), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) },
 516 |     
 517 |     // Tilt right, Left hip out
 518 |     { 100, LeftHipOut(HIP_DELTA), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) },
 519 |     
 520 |     // Tilt right, Feet even
 521 |     { 100, LeftHipCentre(), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) },
 522 |     
 523 |     // Feet flat, Feet even
 524 |     { 300, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() }    
 525 | };
 526 | 
 527 | 
 528 | // Shake Left Leg Animation.
 529 | int shakeLeftLegAnim[][5] = {
 530 |     // Metadata. First element is number of frames.
 531 |     { 12, 0, 0, 0, 0 },
 532 |     
 533 |     // Tilt right, Feet even
 534 |     { 300, LeftHipCentre(), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) },
 535 |     
 536 |     // Tilt right, Left hip in
 537 |     { 100, LeftHipIn(HIP_DELTA), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) },
 538 |     
 539 |     // Tilt right, Feet even
 540 |     { 100, LeftHipCentre(), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) },
 541 |     
 542 |     // Tilt right, Left hip out
 543 |     { 100, LeftHipOut(HIP_DELTA), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) },
 544 |     
 545 |     // Tilt right, Feet even
 546 |     { 100, LeftHipCentre(), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) },
 547 |     
 548 |     // Tilt right, Left hip in
 549 |     { 100, LeftHipIn(HIP_DELTA), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) },
 550 |     
 551 |     // Tilt right, Feet even
 552 |     { 100, LeftHipCentre(), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) },
 553 |     
 554 |     // Tilt right, Left hip out
 555 |     { 100, LeftHipOut(HIP_DELTA), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) },
 556 |     
 557 |     // Tilt right, Feet even
 558 |     { 100, LeftHipCentre(), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) },
 559 |     
 560 |     // Tilt right, Left hip in
 561 |     { 100, LeftHipIn(HIP_DELTA), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) },
 562 |     
 563 |     // Tilt right, Feet even
 564 |     { 100, LeftHipCentre(), LeftFootDown(FOOT_DELTA), RightHipCentre(), RightFootUp(FOOT_DELTA) },
 565 |     
 566 |     // Feet flat, Feet even
 567 |     { 300, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() }    
 568 | };
 569 | 
 570 | 
 571 | // Shake Right Leg Animation.
 572 | int shakeRightLegAnim[][5] = {
 573 |     // Metadata. First element is number of frames.
 574 |     { 12, 0, 0, 0, 0 },
 575 |     
 576 |     // Tilt left, Feet even
 577 |     { 300, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootDown(FOOT_DELTA) },
 578 |     
 579 |     // Tilt left, Right hip in
 580 |     { 100, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipIn(HIP_DELTA), RightFootDown(FOOT_DELTA) },
 581 |     
 582 |     // Tilt left, Feet even
 583 |     { 100, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootDown(FOOT_DELTA) },
 584 |     
 585 |     // Tilt left, Right hip out
 586 |     { 100, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipOut(HIP_DELTA), RightFootDown(FOOT_DELTA) },
 587 |     
 588 |     // Tilt left, Feet even
 589 |     { 100, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootDown(FOOT_DELTA) },
 590 |     
 591 |     // Tilt left, Right hip in
 592 |     { 100, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipIn(HIP_DELTA), RightFootDown(FOOT_DELTA) },
 593 |     
 594 |     // Tilt left, Feet even
 595 |     { 100, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootDown(FOOT_DELTA) },
 596 |     
 597 |     // Tilt left, Right hip out
 598 |     { 100, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipOut(HIP_DELTA), RightFootDown(FOOT_DELTA) },
 599 |     
 600 |     // Tilt left, Feet even
 601 |     { 100, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootDown(FOOT_DELTA) },
 602 |     
 603 |     // Tilt left, Right hip in
 604 |     { 100, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipIn(HIP_DELTA), RightFootDown(FOOT_DELTA) },
 605 |     
 606 |     // Tilt left, Feet even
 607 |     { 100, LeftHipCentre(), LeftFootUp(FOOT_DELTA), RightHipCentre(), RightFootDown(FOOT_DELTA) },
 608 |     
 609 |     // Feet flat, Feet even
 610 |     { 300, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() },    
 611 | };
 612 | 
 613 | 
 614 | //----------------------------------------------------------------------------------
 615 | // Special dynamic animation data for setting/tweening servo positions.
 616 | //----------------------------------------------------------------------------------
 617 | 
 618 | // These are 2 special anim data that we use for the SetServos() function. They have
 619 | // a single frame. Those will change the data in these anim data and play them to 
 620 | // move the servos.
 621 | int setServosAnim1[][5] = {
 622 |     // Metadata. First element is number of frames.
 623 |     { 1, 0, 0, 0, 0 },
 624 |     
 625 |     // Tilt left, Feet even
 626 |     { 0, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() }
 627 | };
 628 | 
 629 | int setServosAnim2[][5] = {
 630 |     // Metadata. First element is number of frames.
 631 |     { 1, 0, 0, 0, 0 },
 632 |     
 633 |     // Tilt left, Feet even
 634 |     { 0, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() }
 635 | };
 636 | 
 637 | 
 638 | //----------------------------------------------------------------------------------
 639 | // Servo Variables
 640 | //----------------------------------------------------------------------------------
 641 | 
 642 | Servo servoLeftHip;
 643 | Servo servoLeftFoot;
 644 | Servo servoRightHip;
 645 | Servo servoRightFoot;
 646 | 
 647 | //----------------------------------------------------------------------------------
 648 | // State variables for playing animations.
 649 | //----------------------------------------------------------------------------------
 650 | 
 651 | // Milliseconds between animation updates.
 652 | const int millisBetweenAnimUpdate = 20;
 653 | 
 654 | // Time when we did the last animation update.
 655 | long timeAtLastAnimUpdate;
 656 | 
 657 | // Related to currently playing anim.
 658 | int  (*currAnim)[5];      // Current animation we're playing.
 659 | int  (*finishAnim)[5];    // Animation to play when the currAnim finishes or is stopped.
 660 | long timeAtStartOfFrame;  // millis() at last keyframe - frame we're lerping from
 661 | int  targetFrame;         // Frame we are lerping to
 662 | int  animNumLoops;        // Number of times to play the animation. -1 means loop forever.
 663 | char animCompleteStr[3] = "--"; // This is a 2 character string. When the anim is complete, 
 664 |                                 // we print out the status as "<" + animComplereStr + ">".
 665 | 
 666 | // Related to anim queue. I.e. Next anim to play.
 667 | bool animInProgress;    // Whether an animation is playing
 668 | 
 669 | int  (*nextAnim)[5];      // This is the next animation to play once the current one is done. 
 670 |                           // i.e. It's like a queue of size 1!
 671 |                           // If curr is non-looping, we play this at the end of the current anim.
 672 |                           // If curr is looping, this starts at the end of the current loop, 
 673 |                           // replacing curr anim.
 674 |                           // If nothing is playing, this starts right away.
 675 |                           
 676 | int  (*nextFinishAnim)[5]; // This is the finish animation for the queued animation.
 677 | 
 678 | int  nextAnimNumLoops;    // Number of times to play the animation. -1 means loop forever.
 679 | 
 680 | char nextAnimCompleteStr[3] = "--"; // This is a 2 character string. When the anim is complete, 
 681 |                                     // we print out the status as "<" + animComplereStr + ">".
 682 | 
 683 | bool interruptInProgressAnim; // Whether to change anim immediately, interrupting the current one.
 684 | 
 685 | 
 686 | // Curr servo positions
 687 | int  currLeftHip;
 688 | int  currLeftFoot;
 689 | int  currRightHip;
 690 | int  currRightFoot;
 691 | 
 692 | // Servo positions at start of current keyframe
 693 | int  startLeftHip;
 694 | int  startLeftFoot;
 695 | int  startRightHip;
 696 | int  startRightFoot;
 697 | 
 698 | 
 699 | //-------------------------------------------------------------------------------
 700 | // Parser Variables
 701 | //-------------------------------------------------------------------------------
 702 | 
 703 | // Constant delimiter tag chars
 704 | const char START_CHAR = '<';
 705 | const char END_CHAR   = '>';
 706 | const char SEP_CHAR   = ',';
 707 | 
 708 | // Constants and a variable for the parser state.
 709 | const int PARSER_WAITING = 0; // Waiting for '<' to start parsing.
 710 | const int PARSER_COMMAND = 1; // Reading the command string.
 711 | const int PARSER_PARAM1  = 2; // Reading param 1.
 712 | const int PARSER_PARAM2  = 3; // Reading param 2.
 713 | const int PARSER_PARAM3  = 4; // Reading param 3.
 714 | const int PARSER_PARAM4  = 5; // Reading param 3.
 715 | const int PARSER_PARAM5  = 6; // Reading param 3.
 716 | const int PARSER_EXECUTE = 7; // Finished parsing a command, so execute it.
 717 | 
 718 | // Current parser state.
 719 | int currParserState = PARSER_WAITING; 
 720 | 
 721 | // String for storing the command. 2 chars for the command and 1 char for '\0'.
 722 | // We store the command here as we're parsing.
 723 | char currCmd[3] = "--";
 724 | 
 725 | // For tracking which letter we are in the command.
 726 | int currCmdIndex;
 727 | 
 728 | // Max command length.
 729 | const int CMD_LENGTH = 2;
 730 | 
 731 | 
 732 | // Current param values. Store them here after we parse them.
 733 | int currParam1Val;
 734 | int currParam2Val;
 735 | int currParam3Val;
 736 | int currParam4Val;
 737 | int currParam5Val;
 738 | 
 739 | // Variable for tracking which digit we're parsing in a param.
 740 | // We use this to convert the single digits back into a decimal value.
 741 | int currParamIndex;
 742 | 
 743 | // Whether the current param is negative.
 744 | boolean currParamNegative;
 745 | 
 746 | // Max parameter length. Stop parsing if it exceeds this.
 747 | const int MAX_PARAM_LENGTH = 6;
 748 | 
 749 | 
 750 | //===============================================================================
 751 | // Arduino setup() and loop().
 752 | //===============================================================================
 753 | 
 754 | void setup() 
 755 | {
 756 |     // Setup the main serial port
 757 |     Serial.begin(SERIAL_SPEED);
 758 |     
 759 |     // Setup the Servos
 760 |     servoLeftHip.attach(  SERVO_LEFT_HIP,   LEFT_HIP_MIN,   LEFT_HIP_MAX);
 761 |     servoLeftFoot.attach( SERVO_LEFT_FOOT,  LEFT_FOOT_MIN,  LEFT_FOOT_MAX);
 762 |     servoRightHip.attach( SERVO_RIGHT_HIP,  RIGHT_HIP_MIN,  RIGHT_HIP_MAX);
 763 |     servoRightFoot.attach(SERVO_RIGHT_FOOT, RIGHT_FOOT_MIN, RIGHT_FOOT_MAX);
 764 | 
 765 |     // Set things up for the parser.
 766 |     setup_Parser();
 767 |     
 768 |     // Set things up for the animation code.
 769 |     setup_Animation();
 770 | }
 771 | 
 772 | void loop() 
 773 | {
 774 |     // Update the parser.
 775 |     loop_Parser();
 776 |     
 777 |     // Update the animation.
 778 |     loop_Animation();
 779 | }
 780 | 
 781 | 
 782 | //===============================================================================
 783 | // Related to the parser
 784 | //===============================================================================
 785 | 
 786 | // Sets up the parser stuff. Called in setup(). Should not be called elsewhere.
 787 | void setup_Parser()
 788 | {
 789 |     // Wait for first command.
 790 |     currParserState = PARSER_WAITING;
 791 |     
 792 |     // Print this response to say we've booted and are ready.
 793 |     Serial.println("<OK>");
 794 | }
 795 | 
 796 | 
 797 | // Loop() for the parser stuff. Called in loop(). Should not be called elsewhere.
 798 | void loop_Parser()
 799 | {
 800 |     //---------------------------------------------------------
 801 |     // PARSER
 802 |     //
 803 |     // If there is data, parse it and process it.
 804 |     //---------------------------------------------------------
 805 |     
 806 |     // Read from pin serial port and write it out on USB port.
 807 |     if (Serial.available() > 0)
 808 |     {
 809 |         char c = Serial.read();
 810 |     
 811 |         // If we're in WAITING state, look for the START_CHAR.
 812 |         if (currParserState == PARSER_WAITING)
 813 |         {
 814 |             // If it's the START_CHAR, move out of this state...
 815 |             if (c == START_CHAR)
 816 |             {
 817 |                 // Start parsing the command.
 818 |                 currParserState = PARSER_COMMAND;
 819 |         
 820 |                 // Reset thing ready for parsing
 821 |                 currCmdIndex = 0;
 822 |                 currCmd[0] = '-';
 823 |                 currCmd[1] = '-';
 824 |                 currParam1Val = 0;
 825 |                 currParam2Val = 0;
 826 |                 currParam3Val = 0;
 827 |                 currParam4Val = 0;
 828 |                 currParam5Val = 0;
 829 |             }
 830 |       
 831 |             // Otherwise, stay in this state.
 832 |         }
 833 |     
 834 |         // In the state to look for the command.
 835 |         else if (currParserState == PARSER_COMMAND)
 836 |         {
 837 |             // Else if it's a separator, parse parameter 1. But make sure it's not
 838 |             // empty, or else it's a parse error.
 839 |             if (c == SEP_CHAR)
 840 |             {
 841 |                 if (currCmdIndex == CMD_LENGTH)
 842 |                 {
 843 |                     currParserState = PARSER_PARAM1;
 844 |                     currParamIndex = 0;
 845 |                     currParamNegative = false;
 846 |                 }
 847 |                 else
 848 |                 {
 849 |                     currParserState = PARSER_WAITING;
 850 |                 }
 851 |             }
 852 |       
 853 |             // Else if it's the end char, there are no parameters, so we're ready to
 854 |             // process. But make sure it's not empty. Otherwise, it's a parse error.
 855 |             else if (c == END_CHAR)
 856 |             {
 857 |                 if (currCmdIndex == CMD_LENGTH)
 858 |                 {
 859 |                     currParserState = PARSER_EXECUTE;
 860 |                 }
 861 |                 else
 862 |                 {
 863 |                     currParserState = PARSER_WAITING;
 864 |                 }
 865 |             }
 866 |       
 867 |             // If we've got too many letters here, we have a parse error,
 868 |             // so abandon and go back to PARSER_WAITING
 869 |             else if ( (currCmdIndex >= CMD_LENGTH) || (c < 'A') || (c > 'Z') )
 870 |             {
 871 |                 currParserState = PARSER_WAITING;
 872 |             }
 873 |       
 874 |             // Store the current character.
 875 |             else
 876 |             {
 877 |                 currCmd[currCmdIndex] = c;
 878 |                 currCmdIndex++;
 879 |             }
 880 |         }
 881 |     
 882 |         // In the state to parse param 1.
 883 |         else if (currParserState == PARSER_PARAM1)
 884 |         {
 885 |             // Else if it's a separator, parse parameter 1.
 886 |             if (c == SEP_CHAR)
 887 |             {
 888 |                 if (currParamNegative)
 889 |                 {
 890 |                     currParam1Val = -1 * currParam1Val;
 891 |                 }
 892 | 
 893 |                 currParserState = PARSER_PARAM2;
 894 |                 currParamIndex = 0;
 895 |                 currParamNegative = false;
 896 |             }
 897 |       
 898 |             // Else if it's the end char, there are no parameters, so we're ready to
 899 |             // process.
 900 |             else if (c == END_CHAR)
 901 |             {
 902 |                 if (currParamNegative)
 903 |                 {
 904 |                     currParam1Val = -1 * currParam1Val;
 905 |                 }
 906 | 
 907 |                 currParserState = PARSER_EXECUTE;
 908 |             }
 909 |       
 910 |             // Check for negative at the start.
 911 |             else if ( (currParamIndex == 0) && (c == '-') )
 912 |             {
 913 |                 currParamNegative = true;
 914 |                 currParamIndex++;
 915 |             }
 916 |             
 917 |             // If it's too long, or the character is not a digit, then it's
 918 |             // a parse error, so abandon and go back to PARSER_WAITING.
 919 |             else if ( (currParamIndex >= MAX_PARAM_LENGTH) || (c < '0') || (c > '9') )
 920 |             {
 921 |                 currParserState = PARSER_WAITING;
 922 |             }
 923 | 
 924 |             // It's a valid character, so process it.
 925 |             else
 926 |             {
 927 |                 // Shift existing value across and add new digit at the bottom.
 928 |                 int currDigitVal = c - '0';
 929 |                 currParam1Val = (currParam1Val * 10) + currDigitVal;
 930 |                 currParamIndex++;
 931 |             }
 932 | 
 933 |         }
 934 |     
 935 |         // In the state to parse param 2.
 936 |         else if (currParserState == PARSER_PARAM2)
 937 |         {
 938 |             // Else if it's a separator, parse parameter 2.
 939 |             if (c == SEP_CHAR)
 940 |             {
 941 |                 if (currParamNegative)
 942 |                 {
 943 |                     currParam2Val = -1 * currParam2Val;
 944 |                 }
 945 | 
 946 |                 currParserState = PARSER_PARAM3;
 947 |                 currParamIndex = 0;
 948 |                 currParamNegative = false;
 949 |             }
 950 |       
 951 |             // Else if it's the end char, there are no parameters, so we're ready to
 952 |             // process.
 953 |             else if (c == END_CHAR)
 954 |             {
 955 |                 if (currParamNegative)
 956 |                 {
 957 |                     currParam2Val = -1 * currParam2Val;
 958 |                 }
 959 | 
 960 |                 currParserState = PARSER_EXECUTE;
 961 |             }
 962 |       
 963 |             // Check for negative at the start.
 964 |             else if ( (currParamIndex == 0) && (c == '-') )
 965 |             {
 966 |                 currParamNegative = true;
 967 |                 currParamIndex++;
 968 |             }
 969 |             
 970 |             // If it's too long, or the character is not a digit, then it's
 971 |             // a parse error, so abandon and go back to PARSER_WAITING.
 972 |             else if ( (currParamIndex >= MAX_PARAM_LENGTH) || (c < '0') || (c > '9') )
 973 |             {
 974 |                 currParserState = PARSER_WAITING;
 975 |             }
 976 | 
 977 |             // It's a valid character, so process it.
 978 |             else
 979 |             {
 980 |                 // Shift existing value across and add new digit at the bottom.
 981 |                 int currDigitVal = c - '0';
 982 |                 currParam2Val = (currParam2Val * 10) + currDigitVal;
 983 |                 currParamIndex++;
 984 |             }
 985 | 
 986 |         }
 987 |     
 988 |         // In the state to parse param 3.
 989 |         else if (currParserState == PARSER_PARAM3)
 990 |         {
 991 |             // Else if it's a separator, parse parameter 2.
 992 |             if (c == SEP_CHAR)
 993 |             {
 994 |                 if (currParamNegative)
 995 |                 {
 996 |                     currParam3Val = -1 * currParam3Val;
 997 |                 }
 998 | 
 999 |                 currParserState = PARSER_PARAM4;
1000 |                 currParamIndex = 0;
1001 |                 currParamNegative = false;
1002 |             }
1003 |       
1004 |             // Else if it's the end char, there are no parameters, so we're ready to
1005 |             // process.
1006 |             else if (c == END_CHAR)
1007 |             {
1008 |                 if (currParamNegative)
1009 |                 {
1010 |                     currParam3Val = -1 * currParam3Val;
1011 |                 }
1012 | 
1013 |                 currParserState = PARSER_EXECUTE;
1014 |             }
1015 |       
1016 |             // Check for negative at the start.
1017 |             else if ( (currParamIndex == 0) && (c == '-') )
1018 |             {
1019 |                 currParamNegative = true;
1020 |                 currParamIndex++;
1021 |             }
1022 |             
1023 |             // If it's too long, or the character is not a digit, then it's
1024 |             // a parse error, so abandon and go back to PARSER_WAITING.
1025 |             else if ( (currParamIndex >= MAX_PARAM_LENGTH) || (c < '0') || (c > '9') )
1026 |             {
1027 |                 currParserState = PARSER_WAITING;
1028 |             }
1029 | 
1030 |             // It's a valid character, so process it.
1031 |             else
1032 |             {
1033 |                 // Shift existing value across and add new digit at the bottom.
1034 |                 int currDigitVal = c - '0';
1035 |                 currParam3Val = (currParam3Val * 10) + currDigitVal;
1036 |                 currParamIndex++;
1037 |             }
1038 | 
1039 |         }
1040 |     
1041 |         // In the state to parse param 4.
1042 |         else if (currParserState == PARSER_PARAM4)
1043 |         {
1044 |             // Else if it's a separator, parse parameter 2.
1045 |             if (c == SEP_CHAR)
1046 |             {
1047 |                 if (currParamNegative)
1048 |                 {
1049 |                     currParam4Val = -1 * currParam4Val;
1050 |                 }
1051 | 
1052 |                 currParserState = PARSER_PARAM5;
1053 |                 currParamIndex = 0;
1054 |                 currParamNegative = false;
1055 |             }
1056 |       
1057 |             // Else if it's the end char, there are no parameters, so we're ready to
1058 |             // process.
1059 |             else if (c == END_CHAR)
1060 |             {
1061 |                 if (currParamNegative)
1062 |                 {
1063 |                     currParam4Val = -1 * currParam4Val;
1064 |                 }
1065 | 
1066 |                 currParserState = PARSER_EXECUTE;
1067 |             }
1068 |       
1069 |             // Check for negative at the start.
1070 |             else if ( (currParamIndex == 0) && (c == '-') )
1071 |             {
1072 |                 currParamNegative = true;
1073 |                 currParamIndex++;
1074 |             }
1075 |             
1076 |             // If it's too long, or the character is not a digit, then it's
1077 |             // a parse error, so abandon and go back to PARSER_WAITING.
1078 |             else if ( (currParamIndex >= MAX_PARAM_LENGTH) || (c < '0') || (c > '9') )
1079 |             {
1080 |                 currParserState = PARSER_WAITING;
1081 |             }
1082 | 
1083 |             // It's a valid character, so process it.
1084 |             else
1085 |             {
1086 |                 // Shift existing value across and add new digit at the bottom.
1087 |                 int currDigitVal = c - '0';
1088 |                 currParam4Val = (currParam4Val * 10) + currDigitVal;
1089 |                 currParamIndex++;
1090 |             }
1091 | 
1092 |         }
1093 |             // In the state to parse param 5.
1094 |         else if (currParserState == PARSER_PARAM5)
1095 |         {
1096 |             // If it's the end char, there are no parameters, so we're ready to
1097 |             // process.
1098 |             if (c == END_CHAR)
1099 |             {
1100 |                 if (currParamNegative)
1101 |                 {
1102 |                     currParam5Val = -1 * currParam5Val;
1103 |                 }
1104 |                 currParserState = PARSER_EXECUTE;
1105 |             }
1106 |       
1107 |             // Check for negative at the start.
1108 |             else if ( (currParamIndex == 0) && (c == '-') )
1109 |             {
1110 |                 currParamNegative = true;
1111 |                 currParamIndex++;
1112 |             }
1113 |             
1114 |             // If it's too long, or the character is not a digit, then it's
1115 |             // a parse error, so abandon and go back to PARSER_WAITING.
1116 |             else if ( (currParamIndex >= MAX_PARAM_LENGTH) || (c < '0') || (c > '9') )
1117 |             {
1118 |                 currParserState = PARSER_WAITING;
1119 |             }
1120 | 
1121 |             // It's a valid character, so process it.
1122 |             else
1123 |             {
1124 |                 // Shift existing value across and add new digit at the bottom.
1125 |                 int currDigitVal = c - '0';
1126 |                 currParam5Val = (currParam5Val * 10) + currDigitVal;
1127 |                 currParamIndex++;
1128 |             }
1129 | 
1130 |         }
1131 |     
1132 |         
1133 |         //---------------------------------------------------------
1134 |         // PARSER CODE HANDLER (Still part of Parser, but section that
1135 |         // processes completed commands)
1136 |         //
1137 |         // If the most recently read char completes a command,
1138 |         // then process the command, and clear the state to
1139 |         // go back to looking for a new command.
1140 |         //
1141 |         // The parsed items are stored in:
1142 |         //    currCmd, currParam1Val, currParam2Val, currParam3Val, 
1143 |         //             currParam4Val, currParam5Val
1144 |         //---------------------------------------------------------
1145 |     
1146 |         if (currParserState == PARSER_EXECUTE)
1147 |         {
1148 |             // Ready/OK Check: <OK>
1149 |             if ((currCmd[0] == 'O') && (currCmd[1] == 'K'))
1150 |             {
1151 |                 Serial.println("<OK>");
1152 |             }
1153 |             
1154 |             // Set Servo: <SV, time, leftHip, leftFoot, rightHip, rightFoot>
1155 |             // time      - time to tween to specified angles
1156 |             // leftHip   - microsecs from centre. -ve is hip in, +ve is hip out 
1157 |             // leftFoot  - microsecs from flat. -ve is foot down, +ve is foot up
1158 |             // rightHip  - microsecs from centre. -ve is hip in, +ve is hip out 
1159 |             // rightFoot - microsecs from flat. -ve is foot down, +ve is foot up
1160 |             else if ((currCmd[0] == 'S') && (currCmd[1] == 'V'))
1161 |             {
1162 |                 int tweenTime = currParam1Val;
1163 |                 if (currParam1Val < 0)
1164 |                 {
1165 |                     tweenTime = 0;
1166 |                 }
1167 |                 SetServos(tweenTime, currParam2Val, currParam3Val, currParam4Val, currParam5Val, "SV");
1168 |             }
1169 |             
1170 |             // Stop/Reset: <ST>, Stops current anim. Also can be used to put robot into reset position.
1171 |             else if ((currCmd[0] == 'S') && (currCmd[1] == 'T'))
1172 |             {
1173 |                 StopAnim("ST");
1174 |             }
1175 |             
1176 |             // Stop Immediate: <SI>
1177 |             else if ((currCmd[0] == 'S') && (currCmd[1] == 'I'))
1178 |             {
1179 |                 StopAnimImmediate("SI");
1180 |             }
1181 |             
1182 |             // Forward: <FW, #Times>, -1 means continuous, 0 or no param is the same as 1 time.
1183 |             else if ((currCmd[0] == 'F') && (currCmd[1] == 'W'))
1184 |             {
1185 |                 int numTimes = currParam1Val;
1186 |                 if (currParam1Val < 0)
1187 |                 {
1188 |                     numTimes = -1;
1189 |                 }
1190 |                 
1191 |                 PlayAnimNumTimes(walkForwardAnim, walkEndAnim, numTimes, "FW");
1192 |             }
1193 |             
1194 |             // Backward: <BW, #Times>, -1 means continuous, 0 or no param is the same as 1 time.
1195 |             else if ((currCmd[0] == 'B') && (currCmd[1] == 'W'))
1196 |             {
1197 |                 int numTimes = currParam1Val;
1198 |                 if (currParam1Val < 0)
1199 |                 {
1200 |                     numTimes = -1;
1201 |                 }
1202 |                 
1203 |                 PlayAnimNumTimes(walkBackwardAnim, walkEndAnim, numTimes, "BW");
1204 |             }
1205 |             
1206 |             // Turn Left: <LT, #Times>, -1 means continuous, 0 or no param is the same as 1 time.
1207 |             else if ((currCmd[0] == 'L') && (currCmd[1] == 'T'))
1208 |             {
1209 |                 int numTimes = currParam1Val;
1210 |                 if (currParam1Val < 0)
1211 |                 {
1212 |                     numTimes = -1;
1213 |                 }
1214 |                 
1215 |                 PlayAnimNumTimes(turnLeftAnim, NULL, numTimes, "LT");
1216 |             }
1217 |             
1218 |             // Turn Right: <RT, #Times>, -1 means continuous, 0 or no param is the same as 1 time.
1219 |             else if ((currCmd[0] == 'R') && (currCmd[1] == 'T'))
1220 |             {
1221 |                 int numTimes = currParam1Val;
1222 |                 if (currParam1Val < 0)
1223 |                 {
1224 |                     numTimes = -1;
1225 |                 }
1226 |                 
1227 |                 PlayAnimNumTimes(turnRightAnim, NULL, numTimes, "RT");
1228 |             }
1229 |             
1230 |             // Shake Head: <SX, #Times>, -1 means continuous, 0 or no param is the same as 1 time.
1231 |             else if ((currCmd[0] == 'S') && (currCmd[1] == 'X'))
1232 |             {
1233 |                 int numTimes = currParam1Val;
1234 |                 if (currParam1Val < 0)
1235 |                 {
1236 |                     numTimes = -1;
1237 |                 }
1238 |                 
1239 |                 PlayAnimNumTimes(shakeHeadAnim, NULL, numTimes, "SX");
1240 |             }
1241 |             
1242 |             // Bounce: <BX, #Times>, -1 means continuous, 0 or no param is the same as 1 time.
1243 |             else if ((currCmd[0] == 'B') && (currCmd[1] == 'X'))
1244 |             {
1245 |                 int numTimes = currParam1Val;
1246 |                 if (currParam1Val < 0)
1247 |                 {
1248 |                     numTimes = -1;
1249 |                 }
1250 |                 
1251 |                 PlayAnimNumTimes(bounceAnim, NULL, numTimes, "BX");
1252 |             }
1253 |             
1254 |             // Wobble: <WX, #Times>, -1 means continuous, 0 or no param is the same as 1 time.
1255 |             else if ((currCmd[0] == 'W') && (currCmd[1] == 'X'))
1256 |             {
1257 |                 int numTimes = currParam1Val;
1258 |                 if (currParam1Val < 0)
1259 |                 {
1260 |                     numTimes = -1;
1261 |                 }
1262 |                 
1263 |                 PlayAnimNumTimes(wobbleAnim, NULL, numTimes, "WX");
1264 |             }
1265 |             
1266 |             // Wobble Left: <WY, #Times>, -1 means continuous, 0 or no param is the same as 1 time.
1267 |             else if ((currCmd[0] == 'W') && (currCmd[1] == 'Y'))
1268 |             {
1269 |                 int numTimes = currParam1Val;
1270 |                 if (currParam1Val < 0)
1271 |                 {
1272 |                     numTimes = -1;
1273 |                 }
1274 |                 
1275 |                 PlayAnimNumTimes(wobbleLeftAnim, NULL, numTimes, "WY");
1276 |             }
1277 |             
1278 |             // Wobble Right: <WZ, #Times>, -1 means continuous, 0 or no param is the same as 1 time.
1279 |             else if ((currCmd[0] == 'W') && (currCmd[1] == 'Z'))
1280 |             {
1281 |                 int numTimes = currParam1Val;
1282 |                 if (currParam1Val < 0)
1283 |                 {
1284 |                     numTimes = -1;
1285 |                 }
1286 |                 
1287 |                 PlayAnimNumTimes(wobbleRightAnim, NULL, numTimes, "WZ");
1288 |             }
1289 |             
1290 |             // Tap Feet: <TX, #Times>, -1 means continuous, 0 or no param is the same as 1 time.
1291 |             else if ((currCmd[0] == 'T') && (currCmd[1] == 'X'))
1292 |             {
1293 |                 int numTimes = currParam1Val;
1294 |                 if (currParam1Val < 0)
1295 |                 {
1296 |                     numTimes = -1;
1297 |                 }
1298 |                 
1299 |                 PlayAnimNumTimes(tapFeetAnim, NULL, numTimes, "TX");
1300 |             }
1301 |             
1302 |             // Tap Left Foot: <TY, #Times>, -1 means continuous, 0 or no param is the same as 1 time.
1303 |             else if ((currCmd[0] == 'T') && (currCmd[1] == 'Y'))
1304 |             {
1305 |                 int numTimes = currParam1Val;
1306 |                 if (currParam1Val < 0)
1307 |                 {
1308 |                     numTimes = -1;
1309 |                 }
1310 |                 
1311 |                 PlayAnimNumTimes(tapLeftFootAnim, NULL, numTimes, "TY");
1312 |             }
1313 |             
1314 |             // Tap Right Foot: <TZ, #Times>, -1 means continuous, 0 or no param is the same as 1 time.
1315 |             else if ((currCmd[0] == 'T') && (currCmd[1] == 'Z'))
1316 |             {
1317 |                 int numTimes = currParam1Val;
1318 |                 if (currParam1Val < 0)
1319 |                 {
1320 |                     numTimes = -1;
1321 |                 }
1322 |                 
1323 |                 PlayAnimNumTimes(tapRightFootAnim, NULL, numTimes, "TZ");
1324 |             }
1325 |             
1326 |             // Shake Legs: <LX, #Times>, -1 means continuous, 0 or no param is the same as 1 time.
1327 |             else if ((currCmd[0] == 'L') && (currCmd[1] == 'X'))
1328 |             {
1329 |                 int numTimes = currParam1Val;
1330 |                 if (currParam1Val < 0)
1331 |                 {
1332 |                     numTimes = -1;
1333 |                 }
1334 |                 
1335 |                 PlayAnimNumTimes(shakeLegsAnim, NULL, numTimes, "LX");
1336 |             }
1337 |             
1338 |             // Shake Left Leg: <LY, #Times>, -1 means continuous, 0 or no param is the same as 1 time.
1339 |             else if ((currCmd[0] == 'L') && (currCmd[1] == 'Y'))
1340 |             {
1341 |                 int numTimes = currParam1Val;
1342 |                 if (currParam1Val < 0)
1343 |                 {
1344 |                     numTimes = -1;
1345 |                 }
1346 |                 
1347 |                 PlayAnimNumTimes(shakeLeftLegAnim, NULL, numTimes, "LY");
1348 |             }
1349 |             
1350 |             // Shake Right Leg: <LZ, #Times>, -1 means continuous, 0 or no param is the same as 1 time.
1351 |             else if ((currCmd[0] == 'L') && (currCmd[1] == 'Z'))
1352 |             {
1353 |                 int numTimes = currParam1Val;
1354 |                 if (currParam1Val < 0)
1355 |                 {
1356 |                     numTimes = -1;
1357 |                 }
1358 |                 
1359 |                 PlayAnimNumTimes(shakeRightLegAnim, NULL, numTimes, "LZ");
1360 |             }
1361 |             
1362 |             //--------------------------------------------------
1363 |             // Clear the state and wait for the next command!
1364 |             // This must be done!
1365 |             //--------------------------------------------------
1366 |             currParserState = PARSER_WAITING;
1367 |         }
1368 |     }
1369 | }
1370 | 
1371 | 
1372 | //===============================================================================
1373 | // Related to playing servo animations.
1374 | //===============================================================================
1375 | 
1376 | // Call this to play the given animation once. Pass in NULL if there is no finishAnim.
1377 | void PlayAnim(int animToPlay[][5], int finishAnim[][5], const char *completeStr)
1378 | {
1379 |     // Put this in the queue.
1380 |     PlayAnimNumTimes(animToPlay, finishAnim, 1, completeStr);
1381 | }
1382 | 
1383 | // Call this to loop the given animation. Pass in NULL if there is no finishAnim.
1384 | void LoopAnim(int animToPlay[][5], int finishAnim[][5], const char *completeStr)
1385 | {
1386 |     // Put this in the queue.
1387 |     PlayAnimNumTimes(animToPlay, finishAnim, -1, completeStr);
1388 | }
1389 | 
1390 | // Call this to play the given animation the specified number of times. 
1391 | // -1 number of times will make it loop forever.
1392 | // Pass in NULL if there is no finishAnim.
1393 | void PlayAnimNumTimes(int animToPlay[][5], int finishAnim[][5], int numTimes, const char *completeStr)
1394 | {
1395 |     // Put this in the queue.
1396 |     nextAnim         = animToPlay;
1397 |     nextFinishAnim   = finishAnim;
1398 |     nextAnimNumLoops = numTimes;
1399 | 
1400 |     // Save the completeStr
1401 |     if (completeStr == NULL)
1402 |     {
1403 |         nextAnimCompleteStr[0] = '-';
1404 |         nextAnimCompleteStr[1] = '-';
1405 |     }
1406 |     else
1407 |     {
1408 |         nextAnimCompleteStr[0] = completeStr[0];
1409 |         nextAnimCompleteStr[1] = completeStr[1];
1410 |     }
1411 | }
1412 | 
1413 | // Stop after the current animation.
1414 | void StopAnim(const char *completeStr)
1415 | {
1416 |     // Put this in the queue.
1417 |     PlayAnimNumTimes(standStraightAnim, NULL, 1, completeStr);
1418 | }
1419 | 
1420 | // Stop immediately and lerp robot to zero position, interrupting 
1421 | // any animation that is in progress.
1422 | void StopAnimImmediate(const char *completeStr)
1423 | {
1424 |     // Put this in the queue.
1425 |     interruptInProgressAnim = true;
1426 |     PlayAnimNumTimes(standStraightAnim, NULL, 1, completeStr);
1427 | }
1428 | 
1429 | // Moves servos to the specified positions. Time 0 will make it immediate. Otherwise,
1430 | // it'll tween it over a specified time.
1431 | // For positions, 0 means centered.
1432 | // For hips, -ve is hip left, +ve is hip right
1433 | // For feet, -ve is foot down, +ve is foot up
1434 | void SetServos(int tweenTime, int leftHip, int leftFoot, int rightHip, int rightFoot, const char* completeStr)
1435 | {
1436 |     // Save the completeStr
1437 |     if (completeStr == NULL)
1438 |     {
1439 |         nextAnimCompleteStr[0] = '-';
1440 |         nextAnimCompleteStr[1] = '-';
1441 |     }
1442 |     else
1443 |     {
1444 |         nextAnimCompleteStr[0] = completeStr[0];
1445 |         nextAnimCompleteStr[1] = completeStr[1];
1446 |     }
1447 |     
1448 |     // Decide which tween data we use. We don't want to over-write the one that is
1449 |     // in progress. We have and reuse these to keep memory allocation fixed.
1450 |     int (*tweenServoData)[5];
1451 |     if (currAnim != setServosAnim1)
1452 |     {
1453 |         tweenServoData = setServosAnim1;
1454 |     }
1455 |     else
1456 |     {
1457 |         tweenServoData = setServosAnim2;
1458 |     }
1459 |     
1460 |     // Set the tween information into the animation data.
1461 |     tweenServoData[1][TWEEN_TIME_VALUE] = tweenTime;
1462 |     tweenServoData[1][LEFT_HIP_VALUE]   = LeftHipIn(leftHip);
1463 |     tweenServoData[1][LEFT_FOOT_VALUE]  = LeftFootUp(leftFoot);
1464 |     tweenServoData[1][RIGHT_HIP_VALUE]  = RightHipIn(rightHip);
1465 |     tweenServoData[1][RIGHT_FOOT_VALUE] = RightFootUp(rightFoot);
1466 |     
1467 |     // Queue this tween to be played next.
1468 |     PlayAnim(tweenServoData, NULL, completeStr);
1469 | }
1470 | 
1471 | 
1472 | // Set up variables for animation. This is called in setup(). Should be not called by anywhere else.
1473 | void setup_Animation()
1474 | {
1475 |     // Set the servos to the feet flat, feet even position.
1476 |     currLeftHip   = LEFT_HIP_CENTRE;
1477 |     currLeftFoot  = LEFT_FOOT_CENTRE;
1478 |     currRightHip  = RIGHT_HIP_CENTRE;
1479 |     currRightFoot = RIGHT_FOOT_CENTRE;
1480 |     UpdateServos();
1481 |     
1482 |     // Set the "start" positions to the current ones. So, when
1483 |     // we pay the next anim, we will tween from the current positions.
1484 |     startLeftHip   = currLeftHip;
1485 |     startLeftFoot  = currLeftFoot;
1486 |     startRightHip  = currRightHip;
1487 |     startRightFoot = currRightFoot;
1488 |     
1489 |     // No animation is playing yet, and nothing in the queue yet.
1490 |     timeAtLastAnimUpdate    = millis();
1491 |     animInProgress          = false;
1492 |     interruptInProgressAnim = false;
1493 |     currAnim       = NULL;
1494 |     finishAnim     = NULL;
1495 |     nextAnim       = NULL;
1496 |     nextFinishAnim = NULL;
1497 | }
1498 | 
1499 | // Loop function for processing animation. This is called in every loop(). Should be be called by anywhere else.
1500 | //
1501 | // NOTE: The way looping animations work is that they basically add themselves back to the queue
1502 | //       when a cycle is done, and if there's nothing already queued up! This way, looping animations
1503 | //       work in a similar way to single-play animations, and fits into the queueing system.
1504 | void loop_Animation()
1505 | {
1506 |     // Get the time at the start of this frame.
1507 |     long currTime = millis();
1508 | 
1509 |     //--------------------------------------------------------------------------------------
1510 |     // Decide if we want to perform the animation update. We don't execute this every frame.
1511 |     //--------------------------------------------------------------------------------------
1512 |     
1513 |     if (timeAtLastAnimUpdate + millisBetweenAnimUpdate > currTime)
1514 |     {
1515 |         // Not yet time to do an anim update, so jump out.
1516 |         return;
1517 |     }
1518 |     else
1519 |     {
1520 |         // We reset the timer, and then proceed below to handle the current anim update.
1521 |         timeAtLastAnimUpdate = currTime;
1522 |     }
1523 |     
1524 |     //--------------------------------------------------------------------------------------
1525 |     // Decide if we need to setup and start a new animation. We do if there's no anim 
1526 |     // playing or we've been asked to interrupt the anim.
1527 |     //--------------------------------------------------------------------------------------
1528 |     
1529 |     if ( (nextAnim != NULL) &&  (!animInProgress || interruptInProgressAnim) )
1530 |     {
1531 |         // If this was an interrupt, we also set the "start" servo positions
1532 |         // to the current ones. This way, the animation system will tween from the
1533 |         // current positions.
1534 |         if (interruptInProgressAnim)
1535 |         {
1536 |             // This is the place to notify someone of an animation finishing after getting interrupted
1537 |             // Print the command string we just finished. -1 parameter indicates it was interrupted.
1538 |             Serial.print("<");
1539 |             Serial.print(animCompleteStr);
1540 |             Serial.println(",-1>");
1541 |             
1542 |             // Set the "start" positions to the current ones. So, when
1543 |             // we pay the next anim, we will tween from the current positions.
1544 |             startLeftHip   = currLeftHip;
1545 |             startLeftFoot  = currLeftFoot;
1546 |             startRightHip  = currRightHip;
1547 |             startRightFoot = currRightFoot;
1548 |             
1549 |             // We've handled any interrupt request, so clear the flag.
1550 |             interruptInProgressAnim = false;
1551 |         }
1552 |         
1553 |         // Store the animation we are now playing.
1554 |         currAnim           = nextAnim;
1555 |         finishAnim         = nextFinishAnim;
1556 |         animCompleteStr[0] = nextAnimCompleteStr[0];
1557 |         animCompleteStr[1] = nextAnimCompleteStr[1];
1558 | 
1559 |         nextAnim               = NULL; // Queue is cleared.
1560 |         nextFinishAnim         = NULL;
1561 |         nextAnimCompleteStr[0] = '-';
1562 |         nextAnimCompleteStr[1] = '-';
1563 |         
1564 |         // Record the number of times to play the animation.
1565 |         animNumLoops = nextAnimNumLoops;
1566 |         
1567 |         // Treat current time as start of frame for the initial lerp to the first frame.
1568 |         timeAtStartOfFrame = currTime;
1569 |         
1570 |         // Set the frame counters.
1571 |         targetFrame = 1; // First frame we are lerping to. Index 0 is metadata, so skip.
1572 |         
1573 |         // An animation is now in progress
1574 |         animInProgress = true;
1575 |     }
1576 | 
1577 |     //--------------------------------------------------------------------------------------
1578 |     // If we are currently playing an animation, then update the animation state and the
1579 |     // servo positions.
1580 |     //--------------------------------------------------------------------------------------
1581 |     
1582 |     if (animInProgress)
1583 |     {
1584 |         // Determine if we need to switch to the next frame.
1585 |         int timeInCurrFrame = currTime - timeAtStartOfFrame;
1586 |         if (timeInCurrFrame > currAnim[targetFrame][TWEEN_TIME_VALUE])
1587 |         {
1588 |             // Set the servo positions to the targetFrame's values.
1589 |             // We only set this if the value is > 0. -ve values means that
1590 |             // the current target keyframe did not alter that servos position.
1591 |             if (currAnim[targetFrame][LEFT_HIP_VALUE] >= 0)
1592 |             {
1593 |                 currLeftHip = currAnim[targetFrame][LEFT_HIP_VALUE];
1594 |             }
1595 |             if (currAnim[targetFrame][LEFT_FOOT_VALUE] >= 0)
1596 |             {
1597 |                 currLeftFoot = currAnim[targetFrame][LEFT_FOOT_VALUE];
1598 |             }
1599 |             if (currAnim[targetFrame][RIGHT_HIP_VALUE] >= 0)
1600 |             {
1601 |                 currRightHip = currAnim[targetFrame][RIGHT_HIP_VALUE];
1602 |             }
1603 |             if (currAnim[targetFrame][RIGHT_FOOT_VALUE] >= 0)
1604 |             {
1605 |                 currRightFoot = currAnim[targetFrame][RIGHT_FOOT_VALUE];
1606 |             }
1607 |             UpdateServos();
1608 |             
1609 |             // These current values are now the start of frame values.
1610 |             startLeftHip   = currLeftHip;
1611 |             startLeftFoot  = currLeftFoot;
1612 |             startRightHip  = currRightHip;
1613 |             startRightFoot = currRightFoot;
1614 |             
1615 |             // Now, we try to move to the next frame.
1616 |             // - If there is a next frame, set that as the new target, and proceed.
1617 |             // - If there's no next frame, but it's looping, we re-add this animation
1618 |             //   to the queue.
1619 |             // - If there's no next frame, and this is not looping, we stop animating.
1620 |             // (Remember that targetFrame is 1-based since the first element of the animation
1621 |             // data array is metadata)
1622 |             
1623 |             // Increment targetFrame, and reset time in the current frame.
1624 |             targetFrame++;
1625 |             timeAtStartOfFrame = currTime;
1626 |             
1627 |             // If there is no next frame, we stop this current animation.
1628 |             // If it is looping, then we re-queue the current animation if the queue is empty.
1629 |             if (targetFrame > NumOfFrames(currAnim))
1630 |             {
1631 |                 // Stop the current animation.
1632 |                 animInProgress = false;
1633 |                 
1634 |                 // If we're looping forever, and there's no next anim, re-queue the 
1635 |                 // animation if the queue is empty.
1636 |                 if ((animNumLoops < 0) && (nextAnim == NULL))
1637 |                 {
1638 |                     LoopAnim(currAnim, finishAnim, animCompleteStr);
1639 |                 }
1640 |                 
1641 |                 // If we're looping forever, and there is something in the queue, then
1642 |                 // finish the animation and proceed.
1643 |                 else if ((animNumLoops < 0) && (nextAnim != NULL))
1644 |                 {
1645 |                     if (finishAnim != NULL)
1646 |                     {
1647 |                         // Switch to the finish anim.
1648 |                         currAnim       = finishAnim;
1649 |                         finishAnim     = NULL;
1650 |                         
1651 |                         // Record the number of times to play the animation.
1652 |                         animNumLoops = 1;
1653 |                         
1654 |                         // Treat current time as start of frame for the initial lerp to the first frame.
1655 |                         timeAtStartOfFrame = currTime;
1656 |                         
1657 |                         // Set the frame counters.
1658 |                         targetFrame = 1; // First frame we are lerping to. Index 0 is metadata, so skip.
1659 |                         
1660 |                         // An animation is now in progress
1661 |                         animInProgress = true;
1662 |                     }
1663 |                     else
1664 |                     {
1665 |                         // We've stopped, so can notify if needed.
1666 |                         // Print the command string we just finished.
1667 |                         Serial.print("<");
1668 |                         Serial.print(animCompleteStr);
1669 |                         Serial.println(">");
1670 |                     }
1671 |                 }
1672 |                 
1673 |                 // If we're looping a limited number of times, and there's no next anim,
1674 |                 // re-queue the animation if the queue is empty.
1675 |                 else if ((animNumLoops > 1) && (nextAnim == NULL))
1676 |                 {
1677 |                     PlayAnimNumTimes(currAnim, finishAnim, animNumLoops-1, animCompleteStr);
1678 |                 }
1679 |                 
1680 |                 // In this case, numAnimLoops is 1, this is the last loop through, so
1681 |                 // we're done. We play the finishAnim first if needed.
1682 |                 else
1683 |                 {
1684 |                     // If there is a finish animation, switch to that animation.
1685 |                     if (finishAnim != NULL)
1686 |                     {
1687 |                         // Switch to the finish anim.
1688 |                         currAnim       = finishAnim;
1689 |                         finishAnim     = NULL;
1690 |                         
1691 |                         // Record the number of times to play the animation.
1692 |                         animNumLoops = 1;
1693 |                         
1694 |                         // Treat current time as start of frame for the initial lerp to the first frame.
1695 |                         timeAtStartOfFrame = currTime;
1696 |                         
1697 |                         // Set the frame counters.
1698 |                         targetFrame = 1; // First frame we are lerping to. Index 0 is metadata, so skip.
1699 |                         
1700 |                         // An animation is now in progress
1701 |                         animInProgress = true;
1702 |                     }
1703 |                     
1704 |                     // Otherwise, we're done! We've played the finishAnim if there was one.
1705 |                     else
1706 |                     {
1707 |                         // Print the command string we just finished.
1708 |                         Serial.print("<");
1709 |                         Serial.print(animCompleteStr);
1710 |                         Serial.println(">");
1711 |                     }
1712 |                 }
1713 |             }
1714 |         }
1715 |         
1716 |         // If we're still animating (i.e. the previous check didn't find that
1717 |         // we've finished the current animation), then proceed.
1718 |         if (animInProgress)
1719 |         {
1720 |             // Set the servos per data in the current frame. We only update the servos that have target
1721 |             // microsecond values > 0. This is to support the feature where we leave a servo at its
1722 |             // existing position if an animation data item is -1.
1723 |             float frameTimeFraction = (currTime - timeAtStartOfFrame) / ((float) currAnim[targetFrame][TWEEN_TIME_VALUE]);
1724 |             
1725 |             if (currAnim[targetFrame][LEFT_HIP_VALUE] >= 0)
1726 |             {
1727 |                 currLeftHip = startLeftHip + ((currAnim[targetFrame][LEFT_HIP_VALUE] - startLeftHip) * frameTimeFraction);
1728 |             }
1729 |             
1730 |             if (currAnim[targetFrame][LEFT_FOOT_VALUE] >= 0)
1731 |             {
1732 |                 currLeftFoot = startLeftFoot + ((currAnim[targetFrame][LEFT_FOOT_VALUE] - startLeftFoot)  * frameTimeFraction);
1733 |             }
1734 |             
1735 |             if (currAnim[targetFrame][RIGHT_HIP_VALUE] >= 0)
1736 |             {
1737 |                 currRightHip = startRightHip  + ((currAnim[targetFrame][RIGHT_HIP_VALUE] - startRightHip) * frameTimeFraction);
1738 |             }
1739 |             
1740 |             if (currAnim[targetFrame][RIGHT_FOOT_VALUE] >= 0)
1741 |             {
1742 |                 currRightFoot = startRightFoot + ((currAnim[targetFrame][RIGHT_FOOT_VALUE] - startRightFoot) * frameTimeFraction);
1743 |             }
1744 |             
1745 |             UpdateServos();
1746 |         }
1747 |     }
1748 | }
1749 | 
1750 | 
1751 | // Move all the servo to the positions set in the curr... variables.
1752 | // In the code, we update those variables and then call this to set the servos.
1753 | void UpdateServos()
1754 | {
1755 |     servoLeftHip.writeMicroseconds(currLeftHip);
1756 |     servoLeftFoot.writeMicroseconds(currLeftFoot);
1757 |     servoRightHip.writeMicroseconds(currRightHip);
1758 |     servoRightFoot.writeMicroseconds(currRightFoot);
1759 | }
1760 | 
1761 | 
1762 | // Return the number of frames in the given animation data.
1763 | // Have this helper function to avoid the "magic number" reference of animData[0][0].
1764 | int NumOfFrames(int animData[][5])
1765 | {
1766 |     return animData[0][0];
1767 | }
1768 | 
1769 | 
1770 | 
1771 | 
1772 | 
1773 | 


--------------------------------------------------------------------------------
/ArduinoCode/README.txt:
--------------------------------------------------------------------------------
 1 | The are 2 versions of the Arduino code here.
 2 | 
 3 | The "MobBob-Control-Bluno" version is designed for use with DFRobot's Bluno boards.
 4 | This is what I use in my MobBob builds as the Bluno board has Bluetooth LE built-in.
 5 | 
 6 | The "MobBob-Control-Bluetooth" version is designed for use with other Arduino
 7 | microcontrollers and Bluetooth boards. The Bluetooth board is connected to the
 8 | Arduino board using 2 digital pins (as a software serial port). Using this version
 9 | will let you use other Arduino boards (E.g. Arduino Nano, Arduino Pro) with other
10 | Bluetooth boards (e.g. JY-MCU, HC-05, etc).
11 | 
12 | For both versions of the code, you will want to:
13 | 
14 |     - Update the pin variables to match your build
15 |     
16 |     - Tweak the servo center, min and max values
17 |     
18 |     - Set "FRONT_JOINT_HIPS" to 1 or -1 depending on which way your
19 |       hip servos are mounted. I mount it with the servo axel at the front
20 |       of MobBob. For this configuration, set this value to 1.
21 | 
22 | All the best! And have fun! :D


--------------------------------------------------------------------------------
/LICENSE:
--------------------------------------------------------------------------------
  1 | GNU GENERAL PUBLIC LICENSE
  2 |                        Version 2, June 1991
  3 | 
  4 |  Copyright (C) 1989, 1991 Free Software Foundation, Inc., <http://fsf.org/>
  5 |  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
  6 |  Everyone is permitted to copy and distribute verbatim copies
  7 |  of this license document, but changing it is not allowed.
  8 | 
  9 |                             Preamble
 10 | 
 11 |   The licenses for most software are designed to take away your
 12 | freedom to share and change it.  By contrast, the GNU General Public
 13 | License is intended to guarantee your freedom to share and change free
 14 | software--to make sure the software is free for all its users.  This
 15 | General Public License applies to most of the Free Software
 16 | Foundation's software and to any other program whose authors commit to
 17 | using it.  (Some other Free Software Foundation software is covered by
 18 | the GNU Lesser General Public License instead.)  You can apply it to
 19 | your programs, too.
 20 | 
 21 |   When we speak of free software, we are referring to freedom, not
 22 | price.  Our General Public Licenses are designed to make sure that you
 23 | have the freedom to distribute copies of free software (and charge for
 24 | this service if you wish), that you receive source code or can get it
 25 | if you want it, that you can change the software or use pieces of it
 26 | in new free programs; and that you know you can do these things.
 27 | 
 28 |   To protect your rights, we need to make restrictions that forbid
 29 | anyone to deny you these rights or to ask you to surrender the rights.
 30 | These restrictions translate to certain responsibilities for you if you
 31 | distribute copies of the software, or if you modify it.
 32 | 
 33 |   For example, if you distribute copies of such a program, whether
 34 | gratis or for a fee, you must give the recipients all the rights that
 35 | you have.  You must make sure that they, too, receive or can get the
 36 | source code.  And you must show them these terms so they know their
 37 | rights.
 38 | 
 39 |   We protect your rights with two steps: (1) copyright the software, and
 40 | (2) offer you this license which gives you legal permission to copy,
 41 | distribute and/or modify the software.
 42 | 
 43 |   Also, for each author's protection and ours, we want to make certain
 44 | that everyone understands that there is no warranty for this free
 45 | software.  If the software is modified by someone else and passed on, we
 46 | want its recipients to know that what they have is not the original, so
 47 | that any problems introduced by others will not reflect on the original
 48 | authors' reputations.
 49 | 
 50 |   Finally, any free program is threatened constantly by software
 51 | patents.  We wish to avoid the danger that redistributors of a free
 52 | program will individually obtain patent licenses, in effect making the
 53 | program proprietary.  To prevent this, we have made it clear that any
 54 | patent must be licensed for everyone's free use or not licensed at all.
 55 | 
 56 |   The precise terms and conditions for copying, distribution and
 57 | modification follow.
 58 | 
 59 |                     GNU GENERAL PUBLIC LICENSE
 60 |    TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION
 61 | 
 62 |   0. This License applies to any program or other work which contains
 63 | a notice placed by the copyright holder saying it may be distributed
 64 | under the terms of this General Public License.  The "Program", below,
 65 | refers to any such program or work, and a "work based on the Program"
 66 | means either the Program or any derivative work under copyright law:
 67 | that is to say, a work containing the Program or a portion of it,
 68 | either verbatim or with modifications and/or translated into another
 69 | language.  (Hereinafter, translation is included without limitation in
 70 | the term "modification".)  Each licensee is addressed as "you".
 71 | 
 72 | Activities other than copying, distribution and modification are not
 73 | covered by this License; they are outside its scope.  The act of
 74 | running the Program is not restricted, and the output from the Program
 75 | is covered only if its contents constitute a work based on the
 76 | Program (independent of having been made by running the Program).
 77 | Whether that is true depends on what the Program does.
 78 | 
 79 |   1. You may copy and distribute verbatim copies of the Program's
 80 | source code as you receive it, in any medium, provided that you
 81 | conspicuously and appropriately publish on each copy an appropriate
 82 | copyright notice and disclaimer of warranty; keep intact all the
 83 | notices that refer to this License and to the absence of any warranty;
 84 | and give any other recipients of the Program a copy of this License
 85 | along with the Program.
 86 | 
 87 | You may charge a fee for the physical act of transferring a copy, and
 88 | you may at your option offer warranty protection in exchange for a fee.
 89 | 
 90 |   2. You may modify your copy or copies of the Program or any portion
 91 | of it, thus forming a work based on the Program, and copy and
 92 | distribute such modifications or work under the terms of Section 1
 93 | above, provided that you also meet all of these conditions:
 94 | 
 95 |     a) You must cause the modified files to carry prominent notices
 96 |     stating that you changed the files and the date of any change.
 97 | 
 98 |     b) You must cause any work that you distribute or publish, that in
 99 |     whole or in part contains or is derived from the Program or any
100 |     part thereof, to be licensed as a whole at no charge to all third
101 |     parties under the terms of this License.
102 | 
103 |     c) If the modified program normally reads commands interactively
104 |     when run, you must cause it, when started running for such
105 |     interactive use in the most ordinary way, to print or display an
106 |     announcement including an appropriate copyright notice and a
107 |     notice that there is no warranty (or else, saying that you provide
108 |     a warranty) and that users may redistribute the program under
109 |     these conditions, and telling the user how to view a copy of this
110 |     License.  (Exception: if the Program itself is interactive but
111 |     does not normally print such an announcement, your work based on
112 |     the Program is not required to print an announcement.)
113 | 
114 | These requirements apply to the modified work as a whole.  If
115 | identifiable sections of that work are not derived from the Program,
116 | and can be reasonably considered independent and separate works in
117 | themselves, then this License, and its terms, do not apply to those
118 | sections when you distribute them as separate works.  But when you
119 | distribute the same sections as part of a whole which is a work based
120 | on the Program, the distribution of the whole must be on the terms of
121 | this License, whose permissions for other licensees extend to the
122 | entire whole, and thus to each and every part regardless of who wrote it.
123 | 
124 | Thus, it is not the intent of this section to claim rights or contest
125 | your rights to work written entirely by you; rather, the intent is to
126 | exercise the right to control the distribution of derivative or
127 | collective works based on the Program.
128 | 
129 | In addition, mere aggregation of another work not based on the Program
130 | with the Program (or with a work based on the Program) on a volume of
131 | a storage or distribution medium does not bring the other work under
132 | the scope of this License.
133 | 
134 |   3. You may copy and distribute the Program (or a work based on it,
135 | under Section 2) in object code or executable form under the terms of
136 | Sections 1 and 2 above provided that you also do one of the following:
137 | 
138 |     a) Accompany it with the complete corresponding machine-readable
139 |     source code, which must be distributed under the terms of Sections
140 |     1 and 2 above on a medium customarily used for software interchange; or,
141 | 
142 |     b) Accompany it with a written offer, valid for at least three
143 |     years, to give any third party, for a charge no more than your
144 |     cost of physically performing source distribution, a complete
145 |     machine-readable copy of the corresponding source code, to be
146 |     distributed under the terms of Sections 1 and 2 above on a medium
147 |     customarily used for software interchange; or,
148 | 
149 |     c) Accompany it with the information you received as to the offer
150 |     to distribute corresponding source code.  (This alternative is
151 |     allowed only for noncommercial distribution and only if you
152 |     received the program in object code or executable form with such
153 |     an offer, in accord with Subsection b above.)
154 | 
155 | The source code for a work means the preferred form of the work for
156 | making modifications to it.  For an executable work, complete source
157 | code means all the source code for all modules it contains, plus any
158 | associated interface definition files, plus the scripts used to
159 | control compilation and installation of the executable.  However, as a
160 | special exception, the source code distributed need not include
161 | anything that is normally distributed (in either source or binary
162 | form) with the major components (compiler, kernel, and so on) of the
163 | operating system on which the executable runs, unless that component
164 | itself accompanies the executable.
165 | 
166 | If distribution of executable or object code is made by offering
167 | access to copy from a designated place, then offering equivalent
168 | access to copy the source code from the same place counts as
169 | distribution of the source code, even though third parties are not
170 | compelled to copy the source along with the object code.
171 | 
172 |   4. You may not copy, modify, sublicense, or distribute the Program
173 | except as expressly provided under this License.  Any attempt
174 | otherwise to copy, modify, sublicense or distribute the Program is
175 | void, and will automatically terminate your rights under this License.
176 | However, parties who have received copies, or rights, from you under
177 | this License will not have their licenses terminated so long as such
178 | parties remain in full compliance.
179 | 
180 |   5. You are not required to accept this License, since you have not
181 | signed it.  However, nothing else grants you permission to modify or
182 | distribute the Program or its derivative works.  These actions are
183 | prohibited by law if you do not accept this License.  Therefore, by
184 | modifying or distributing the Program (or any work based on the
185 | Program), you indicate your acceptance of this License to do so, and
186 | all its terms and conditions for copying, distributing or modifying
187 | the Program or works based on it.
188 | 
189 |   6. Each time you redistribute the Program (or any work based on the
190 | Program), the recipient automatically receives a license from the
191 | original licensor to copy, distribute or modify the Program subject to
192 | these terms and conditions.  You may not impose any further
193 | restrictions on the recipients' exercise of the rights granted herein.
194 | You are not responsible for enforcing compliance by third parties to
195 | this License.
196 | 
197 |   7. If, as a consequence of a court judgment or allegation of patent
198 | infringement or for any other reason (not limited to patent issues),
199 | conditions are imposed on you (whether by court order, agreement or
200 | otherwise) that contradict the conditions of this License, they do not
201 | excuse you from the conditions of this License.  If you cannot
202 | distribute so as to satisfy simultaneously your obligations under this
203 | License and any other pertinent obligations, then as a consequence you
204 | may not distribute the Program at all.  For example, if a patent
205 | license would not permit royalty-free redistribution of the Program by
206 | all those who receive copies directly or indirectly through you, then
207 | the only way you could satisfy both it and this License would be to
208 | refrain entirely from distribution of the Program.
209 | 
210 | If any portion of this section is held invalid or unenforceable under
211 | any particular circumstance, the balance of the section is intended to
212 | apply and the section as a whole is intended to apply in other
213 | circumstances.
214 | 
215 | It is not the purpose of this section to induce you to infringe any
216 | patents or other property right claims or to contest validity of any
217 | such claims; this section has the sole purpose of protecting the
218 | integrity of the free software distribution system, which is
219 | implemented by public license practices.  Many people have made
220 | generous contributions to the wide range of software distributed
221 | through that system in reliance on consistent application of that
222 | system; it is up to the author/donor to decide if he or she is willing
223 | to distribute software through any other system and a licensee cannot
224 | impose that choice.
225 | 
226 | This section is intended to make thoroughly clear what is believed to
227 | be a consequence of the rest of this License.
228 | 
229 |   8. If the distribution and/or use of the Program is restricted in
230 | certain countries either by patents or by copyrighted interfaces, the
231 | original copyright holder who places the Program under this License
232 | may add an explicit geographical distribution limitation excluding
233 | those countries, so that distribution is permitted only in or among
234 | countries not thus excluded.  In such case, this License incorporates
235 | the limitation as if written in the body of this License.
236 | 
237 |   9. The Free Software Foundation may publish revised and/or new versions
238 | of the General Public License from time to time.  Such new versions will
239 | be similar in spirit to the present version, but may differ in detail to
240 | address new problems or concerns.
241 | 
242 | Each version is given a distinguishing version number.  If the Program
243 | specifies a version number of this License which applies to it and "any
244 | later version", you have the option of following the terms and conditions
245 | either of that version or of any later version published by the Free
246 | Software Foundation.  If the Program does not specify a version number of
247 | this License, you may choose any version ever published by the Free Software
248 | Foundation.
249 | 
250 |   10. If you wish to incorporate parts of the Program into other free
251 | programs whose distribution conditions are different, write to the author
252 | to ask for permission.  For software which is copyrighted by the Free
253 | Software Foundation, write to the Free Software Foundation; we sometimes
254 | make exceptions for this.  Our decision will be guided by the two goals
255 | of preserving the free status of all derivatives of our free software and
256 | of promoting the sharing and reuse of software generally.
257 | 
258 |                             NO WARRANTY
259 | 
260 |   11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY
261 | FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW.  EXCEPT WHEN
262 | OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES
263 | PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED
264 | OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
265 | MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.  THE ENTIRE RISK AS
266 | TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU.  SHOULD THE
267 | PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING,
268 | REPAIR OR CORRECTION.
269 | 
270 |   12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING
271 | WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR
272 | REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES,
273 | INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING
274 | OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED
275 | TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY
276 | YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER
277 | PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE
278 | POSSIBILITY OF SUCH DAMAGES.
279 | 
280 |                      END OF TERMS AND CONDITIONS
281 | 
282 |             How to Apply These Terms to Your New Programs
283 | 
284 |   If you develop a new program, and you want it to be of the greatest
285 | possible use to the public, the best way to achieve this is to make it
286 | free software which everyone can redistribute and change under these terms.
287 | 
288 |   To do so, attach the following notices to the program.  It is safest
289 | to attach them to the start of each source file to most effectively
290 | convey the exclusion of warranty; and each file should have at least
291 | the "copyright" line and a pointer to where the full notice is found.
292 | 
293 |     {description}
294 |     Copyright (C) {year}  {fullname}
295 | 
296 |     This program is free software; you can redistribute it and/or modify
297 |     it under the terms of the GNU General Public License as published by
298 |     the Free Software Foundation; either version 2 of the License, or
299 |     (at your option) any later version.
300 | 
301 |     This program is distributed in the hope that it will be useful,
302 |     but WITHOUT ANY WARRANTY; without even the implied warranty of
303 |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
304 |     GNU General Public License for more details.
305 | 
306 |     You should have received a copy of the GNU General Public License along
307 |     with this program; if not, write to the Free Software Foundation, Inc.,
308 |     51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
309 | 
310 | Also add information on how to contact you by electronic and paper mail.
311 | 
312 | If the program is interactive, make it output a short notice like this
313 | when it starts in an interactive mode:
314 | 
315 |     Gnomovision version 69, Copyright (C) year name of author
316 |     Gnomovision comes with ABSOLUTELY NO WARRANTY; for details type `show w'.
317 |     This is free software, and you are welcome to redistribute it
318 |     under certain conditions; type `show c' for details.
319 | 
320 | The hypothetical commands `show w' and `show c' should show the appropriate
321 | parts of the General Public License.  Of course, the commands you use may
322 | be called something other than `show w' and `show c'; they could even be
323 | mouse-clicks or menu items--whatever suits your program.
324 | 
325 | You should also get your employer (if you work as a programmer) or your
326 | school, if any, to sign a "copyright disclaimer" for the program, if
327 | necessary.  Here is a sample; alter the names:
328 | 
329 |   Yoyodyne, Inc., hereby disclaims all copyright interest in the program
330 |   `Gnomovision' (which makes passes at compilers) written by James Hacker.
331 | 
332 |   {signature of Ty Coon}, 1 April 1989
333 |   Ty Coon, President of Vice
334 | 
335 | This General Public License does not permit incorporating your program into
336 | proprietary programs.  If your program is a subroutine library, you may
337 | consider it more useful to permit linking proprietary applications with the
338 | library.  If this is what you want to do, use the GNU Lesser General
339 | Public License instead of this License.
340 | 
341 | 


--------------------------------------------------------------------------------