/* LICENSE:
  =========================================================================
    CMPack'04 Source Code Release for OPEN-R SDK 1.1.5-r2 for ERS7
    Copyright (C) 2004 Multirobot Lab [Project Head: Manuela Veloso]
    School of Computer Science, Carnegie Mellon University
    All rights reserved.
  ========================================================================= */

#include "genmot.h"


void make_getups_ers7()
{
  MotionMaker m;

  //==== Get up off of side ====//

  m.start("gu_sideE7.mot",1.0);
  m.trans(500);

  m.headAng( 0.0, 0.0, 0.0);
  m.legAng(0, 0.0, 0.0, 0.0);
  m.legAng(1, 0.0, 0.0, 0.0);
  m.legAng(2, 0.0, 0.0, 0.0);
  m.legAng(3, 0.0, 0.0, 0.0);
  m.trans(500);

  m.legAng(0, 2.25, 0.0, 2.13);
  m.legAng(1, 0.00, 0.0, 0.00);
  m.legAng(2, 2.25, 0.0, 2.13);
  m.legAng(3, 0.00, 0.0, 0.00);
  m.trans(500);

  // m.legAng(1, 2.25, 0.0, 2.13);
  // m.legAng(3, 2.25, 0.0, 2.13);

  m.headAng(-1.2, 0.0,-0.25);
  m.legAng(0, 1.50, 1.5, 1.50);
  m.legAng(1, 0.00, 0.0, 1.50);
  m.legAng(2, 1.50, 1.5, 1.50);
  m.legAng(3, 0.00, 0.0, 1.50);
  m.trans(500);

  /*
  m.hold(500);

  m.headAng(-1.5, 0.0, 0.0);
  m.legAng(0, 2.25, 1.5, 2.13);
  m.legAng(1, 2.25, 1.5, 2.13);
  m.legAng(2, 2.25, 1.5, 2.13);
  m.legAng(3, 2.25, 1.5, 2.13);
  m.trans(1000);
  */

  m.legAng(0, 1.0, 0.0, 0.50);
  m.legAng(1, 1.0, 0.0, 0.50);
  m.legAng(2,-1.2, 0.0, 2.13);
  m.legAng(3,-1.2, 0.0, 2.13);

  m.end();
}

void make_getups_ers210()
{
  Motion::BodyStateMotion m[16];
  Motion::BodyState b;
  FILE *out;
  int i,n;


  //==== Get up off of front ====//
  mzero(b);
  b.head.attr = ATTR_ANGLES;
  set3(b.head.angles,0.0,0.0,0.0);

  for(i=0; i<4; i++) b.leg[i].attr = ATTR_ANGLES;
  n = 0;

  m[n].body = b;
  m[n].time = 500;
  n++;

  set3(b.leg[0].angles, 0.0, 0.0, 0.0);
  set3(b.leg[1].angles, 0.0, 0.0, 0.0);
  set3(b.leg[2].angles, 0.0, 0.0, 0.0);
  set3(b.leg[3].angles, 0.0, 0.0, 0.0);
  m[n].body = b;
  m[n].time = 500;
  n++;

  set3(b.leg[0].angles, 0.0, 1.6, 0.0);
  set3(b.leg[1].angles, 0.0, 1.6, 0.0);
  set3(b.leg[2].angles, 1.0, 0.0, 2.5);
  set3(b.leg[3].angles, 1.0, 0.0, 2.5);
  m[n].body = b;
  m[n].time = 500;
  n++;

  set3(b.leg[0].angles, 1.8, 1.6, 0.0);
  set3(b.leg[1].angles, 1.8, 1.6, 0.0);
  set3(b.leg[2].angles,-1.8, 0.0, 2.5);
  set3(b.leg[3].angles,-1.8, 0.0, 2.5);
  m[n].body = b;
  m[n].time = 500;
  n++;

  set3(b.leg[0].angles, 1.8, 0.0, 2.5);
  set3(b.leg[1].angles, 1.8, 0.0, 2.5);
  set3(b.leg[2].angles,-1.8, 0.0, 2.5);
  set3(b.leg[3].angles,-1.8, 0.0, 2.5);
  m[n].body = b;
  m[n].time = 500;
  n++;

  set3(b.leg[0].angles, 1.0, 0.0, 0.5);
  set3(b.leg[1].angles, 1.0, 0.0, 0.5);
  set3(b.leg[2].angles,-1.2, 0.0, 2.5);
  set3(b.leg[3].angles,-1.2, 0.0, 2.5);
  m[n].body = b;
  m[n].time = 500;
  n++;

  out = fopen("gu_front.mot","wb");
  fwrite(&n,sizeof(n),1,out);
  fwrite(m,sizeof(Motion::BodyStateMotion),n,out);
  fclose(out);

  //==== Get up off of back ====//
  mzero(b);
  b.head.attr = ATTR_ANGLES;
  set3(b.head.angles,RAD(-90),0.0,0.0);

  for(i=0; i<4; i++) b.leg[i].attr = ATTR_ANGLES;
  n = 0;

  /*
  m[n].body = b;
  m[n].time = 500;
  n++;

  set3(b.leg[0].angles, 0.0, 0.0, 0.0);
  set3(b.leg[1].angles, 0.0, 0.0, 0.0);
  set3(b.leg[2].angles, 0.0, 0.0, 0.0);
  set3(b.leg[3].angles, 0.0, 0.0, 0.0);
  */
  m[n].body = b;
  m[n].time = 250;
  n++;

  set3(b.leg[0].angles,-0.5, 0.0, 0.0);
  set3(b.leg[1].angles,-0.5, 0.0, 0.0);
  set3(b.leg[2].angles,-1.0, 0.5, 2.0);
  set3(b.leg[3].angles,-1.0, 0.5, 2.0);
  m[n].body = b;
  m[n].time = 500;
  n++;

  set3(b.leg[0].angles, 0.0, 0.0, 1.0);
  set3(b.leg[1].angles, 0.0, 0.0, 1.0);
  set3(b.leg[2].angles,-1.8, 0.1, 2.5);
  set3(b.leg[3].angles,-1.8, 0.1, 2.5);
  m[n].body = b;
  m[n].time = 250;
  n++;

  set3(b.leg[0].angles, 1.0, 0.0, 0.5);
  set3(b.leg[1].angles, 1.0, 0.0, 0.5);
  set3(b.leg[2].angles,-1.2, 0.0, 2.5);
  set3(b.leg[3].angles,-1.2, 0.0, 2.5);
  m[n].body = b;
  m[n].time = 500;
  n++;

  out = fopen("gu_back.mot","wb");
  fwrite(&n,sizeof(n),1,out);
  fwrite(m,sizeof(Motion::BodyStateMotion),n,out);
  fclose(out);

  //==== Get up off of side ====//
  mzero(b);
  b.head.attr = ATTR_ANGLES;
  set3(b.head.angles,0.0,0.0,0.0);

  for(i=0; i<4; i++) b.leg[i].attr = ATTR_ANGLES;
  n = 0;

  m[n].body = b;
  m[n].time = 500;
  n++;

  set3(b.leg[0].angles, 0.0, 0.0, 0.0);
  set3(b.leg[1].angles, 0.0, 0.0, 0.0);
  set3(b.leg[2].angles, 0.0, 0.0, 0.0);
  set3(b.leg[3].angles, 0.0, 0.0, 0.0);
  m[n].body = b;
  m[n].time = 500;
  n++;

  set3(b.leg[0].angles, 2.0, 0.0, 2.5);
  set3(b.leg[1].angles, 2.0, 0.0, 2.5);
  set3(b.leg[2].angles, 2.0, 0.0, 2.5);
  set3(b.leg[3].angles, 2.0, 0.0, 2.5);
  m[n].body = b;
  m[n].time = 300;
  n++;

  set3(b.leg[0].angles, 2.0, 1.6, 2.5);
  set3(b.leg[1].angles, 2.0, 1.6, 2.5);
  set3(b.leg[2].angles, 2.0, 1.6, 2.5);
  set3(b.leg[3].angles, 2.0, 1.6, 2.5);
  m[n].body = b;
  m[n].time = 700;
  n++;

  set3(b.leg[0].angles, 1.0, 0.0, 0.5);
  set3(b.leg[1].angles, 1.0, 0.0, 0.5);
  set3(b.leg[2].angles,-1.2, 0.0, 2.5);
  set3(b.leg[3].angles,-1.2, 0.0, 2.5);
  m[n].body = b;
  m[n].time = 500;
  n++;

  out = fopen("gu_side.mot","wb");
  fwrite(&n,sizeof(n),1,out);
  fwrite(m,sizeof(Motion::BodyStateMotion),n,out);
  fclose(out);
}

void make_getups()
{
  make_getups_ers210();
  make_getups_ers7();
}

void make_blocks()
{
  Motion::BodyStateMotion m[16];
  Motion::BodyState b;
  FILE *out;
  int n;

  //==== Supporter Block ====//
  mzero(b);

  n = 0;
  m[n].body = b;
  m[n].time = 200;
  n++;

  BodyPos(b,100.0,RAD(17.5));
  LegPos(b,0,  110, 180,0); // originally 120, 170
  LegPos(b,1,  110,-180,0);
  LegPos(b,2,  -70,  75,0);
  LegPos(b,3,  -70, -75,0);  
  m[n].body = b;
  m[n].time = 0;
  n++;

  m[n].body = b;
  m[n].time = 500;
  n++;
  
  m[n].body = b;
  m[n].time = 500;
  n++;
  
  BodyPos(b,100.0,RAD(17.5));
  LegAng(b,0,  1.5,   0,0);
  LegAng(b,1,  1.5,   0,0);
  LegPos(b,2,  -70,  75,0);
  LegPos(b,3,  -70, -75,0);  
  m[n].body = b;
  m[n].time = 0;
  n++;

  out = fopen("b_supp.mot","wb");
  fwrite(&n,sizeof(n),1,out);
  fwrite(m,sizeof(Motion::BodyStateMotion),n,out);
  fclose(out);

  //==== Goalie Center Block ====//
  mzero(b);

  n = 0;
  m[n].body = b;
  m[n].time = 200;
  n++;

  BodyPos(b,100.0,RAD(17.5));
  LegPos(b,0,  120, 170,0);
  LegPos(b,1,  120,-170,0);
  LegPos(b,2,  -70,  75,0);
  LegPos(b,3,  -70, -75,0);  
  m[n].body = b;
  m[n].time = 0;
  n++;

  m[n].body = b;
  m[n].time = 500;
  n++;

  m[n].body = b;
  m[n].time = 300;
  n++;

  BodyPos(b,100.0,RAD(17.5));
  LegAng(b,0,  1.5,   0,0);
  LegAng(b,1,  1.5,   0,0);
  LegPos(b,2,  -70,  75,0);
  LegPos(b,3,  -70, -75,0);  
  m[n].body = b;
  m[n].time = 0;
  n++;


  out = fopen("b_gcntr.mot","wb");
  fwrite(&n,sizeof(n),1,out);
  fwrite(m,sizeof(Motion::BodyStateMotion),n,out);
  fclose(out);
  
  //==== Goalie Dive Block ====//

  mzero(b);

  n = 0;
  m[n].body = b;
  m[n].time = 100;
  n++;

  // start throwing out the arm
  BodyPos(b,100.0,RAD(17.5));
  LegAng  (b,0,   1.2,   1.5,     0);
  LegAng  (b,1,-0.426, 0.356,  1.55);
  LegAng  (b,2,-0.662, 0.456, 1.784);
  LegAng  (b,3,-0.662, 0.456, 1.784);
  m[n].body = b;
  m[n].time = 80;
  n++;

  BodyPos(b,100.0,RAD(17.5));
  LegAng  (b,0,   1.2,   1.5,     0);
  LegAng  (b,1,-0.426, 0.356,  1.55);
  LegAng  (b,2,-0.662, 0.456, 1.784);
  LegAng  (b,3,-0.662, 0.456, 1.784);
  m[n].body = b;
  m[n].time = 0;
  n++;

  // arm fully stuck out, down on ground
  BodyPos(b,60,RAD(20));
  LegAng  (b,0,   1.8,   1.5,   0.3);
  LegAng  (b,1,-0.426,   0.8,  0.95);
  LegAng  (b,2,  -0.3,   0.2,   2.5);
  LegAng  (b,3,   0.5,   0.8,     0);
  m[n].body = b;
  m[n].time = 1200;
  n++;
  
  BodyPos(b,60,RAD(20));
  LegAng  (b,0,   1.8,   1.5,   0.3);
  LegAng  (b,1,-0.426,   0.8,  0.95);
  LegAng  (b,2,  -0.3,   0.2,   2.5);
  LegAng  (b,3,   0.5,   0.8,     0);
  m[n].body = b;
  m[n].time = 0;
  n++;

#if 0  
  // arm fully stuck out, down on ground (orig) PER 7/1/04
  BodyPos(b,60,RAD(11));
  LegAng  (b,0,   1.8,     0,   0.3);
  LegAng  (b,1,   1.5,   0.5,   1.2);
  LegAng  (b,2,  -0.3,   0.2,   2.5);
  LegAng  (b,3,   0.5,   0.8,     0);
  m[n].body = b;
  m[n].time = 800;
  n++;
  
  BodyPos(b,60,RAD(11));
  LegAng  (b,0,   1.8,     0,   0.3);
  LegAng  (b,1,   1.5,   0.5,   1.2);
  LegAng  (b,2,  -0.3,   0.2,   2.5);
  LegAng  (b,3,   0.5,   0.8,     0);
  m[n].body = b;
  m[n].time = 0;
  n++;
#else
  // arm fully stuck out, down on ground (new) PER 7/1/04
  BodyPos(b,60,RAD(11));
  LegAng  (b,0,   1.3,   0.2,   1.8);
  LegAng  (b,1,   1.3,   0.2,   1.8);
  LegAng  (b,2,  -0.3,   0.2,   2.5);
  LegAng  (b,3,   0.5,   0.2,   2.5);
  m[n].body = b;
  m[n].time = 300;
  n++;
  
  BodyPos(b,60,RAD(11));
  LegAng  (b,0,   1.3,   0.2,   1.8);
  LegAng  (b,1,   1.3,   0.2,   1.8);
  LegAng  (b,2,  -0.3,   0.3,   2.5);
  LegAng  (b,3,  -0.3,   0.3,   2.5);
  m[n].body = b;
  m[n].time = 600;
  n++;
#endif

  // getting back up  
  BodyPos(b,100.0,RAD(17.5));
  LegAng  (b,0,-0.426, 0.356,  1.55);
  LegAng  (b,1,-0.426, 0.356,  1.55);
  LegAng  (b,2,-0.662, 0.456, 1.784);
  LegAng  (b,3,-0.662, 0.456, 1.784);
  m[n].body = b;
  m[n].time = 400; // 800 was old time PER 7/1/04
  n++;
  
  BodyPos(b,100.0,RAD(17.5));
  LegAng  (b,0,-0.426, 0.356,  1.55);
  LegAng  (b,1,-0.426, 0.356,  1.55);
  LegAng  (b,2,-0.662, 0.456, 1.784);
  LegAng  (b,3,-0.662, 0.456, 1.784);
  m[n].body = b;
  m[n].time = 0;
  n++;

  out = fopen("b_gdive.mot","wb");
  fwrite(&n,sizeof(n),1,out);
  fwrite(m,sizeof(Motion::BodyStateMotion),n,out);
  fclose(out);

  //==== Supporter Dive Block / Catch ====//

  mzero(b);

  n = 0;
  m[n].body = b;
  m[n].time = 100;
  n++;

  BodyPos(b,100.0,RAD(17.5));
  LegAng  (b,0,   1.2,   1.5,     0);
  LegAng  (b,1,-0.426, 0.356,  1.55);
  LegAng  (b,2,-0.662, 0.456, 1.784);
  LegAng  (b,3,-0.662, 0.456, 1.784);
  m[n].body = b;
  m[n].time = 80;
  n++;

  BodyPos(b,100.0,RAD(17.5));
  LegAng  (b,0,   1.2,   1.5,     0);
  LegAng  (b,1,-0.426, 0.356,  1.55);
  LegAng  (b,2,-0.662, 0.456, 1.784);
  LegAng  (b,3,-0.662, 0.456, 1.784);
  m[n].body = b;
  m[n].time = 0;
  n++;

  // get arm stuck out fully
  BodyPos(b,60,RAD(20));
  LegAng  (b,0,   1.8,   1.5,   0.3);
  LegAng  (b,1,-0.426,   0.8,  0.95);
  LegAng  (b,2,  -0.3,   0.2,   2.5);
  LegAng  (b,3,   0.5,   0.8,     0);
  m[n].body = b;
  m[n].time = 1200;
  n++;

  // stay out for a while
  BodyPos(b,60,RAD(20));
  LegAng  (b,0,   1.8,   1.5,   0.3);
  LegAng  (b,1,-0.426,   0.8,  0.95);
  LegAng  (b,2,  -0.3,   0.2,   2.5);
  LegAng  (b,3,   0.5,   0.8,     0);
  m[n].body = b;
  m[n].time = 0;
  n++;
 
  // get up slowly, trying NOT to kick the ball away
  BodyPos(b,100.0,RAD(17.5));
  LegAng  (b,0,-0.426, 0.356,  1.55);
  LegAng  (b,1,-0.426, 0.356,  1.55);
  LegAng  (b,2,-0.662, 0.456, 1.784);
  LegAng  (b,3,-0.662, 0.456, 1.784);
  m[n].body = b;
  m[n].time = 0;
  n++;
    
  out = fopen("b_sdive.mot","wb");
  fwrite(&n,sizeof(n),1,out);
  fwrite(m,sizeof(Motion::BodyStateMotion),n,out);
  fclose(out);

}



void make_fun_motions()
{
  Motion::BodyStateMotion m[96];
  Motion::BodyState b;
  RobotConstants rc;
  FILE *out;
  int i,n;
  int t;

  //==== Warmup (Pushups) ====//
  mzero(b);
  mzero(m,96);
  t = 1;

  n = 0;
  m[n].body = b;
  m[n].time = 200;
  n++;

  LegAng(b,0, -0.2, 0.3, 1.7);
  LegAng(b,1, -0.2, 0.3, 1.7);
  LegAng(b,2, -1.1, 0.3, 2.0);
  LegAng(b,3, -1.1, 0.3, 2.0);
  m[n].body = b;
  m[n].time = 500;
  n++;

  //up down 10 times
  for(i = 0; i < 10; i++){
    //up
    LegAng(b,0, -0.5, 0.2, 1.3);
    LegAng(b,1, -0.5, 0.2, 1.3);
    LegAng(b,2, .7, -0.03, 0.4);
    LegAng(b,3, .7, -0.03, 0.4);
    m[n].body = b;
    m[n].time = 500;
    n++;
    //down
    LegAng(b,0, -0.5, 0.2, 2.3);
    LegAng(b,1, -0.5, 0.2, 2.3);
    LegAng(b,2, 1.0, -0.03, 0.4);
    LegAng(b,3, 1.0, -0.03, 0.4);
    m[n].body = b;
    m[n].time = 500;
    n++;
  }

  out = fopen("warmup.mot","wb");
  fwrite(&n,sizeof(n),1,out);
  fwrite(m,sizeof(Motion::BodyStateMotion),n,out);
  fclose(out);


  //==== Climb into Field ====//
#define SHOULD_FLAIL 0
  mzero(b);
  
  n = 0;
  m[n].body = b;
  m[n].time = 1000;
  n++;

  // sit down
  LegAng(b, 0, -0.253956, -0.131986, 0.470636);
  LegAng(b, 1, -0.257699, -0.139626, 0.478557);
  LegAng(b, 2, -1.160850, -0.168714, 2.277713);
  LegAng(b, 3, -1.167226, -0.162013, 2.273294);
  HeadAng(b, 0.0, 0.0, 0.0);
  m[n].body = b;
  m[n].time = 300;
  n++;
  
  LegAng(b, 0, -0.263956, -0.141986, 0.484838);
  LegAng(b, 1, -0.269413, -0.145443, 0.484187);
  LegAng(b, 2, -1.166392, -0.168714, 2.271955);
  LegAng(b, 3, -1.165705, -0.162013, 2.274890);
  HeadAng(b, 0.0,0.0,0.0);
  m[n].body = b;
  m[n].time = 300;
  n++;
  
  
  LegAng(b, 0, -0.853081, -0.133808, 0.237025);
  LegAng(b, 1, -0.855093, -0.139626, 0.230833);
  LegAng(b, 2, -1.743007, 0.412529, 2.565634);
  LegAng(b, 3, -1.745343, 0.412627, 2.565634);
  HeadAng(b, -0.3, 0.0, 0.0);
  m[n].body = b;
  m[n].time = 300;
  n++;
  
  LegAng(b, 0, -0.848842, -0.133808, 0.231227);
  LegAng(b, 1, -0.849236, -0.139626, 0.230833);
  LegAng(b, 2, -1.747465, 0.418229, 2.565634);
  LegAng(b, 3, -1.744146, 0.415625, 2.565634);
  HeadAng(b, -0.5, 0.0, 0.0);
  m[n].body = b;
  m[n].time = 500;
  n++;
  
  // sitting position - hold for 5 seconds
  LegAng(b, 0, -0.85, -0.14, 0.23);
  LegAng(b, 1, -0.85, -0.14, 0.23);
  LegAng(b, 2, -1.40, 0.42, 2.56);
  LegAng(b, 3, -1.40, 0.42, 2.56);
  HeadAng(b, -0.5, 0.0, 0.0);
  m[n].body = b;
  m[n].time = 5000;
  n++;

  m[n].body = b;
  m[n].time = 500;
  n++;


  // bend left leg at elbow
  LegAng(b, 0, -1.29, -.16, 2.56);
  LegAng(b, 1, -0.85, -0.14, 0.23);
  LegAng(b, 2, -1.40, 0.42, 2.56);
  LegAng(b, 3, -1.40, 0.42, 2.56);
  HeadAng(b, -0.5, 0.0, 0.0);
  m[n].body = b;
  m[n].time = 300;
  n++;


  // rotate left leg up
  LegAng(b, 0, 1, -.16, 2);
  LegAng(b, 1, -0.85, -0.14, 0.23);
  LegAng(b, 2, -1.40, 0.42, 2.56);
  LegAng(b, 3, -1.40, 0.42, 2.56);
  HeadAng(b, -0.5, 0.0, 0.0);
  m[n].body = b;
  m[n].time = 300;
  n++;


  // extend left paw over wall
  LegAng(b, 0, 1, -.16, .5);
  LegAng(b, 1, -0.85, -0.14, 0.23);
  LegAng(b, 2, -1.40, 0.42, 2.56);
  LegAng(b, 3, -1.40, 0.42, 2.56);
  HeadAng(b, -0.5, 0.0, 0.0);
  m[n].body = b;
  m[n].time = 200;
  n++;

  // lower left leg onto wall
  LegAng(b, 0, .48, -.16, 0);
  LegAng(b, 1, -0.85, -0.14, 0.23);
  LegAng(b, 2, -1.40, 0.42, 2.56);
  LegAng(b, 3, -1.40, 0.42, 2.56);
  HeadAng(b, -0.5, 0.0, 0.0);
  m[n].body = b;
  m[n].time = 400;
  n++;

  // bend right leg at elbow
  LegAng(b, 0, .48, -.16, 0);
  LegAng(b, 1, -1.29, -.16, 2.56);
  LegAng(b, 2, -1.40, 0.42, 2.56);
  LegAng(b, 3, -1.40, 0.42, 2.56);
  HeadAng(b, -0.5, 0.0, 0.0);
  m[n].body = b;
  m[n].time = 300;
  n++;


  // rotate right leg up
  LegAng(b, 0, .48, -.16, 0);
  LegAng(b, 1, 1, -.16, 2);
  LegAng(b, 2, -1.40, 0.42, 2.56);
  LegAng(b, 3, -1.40, 0.42, 2.56);
  HeadAng(b, -0.5, 0.0, 0.0);
  m[n].body = b;
  m[n].time = 300;
  n++;


  // extend right leg over wall
  LegAng(b, 0, .48, -.16, 0);
  LegAng(b, 1, 1, -.16, .5);
  LegAng(b, 2, -1.40, 0.42, 2.56);
  LegAng(b, 3, -1.40, 0.42, 2.56);
  HeadAng(b, -0.5, 0.0, 0.0);
  m[n].body = b;
  m[n].time = 200;
  n++;

  // lower right leg onto wall
  LegAng(b, 0, .48, -.16, 0);
  LegAng(b, 1, .48, -.16, 0);
  LegAng(b, 2, -1.40, 0.42, 2.56);
  LegAng(b, 3, -1.40, 0.42, 2.56);
  HeadAng(b, -0.5, 0.0, 0.0);
  m[n].body = b;
  m[n].time = 500;
  n++;

  // lift up back legs from ground - robot sitting on rear
  LegAng(b, 0, .48, -.16, 0);
  LegAng(b, 1, .48, -.16, 0);
  LegAng(b, 2, -1.6, 0.42, 2.56);
  LegAng(b, 3, -1.6, 0.42, 2.56);
  HeadAng(b, -0.5, 0.0, 0.0);
  m[n].body = b;
  m[n].time = 500;
  n++;
  
  // extends back legs forward
  LegAng(b, 0, .48, -.16, 0);
  LegAng(b, 1, .48, -.16, 0);
  LegAng(b, 2, -.9, 0.42, 0);
  LegAng(b, 3, -.9, 0.42, 0);
  HeadAng(b, -0.5, 0.0, 0.0);
  m[n].body = b;
  m[n].time = 500;
  n++;

  // Swing legs out to sides
  LegAng(b, 0, .48, -.16, 0);
  LegAng(b, 1, .48, -.16, 0);
  LegAng(b, 2, rc.rotator_min, rc.shoulder_max, 0);
  LegAng(b, 3, rc.rotator_min, rc.shoulder_max, 0);
  HeadAng(b, -0.5, 0.0, 0.0);
  m[n].body = b;
  m[n].time = 500;
  n++;

  // bend knees 
  LegAng(b, 0, .48, -.16, 0);
  LegAng(b, 1, .48, -.16, 0);
  LegAng(b, 2, rc.rotator_min, rc.shoulder_max, rc.knee_max);
  LegAng(b, 3, rc.rotator_min, rc.shoulder_max, rc.knee_max);
  HeadAng(b, rc.head_tilt_min, 0.0, 0.0);
  m[n].body = b;
  m[n].time = 1000;
  n++;

  // push forward slightly by rotating bent legs down
  LegAng(b, 0, .48, -.16, 0);
  LegAng(b, 1, .48, -.16, 0);
  LegAng(b, 2, -1.57, rc.shoulder_max, rc.knee_max);
  LegAng(b, 3, -1.57, rc.shoulder_max, rc.knee_max);
  HeadAng(b, rc.head_tilt_min, 0.0, 0.0);
  m[n].body = b;
  m[n].time = 500;
  n++;

  // tuck knees in (gets legs beneath bot)
  LegAng(b, 0, .48, -.16, 0);
  LegAng(b, 1, .48, -.16, 0);
  LegAng(b, 2, -.87, .2, rc.knee_max);
  LegAng(b, 3, -.87, .2, rc.knee_max);
  HeadAng(b, rc.head_tilt_min, 0.0, 0.0);
  m[n].body = b;
  m[n].time = 2000;
  n++;

  // stand up
  LegAng(b, 0, .48, -.16, 0);
  LegAng(b, 1, .48, -.16, 0);
  LegAng(b, 2, .3, 0, .3);
  LegAng(b, 3, .3, 0, .3);
  HeadAng(b, rc.head_tilt_min, 0.0, 0.0);
  m[n].body = b;
  m[n].time = 500;
  n++;


  // pull self over with arms
  LegAng(b, 0, -.2, 0, rc.knee_min);
  LegAng(b, 1, -.2, 0, rc.knee_min);
  LegAng(b, 2, .3, 0, .3);
  LegAng(b, 3, .3, 0, .3);
  HeadAng(b, rc.head_tilt_min, 0.0, 0.0);
  m[n].body = b;
  m[n].time = 750;
  n++;

  // ben forearms to let self down
  LegAng(b, 0, .34, 0, rc.knee_max);
  LegAng(b, 1, .34, 0, rc.knee_max);
  LegAng(b, 2, 1, 0, 1);
  LegAng(b, 3, 1, 0, 1);
  HeadAng(b, rc.head_tilt_min, 0.0, 0.0);
  m[n].body = b;
  m[n].time = 500;
  n++;

  // slide forward
  LegAng(b, 0, .95, 0, 1.22);
  LegAng(b, 1, .95, 0, 1.22);
  LegAng(b, 2, 1.57, 0, 1.57);
  LegAng(b, 3, 1.57, 0, 1.57);
  HeadAng(b, rc.head_tilt_min, 0.0, 0.0);
  m[n].body = b;
  m[n].time = 500;
  n++;

#if (SHOULD_FLAIL) 
  for (i = 0; i < 3; i++) {
    // FLAIL
    LegAng(b, 2, rc.rotator_max, 0, rc.knee_max/2);
    LegAng(b, 3, .5, 0, 0);
    HeadAng(b, rc.head_tilt_max, rc.head_pan_min/3, rc.head_roll_min/3);
    m[n].body = b;
    m[n].time = 500;
    n++;
    
    LegAng(b, 2, .5, 0, 0);
    LegAng(b, 3, rc.rotator_max, 0, rc.knee_max/2);
    HeadAng(b, rc.head_tilt_max, rc.head_pan_min, rc.head_roll_min);
    m[n].body = b;
    m[n].time = 350;
    n++;
    
    LegAng(b, 2, rc.rotator_max, 0, rc.knee_max/2);
    LegAng(b, 3, .5, 0, 0);
    HeadAng(b, rc.head_tilt_max, rc.head_pan_min/3, rc.head_roll_min/3);
    m[n].body = b;
    m[n].time = 350;
    n++;
    
    LegAng(b, 2, .5, 0, 0);
    LegAng(b, 3, rc.rotator_max, 0, rc.knee_max/2);
    HeadAng(b, rc.head_tilt_max, rc.head_pan_max/3, rc.head_roll_max/3);
    m[n].body = b;
    m[n].time = 350;
    n++;
    
    LegAng(b, 2, rc.rotator_max, 0, 0);
    LegAng(b, 3, .5, 0, knee_max/2);
    HeadAng(b, rc.head_tilt_max, rc.head_pan_max, rc.head_roll_max);
    m[n].body = b;
    m[n].time = 350;
    n++;
    
    LegAng(b, 2, .5, 0, 0);
    LegAng(b, 3, rc.rotator_max, 0, rc.knee_max/2);
    HeadAng(b, rc.head_tilt_max, rc.head_pan_max/3, rc.head_roll_max/3);
    m[n].body = b;
    m[n].time = 350;
    n++;
  }

  LegAng(b, 2, rc.rotator_max, 0, rc.knee_max/2);
  LegAng(b, 3, rc.rotator_max, 0, 0);
  HeadAng(b, rc.head_tilt_max, rc.head_pan_min/3, rc.head_roll_min/3);
  m[n].body = b;
  m[n].time = 400;
  n++;
    
  
  LegAng(b, 2, rc.rotator_max, rc.shoulder_max, 0);
  LegAng(b, 3, rc.rotator_max, rc.shoulder_max, 0);
  HeadAng(b, rc.head_tilt_max, 0, 0);
  m[n].body = b;
  m[n].time = 200;
  n++;
  // End Flail
#endif

  // bring legs straight out to the sides - to clear wall
  LegAng(b, 0, .95, 0, 1.22);
  LegAng(b, 1, .95, 0, 1.22);
  LegAng(b, 2, 0, rc.shoulder_max, 0);
  LegAng(b, 3, 0, rc.shoulder_max, 0);
  HeadAng(b, rc.head_tilt_max, 0.0, 0.0);
  m[n].body = b;
  m[n].time = 250;
  n++;

  LegAng(b, 2, rc.rotator_min, rc.shoulder_max, 0);
  LegAng(b, 3, rc.rotator_min, rc.shoulder_max, 0);
  m[n].body = b;
  m[n].time = 400;
  n++;

  // bring forward
  LegAng(b, 0, .95, 0, 1.22);
  LegAng(b, 1, .95, 0, 1.22);
  LegAng(b, 2, rc.rotator_min, .78, 0);
  LegAng(b, 3, rc.rotator_min, .78, 0);
  HeadAng(b, rc.head_tilt_max, 0.0, 0.0);
  m[n].body = b;
  m[n].time = 400;
  n++;

  // bend knees under halfway
  LegAng(b, 0, .95, 0, 1.22);
  LegAng(b, 1, .95, 0, 1.22);
  LegAng(b, 2, rc.rotator_min, .78, 1.57);
  LegAng(b, 3, rc.rotator_min, .78, 1.57);
  HeadAng(b, rc.head_tilt_max, 0.0, 0.0);
  m[n].body = b;
  m[n].time = 300;
  n++;

  // bend knees under all the way
  LegAng(b, 0, 1.22, 0, .57);
  LegAng(b, 1, 1.22, 0, .57);
  LegAng(b, 2, rc.rotator_min, .26, rc.knee_max);
  LegAng(b, 3, rc.rotator_min, .26, rc.knee_max);
  HeadAng(b, rc.head_tilt_max, 0.0, 0.0);
  m[n].body = b;
  m[n].time = 300;
  n++;

  // push forward
  LegAng(b, 0, 1.22, 0, .57);
  LegAng(b, 1, 1.22, 0, .57);
  LegAng(b, 2, -1.22, .26, rc.knee_max);
  LegAng(b, 3, -1.22, .26, rc.knee_max);
  HeadAng(b, rc.head_tilt_max, 0.0, 0.0);
  m[n].body = b;
  m[n].time = 750;
  n++;

  // extend
  LegAng(b, 0, 1.22, 0, .57);
  LegAng(b, 1, 1.22, 0, .57);
  LegAng(b, 2, 0, .26, .61);
  LegAng(b, 3, 0, .26, .61);
  HeadAng(b, rc.head_tilt_max, 0.0, 0.0);
  m[n].body = b;
  m[n].time = 5000;
  n++;
  
  LegAng(b, 0, 1.22, 0, .57);
  LegAng(b, 1, 1.22, 0, .57);
  LegAng(b, 2, 0, .26, .61);
  LegAng(b, 3, 0, .26, .61);
  HeadAng(b, rc.head_tilt_max, 0.0, 0.0);
  m[n].body = b;
  m[n].time = 10;
  n++;

  //
  out = fopen("enter_field.mot", "wb");
  fwrite(&n, sizeof(n), 1, out);
  fwrite(m, sizeof(Motion::BodyStateMotion), n, out);
  fclose(out);

  //==== Chicken Dance ====//
  mzero(b);
  
  n = 0;
  m[n].body = b;
  m[n].time = 200;
  n++;

  LegAng(b, 0, -0.253956, -0.131986, 0.470636);
  LegAng(b, 1, -0.257699, -0.139626, 0.478557);
  LegAng(b, 2, -1.160850, -0.168714, 2.277713);
  LegAng(b, 3, -1.167226, -0.162013, 2.273294);
  HeadAng(b, 0.0, 0.0, 0.0);
  m[n].body = b;
  m[n].time = 300;
  n++;
  
  LegAng(b, 0, -0.263956, -0.141986, 0.484838);
  LegAng(b, 1, -0.269413, -0.145443, 0.484187);
  LegAng(b, 2, -1.166392, -0.168714, 2.271955);
  LegAng(b, 3, -1.165705, -0.162013, 2.274890);
  HeadAng(b, 0.0,0.0,0.0);
  m[n].body = b;
  m[n].time = 300;
  n++;
  
  
  LegAng(b, 0, -0.853081, -0.133808, 0.237025);
  LegAng(b, 1, -0.855093, -0.139626, 0.230833);
  LegAng(b, 2, -1.743007, 0.412529, 2.565634);
  LegAng(b, 3, -1.745343, 0.412627, 2.565634);
  HeadAng(b, -0.3, 0.0, 0.0);
  m[n].body = b;
  m[n].time = 300;
  n++;
  
  LegAng(b, 0, -0.848842, -0.133808, 0.231227);
  LegAng(b, 1, -0.849236, -0.139626, 0.230833);
  LegAng(b, 2, -1.747465, 0.418229, 2.565634);
  LegAng(b, 3, -1.744146, 0.415625, 2.565634);
  HeadAng(b, -0.5, 0.0, 0.0);
  m[n].body = b;
  m[n].time = 700;
  n++;
  //arm stuff;
  
  // start flap
  LegAng(b, 0, -1.248842, -0.193808, 0.230833);
  LegAng(b, 1, -1.249236, -0.199626, 0.230833);
  LegAng(b, 2, -0.8518614, 0.215208, 1.7508878);
  LegAng(b, 3, -0.8512491, 0.214532, 1.75005634);
  HeadAng(b, -0.5, 0.0, 0.0);
  m[n].body = b;
  m[n].time = 300;
  n++;
  
  // bring arms up.
  LegAng(b, 0, -.7, .3, 2.5);
  LegAng(b, 1, -.7, .3, 2.5);
  LegAng(b, 2, -0.8518614, 0.215208, 1.7508878);
  LegAng(b, 3, -0.8512491, 0.214532, 1.75005634);
  HeadAng(b, -0.5, 0.0, 0.0);
  m[n].body = b;
  m[n].time = 300;
  n++;
  
  for (i = 0; i < 7; i++) {    
    // flap up
    LegAng(b, 0, -1.3, rc.shoulder_max, 2.5);
    LegAng(b, 1, -1.3, rc.shoulder_max, 2.5);
    LegAng(b, 2, -0.8518614, 0.215208, 1.7508878);
    LegAng(b, 3, -0.8512491, 0.214532, 1.75005634);
    HeadAng(b, -0.6, 0.0, 0.0);
    m[n].body = b;
    m[n].time = 300;
    n++;
    
    // flap down
    LegAng(b, 0, -1.3, .69, 2.5);
    LegAng(b, 1, -1.3, .69, 2.5);
    LegAng(b, 2, -0.8518614, 0.215208, 1.7508878);
    LegAng(b, 3, -0.8512491, 0.214532, 1.75005634);
    HeadAng(b, -0.87, 0.0, 0.0);
    m[n].body = b;
    m[n].time = 400;
    n++;
  }

  // hold position
  m[n].body = b;
  m[n].time = 2000;
  n++;

  m[n].body = b;
  m[n].time = 1000;
  n++;
    
  out = fopen("chicken_dance.mot","wb");
  fwrite(&n,sizeof(n),1,out);
  fwrite(m,sizeof(Motion::BodyStateMotion),n,out);
  fclose(out);

  //==== Victory Dance 1 ====//
  mzero(b);

  n = 0;
  m[n].body = b;
  m[n].time = 200;
  n++;

  LegAng(b, 0, -0.253956, -0.131986, 0.470636);
  LegAng(b, 1, -0.257699, -0.139626, 0.478557);
  LegAng(b, 2, -1.160850, -0.168714, 2.277713);
  LegAng(b, 3, -1.167226, -0.162013, 2.273294);
  HeadAng(b, 0.0, 0.0, 0.0);
  m[n].body = b;
  m[n].time = 300;
  n++;
  
  LegAng(b, 0, -0.263956, -0.141986, 0.484838);
  LegAng(b, 1, -0.269413, -0.145443, 0.484187);
  LegAng(b, 2, -1.166392, -0.168714, 2.271955);
  LegAng(b, 3, -1.165705, -0.162013, 2.274890);
  HeadAng(b, 0.0,0.0,0.0);
  m[n].body = b;
  m[n].time = 300;
  n++;
  
  
  LegAng(b, 0, -0.853081, -0.133808, 0.237025);
  LegAng(b, 1, -0.855093, -0.139626, 0.230833);
  LegAng(b, 2, -1.743007, 0.412529, 2.565634);
  LegAng(b, 3, -1.745343, 0.412627, 2.565634);
  HeadAng(b, -0.3, 0.0, 0.0);
  m[n].body = b;
  m[n].time = 300;
  n++;
  
  LegAng(b, 0, -0.848842, -0.133808, 0.231227);
  LegAng(b, 1, -0.849236, -0.139626, 0.230833);
  LegAng(b, 2, -1.747465, 0.418229, 2.565634);
  LegAng(b, 3, -1.744146, 0.415625, 2.565634);
  HeadAng(b, -0.5, 0.0, 0.0);
  m[n].body = b;
  m[n].time = 500;
  n++;
  ///arm stuff

  
  LegAng(b, 0, -1.248842, -0.193808, 0.231227);
  LegAng(b, 1, -1.249236, -0.199626, 0.230833);
  LegAng(b, 2, -0.8518614, 0.215208, 1.7508878);
  LegAng(b, 3, -0.8512491, 0.214532, 1.75005634);
  HeadAng(b, -0.5, 0.0, 0.0);
  m[n].body = b;
  m[n].time = 500;
  n++;

  LegAng(b, 0, 2.003999, -0.005817, 0.007958);
  LegAng(b, 1, 2.005711, -0.003084, 0.005634);
  LegAng(b, 2, -0.8518614, 0.215208, 1.7508878);
  LegAng(b, 3, -0.8512491, 0.214532, 1.75005634);
  HeadAng(b, -0.5, 0.0, 0.0);
  m[n].body = b;
  m[n].time = 500;
  n++;


  LegAng(b, 0, 0.05999, 0.005817, 2.607958);
  LegAng(b, 1, 0.055711, 0.003084, 2.605634);
  LegAng(b, 2, -0.8518614, 0.215208, 1.7508878);
  LegAng(b, 3, -0.8512491, 0.214532, 1.75005634);
  HeadAng(b, -0.5, 0.0, 0.0);
  m[n].body = b;
  m[n].time = 500;
  n++;


  LegAng(b, 0, 2.003999, -0.005817, 0.007958);
  LegAng(b, 1, 2.005711, -0.003084, 0.005634);
  LegAng(b, 2, -0.8518614, 0.215208, 1.7508878);
  LegAng(b, 3, -0.8512491, 0.214532, 1.75005634);
  HeadAng(b, -0.5, 0.0, 0.0);
  m[n].body = b;
  m[n].time = 500;
  n++;

  LegAng(b, 0, 0.0, -0.0, 2.6);
  LegAng(b, 1, 0.0, -0.0, 2.6);
  LegAng(b, 2, -0.85, 0.21, 1.750);
  LegAng(b, 3, -0.85, 0.21, 1.750);
  HeadAng(b, -0.5, 0.0, 0.0);
  m[n].body = b;
  m[n].time = 500;
  n++;
 

  out = fopen("dance1.mot","wb");
  fwrite(&n,sizeof(n),1,out);
  fwrite(m,sizeof(Motion::BodyStateMotion),n,out);
  fclose(out);


  //==== Victory Dance 2 ====//
  mzero(b);

  n = 0;
  m[n].body = b;
  m[n].time = 200;
  n++;

  LegAng(b, 0, -0.253956, -0.131986, 0.470636);
  LegAng(b, 1, -0.257699, -0.139626, 0.478557);
  LegAng(b, 2, -1.160850, -0.168714, 2.277713);
  LegAng(b, 3, -1.167226, -0.162013, 2.273294);
  HeadAng(b, 0.0, 0.0, 0.0);
  m[n].body = b;
  m[n].time = 300;
  n++;
  
  LegAng(b, 0, -0.263956, -0.141986, 0.484838);
  LegAng(b, 1, -0.269413, -0.145443, 0.484187);
  LegAng(b, 2, -1.166392, -0.168714, 2.271955);
  LegAng(b, 3, -1.165705, -0.162013, 2.274890);
  HeadAng(b, 0.0,0.0,0.0);
  m[n].body = b;
  m[n].time = 300;
  n++;
  
  
  LegAng(b, 0, -0.853081, -0.133808, 0.237025);
  LegAng(b, 1, -0.855093, -0.139626, 0.230833);
  LegAng(b, 2, -1.743007, 0.412529, 2.565634);
  LegAng(b, 3, -1.745343, 0.412627, 2.565634);
  HeadAng(b, -0.3, 0.0, 0.0);
  m[n].body = b;
  m[n].time = 300;
  n++;
  
  LegAng(b, 0, -0.848842, -0.133808, 0.231227);
  LegAng(b, 1, -0.849236, -0.139626, 0.230833);
  LegAng(b, 2, -1.747465, 0.418229, 2.565634);
  LegAng(b, 3, -1.744146, 0.415625, 2.565634);
  HeadAng(b, -0.5, 0.0, 0.0);
  m[n].body = b;
  m[n].time = 500;
  n++;
  ///arm stuff

  
  LegAng(b, 0, -1.248842, -0.193808, 0.231227);
  LegAng(b, 1, -1.249236, -0.199626, 0.230833);
  LegAng(b, 2, -0.8518614, 0.215208, 1.7508878);
  LegAng(b, 3, -0.8512491, 0.214532, 1.75005634);
  HeadAng(b, -0.5, 0.0, 0.0);
  m[n].body = b;
  m[n].time = 500;
  n++;

  for(i = 0; i < 2; i++){
  LegAng(b, 0, 2.003999, 0.7, 0.007958);//up
  LegAng(b, 1, 0.05999, 0.005817, 2.607958);//down
  LegAng(b, 2, -0.8518614, 0.215208, 1.7508878);
  LegAng(b, 3, -0.8512491, 0.214532, 1.75005634);
  HeadAng(b, -0.5, 0.0, 0.0);
  m[n].body = b;
  m[n].time = 500;
  n++;


  LegAng(b, 0, 0.05999, 0.005817, 2.607958);//down
  LegAng(b, 1, 2.003999, 0.7, 0.007958);//up
  LegAng(b, 2, -0.8518614, 0.215208, 1.7508878);
  LegAng(b, 3, -0.8512491, 0.214532, 1.75005634);
  HeadAng(b, -0.5, 0.0, 0.0);
  m[n].body = b;
  m[n].time = 500;
  n++;
  }

  LegAng(b, 0, 2.003999, -0.005817, 0.007958);
  LegAng(b, 1, 2.005711, -0.003084, 0.005634);
  LegAng(b, 2, -0.8518614, 0.215208, 1.7508878);
  LegAng(b, 3, -0.8512491, 0.214532, 1.75005634);
  HeadAng(b, -0.5, 0.0, 0.0);
  m[n].body = b;
  m[n].time = 500;
  n++;

  LegAng(b, 0, 0.0, -0.0, 2.6);
  LegAng(b, 1, 0.0, -0.0, 2.6);
  LegAng(b, 2, -0.85, 0.21, 1.750);
  LegAng(b, 3, -0.85, 0.21, 1.750);
  HeadAng(b, -0.5, 0.0, 0.0);
  m[n].body = b;
  m[n].time = 500;
  n++;
 

  out = fopen("dance2.mot","wb");
  fwrite(&n,sizeof(n),1,out);
  fwrite(m,sizeof(Motion::BodyStateMotion),n,out);
  fclose(out);

  //==== Victory Dance 3 ====//
  mzero(b);

  n = 0;
  m[n].body = b;
  m[n].time = 200;
  n++;

  LegAng(b, 0, 0.9, -0.2, 0.5);
  LegAng(b, 1, 0.9, -0.2, 0.5);
  LegAng(b, 2, -1.2, 0.2, 2.0);
  LegAng(b, 3, -1.2, 0.2, 2.0);
  HeadAng(b, 0.3,0,0);
  MouthAng(b,-.7);
  m[n].body = b;
  m[n].time = 200;
  n++;
  
  for(i = 0; i < 10;i++){
    LegAng(b, 0, 0.9, -0.2, 0.5);
    LegAng(b, 1, 0.9, -0.2, 0.5);
    LegAng(b, 2, -1.2, 0.4, 2.0);
    LegAng(b, 3, -1.2, -0.1, 2.0);
    HeadAng(b, 0.3,0,0);
    m[n].body = b;
    m[n].time = 200;
    n++;
  
    LegAng(b, 0, 0.9, -0.2, 0.5);
    LegAng(b, 1, 0.9, -0.2, 0.5);
    LegAng(b, 2, -1.0, -0.1, 2.0);
    LegAng(b, 3, -1.0, 0.4, 2.0);
    HeadAng(b, 0.3,0,0);
    m[n].body = b;
    m[n].time = 200;
    n++;
  }
 
  out = fopen("dance3.mot","wb");
  fwrite(&n,sizeof(n),1,out);
  fwrite(m,sizeof(Motion::BodyStateMotion),n,out);
  fclose(out);



  //==== Sad ====//
  mzero(b);

  n = 0;
  m[n].body = b;
  m[n].time = 200;
  n++;

  for(i = 0; i < 5;i++){
    BodyPos(b,100.0,RAD(17.5));
    LegAng(b, 0, 0.7, 0.3, 1.2);
    LegAng(b, 1, 0.7, 0.3, 1.2);
    LegPos(b,2,  -90,  77,0);
    LegPos(b,3,  -90, -77,0);  
    HeadAng(b, -1.0,1.0,0);
    m[n].body = b;
    m[n].time = 1000;
    n++;
    

    HeadAng(b, -1.0,-1.0,0);
    m[n].body = b;
    m[n].time = 1000;
    n++;
  }
 
  out = fopen("sad.mot","wb");
  fwrite(&n,sizeof(n),1,out);
  fwrite(m,sizeof(Motion::BodyStateMotion),n,out);
  fclose(out);


}

void make_skateboard_motions()
{
  Motion::BodyStateMotion m[64];
  Motion::BodyState b;
  FILE *out;
  int n;

  //====================================================//
  //==== Get on skate board (right side, kinematic) ====//
  //====================================================//
  double x,y,z;

  static const double rx_y = 20.0;

  mzero(b);

  n = 0;
  m[n].body = b;
  m[n].time = 2000;
  n++;

  // ** stand tall
  HeadAng(b, 0.5,-1.5, 0.0);
  BodyPos(b, 135, 0.0);
  x = 0.0;
  y = 0.0;
  LegPos(b,0, 59.5-x, 59.2-y, 0.0);
  LegPos(b,1, 59.5-x,-59.2-y, 0.0);
  LegPos(b,2,-59.5-x, 59.2-y, 0.0);
  LegPos(b,3,-59.5-x,-59.2-y, 0.0);
  m[n].body = b;
  m[n].time = 1000;
  n++;
  
  // ** move right back leg in
  // shift left and forward
  x = 20.0;
  y = 30.0;
  LegPos(b,0, 59.5-x, 59.2-y, 0.0);
  LegPos(b,1, 59.5-x,-59.2-y, 0.0);
  LegPos(b,2,-59.5-x, 59.2-y, 0.0);
  LegPos(b,3,-59.5-x,-59.2-y, 0.0);
  m[n].body = b;
  m[n].time = 800;
  n++;

  // lift right back leg
  z = 40.0;
  LegPos(b,3,-59.5-x,-59.2-y, 0.0+z);
  m[n].body = b;
  m[n].time = 800;
  n++;
    
  // reposition inwards
  LegPos(b,3,-59.5-x,-59.2+rx_y-y, 0.0+z);
  m[n].body = b;
  m[n].time = 800;
  n++;
    
  // put right back leg down
  LegPos(b,3,-59.5-x,-59.2+rx_y-y, 0.0);
  m[n].body = b;
  m[n].time = 800;
  n++;
    
  // ** move right front leg in
  // shift backwards
  x = -30.0;
  LegPos(b,0, 59.5-x, 59.2     -y, 0.0);
  LegPos(b,1, 59.5-x,-59.2     -y, 0.0);
  LegPos(b,2,-59.5-x, 59.2     -y, 0.0);
  LegPos(b,3,-59.5-x,-59.2+rx_y-y, 0.0);
  m[n].body = b;
  m[n].time = 800;
  n++;
    
  // lift right front leg
  LegPos(b,1, 59.5-x,-59.2     -y, 0.0+z);
  m[n].body = b;
  m[n].time = 800;
  n++;
    
  // reposition inwards
  LegPos(b,1, 59.5-x,-59.2+rx_y-y, 0.0+z);
  m[n].body = b;
  m[n].time = 800;
  n++;
    
  // put right front leg down
  LegPos(b,1, 59.5-x,-59.2+rx_y-y, 0.0);
  m[n].body = b;
  m[n].time = 800;
  n++;
    
  // ** get left back leg on skate board
  // shift forwards and right
  // lower right front leg a little to help transfer weight
  x =  30.0;
  y = -10.0;
  z =  10.0;
  LegPos(b,0, 59.5-x, 59.2     -y, 0.0);
  LegPos(b,1, 59.5-x,-59.2+rx_y-y, 0.0+z);
  LegPos(b,2,-59.5-x, 59.2     -y, 0.0);
  LegPos(b,3,-59.5-x,-59.2+rx_y-y, 0.0);
  m[n].body = b;
  m[n].time = 1000;
  n++;
    
  // rotate left back leg backwards and bend knee
  LegAng(b,2, 1.5, 0.2, 2.3);
  m[n].body = b;
  m[n].time = 1000;
  n++;
  
  // rotate shoulder out to get leg over board
  LegAng(b,2, 1.5, 1.5, 2.3);
  m[n].body = b;
  m[n].time = 1000;
  n++;
    
  // rotate rc.rotator to get on good surface
  // this angle of rc.rotator locks knee behind back ridge
  //   and toes on back rubber
  LegAng(b,2,-0.2, 1.5, 2.3);
  m[n].body = b;
  m[n].time = 1000;
  n++;

  // ** transfer weight onto left back leg (on board)
  // shift left
  y = 20.0;
  LegPos(b,0, 59.5-x, 59.2     -y, 0.0);
  LegPos(b,1, 59.5-x,-59.2+rx_y-y, 0.0);
  LegPos(b,3,-59.5-x,-59.2+rx_y-y, 0.0);
  m[n].body = b;
  m[n].time = 1000;
  n++;

  // put left back leg down
  HeadAng(b, 0.5, 1.5, 0.0);
  LegAng(b,2,-0.2, 1.0, 2.3);
  m[n].body = b;
  m[n].time = 1000;
  n++;

  // ** reposition right back leg for better stability during transfer of weight
  //   off of left front leg
  // lift right back leg
  LegPos(b,3,-59.5     -x,-59.2+rx_y-y, 40.0);
  m[n].body = b;
  m[n].time = 1000;
  n++;

  static const double rb_x = -40.0;
    
  // reposition backwards
  LegPos(b,3,-59.5+rb_x-x,-59.2+rx_y-y, 40.0);
  m[n].body = b;
  m[n].time = 1000;
  n++;
    
  // set right back leg down
  LegPos(b,3,-59.5+rb_x-x,-59.2+rx_y-y, 0.0);
  m[n].body = b;
  m[n].time = 1000;
  n++;
    
  // ** get left front leg on skate board
  // shift back and left some
  x =   0.0;
  y =  10.0;
  z =  10.0;
  HeadAng(b, 0.5, -1.5, 0.0);
  LegPos(b,0, 59.5     -x, 59.2     -y, 0.0);
  LegPos(b,1, 59.5     -x,-59.2+rx_y-y, 0.0);
  LegPos(b,3,-59.5+rb_x-x,-59.2+rx_y-y, 0.0+z);
  m[n].body = b;
  m[n].time = 1000;
  n++;
    
  // lift left front foot
  LegPos(b,0, 59.5     -x, 59.2     -y,40.0);
  m[n].body = b;
  m[n].time = 1000;
  n++;
    
  // get as high as possible using backwards swinging motion
  // not using forward motion since can get stuck on board
  //   handle bars and front axle
  LegAng(b,0, -2.0,-0.1, 2.6);
  m[n].body = b;
  m[n].time = 1000;
  n++;

  // rotate shoulder out to get leg over board
  // bumps board in process
  LegAng(b,0,-2.0, 1.5, 2.6);
  m[n].body = b;
  m[n].time = 1000;
  n++;

  // level out leg over board using rotator
  LegAng(b,0, 0.0, 1.5, 2.6);
  m[n].body = b;
  m[n].time = 1000;
  n++;

  // get knee at a good angle for setting leg down
  LegAng(b,0, 0.0, 1.5, 2.3);
  m[n].body = b;
  m[n].time = 1000;
  n++;

  // ** transfer weight onto left front leg
  // raise right back leg so robot is level again
  x =   0.0;
  y =  10.0;
  z =  10.0;
  LegPos(b,1, 59.5     -x,-59.2+rx_y-y, 0.0);
  LegPos(b,3,-59.5+rb_x-x,-59.2+rx_y-y, 0.0);
  m[n].body = b;
  m[n].time = 1000;
  n++;
    
  // put left front leg down
  HeadAng(b, 0.5, 1.5, 0.0);
  LegAng(b,0, 0.0, 1.0, 2.3);
  m[n].body = b;
  m[n].time = 1000;
  n++;
  
  // ** unlock back left toes from back rubber of board
  // rotate leg backwards
  LegAng(b,2, 0.1, 1.0, 2.3);
  m[n].body = b;
  m[n].time = 1000;
  n++;
  
  // ** slide left
  // results in left legs sliding off to left of board
  //   body resting against side of board supported by
  //   extended right legs
  y =  80.0;
  LegPos(b,1, 59.5     -x,-59.2+rx_y-y, 0.0);
  LegPos(b,3,-59.5+rb_x-x,-59.2+rx_y-y, 0.0);
  m[n].body = b;
  m[n].time = 3000;
  n++;
    
  // ** lock left legs around board
  // extend knees
  LegAng(b,0, 0.0, 1.0, 1.3);
  LegAng(b,2, 0.1, 1.0, 1.3);
  m[n].body = b;
  m[n].time = 3000;
  n++;
    
  // stick legs out using shoulder
  LegAng(b,0, 0.0, 1.5, 1.3);
  LegAng(b,2, 0.1, 1.5, 1.3);
  m[n].body = b;
  m[n].time = 1000;
  n++;
    
  // rotate bottom of legs below board
  LegAng(b,0,-1.5, 1.5, 1.3);
  LegAng(b,2,-1.5, 1.5, 1.3);
  m[n].body = b;
  m[n].time = 1000;
  n++;
    
  // bend knees to lock around board
  LegAng(b,0,-1.5, 1.5, 1.7);
  LegAng(b,2,-1.5, 1.5, 1.9);
  m[n].body = b;
  m[n].time = 1000;
  n++;
    
  // ** move right legs inwards
  // rebase body position roughly on robot's actual position
  BodyPos(b, 115, 0.0);
  x =   0.0;
  y =  60.0;
  LegPos(b,1, 59.5     -x,-59.2-y, 0.0);
  LegPos(b,3,-59.5+rb_x-x,-59.2-y, 0.0);
  m[n].body = b;
  m[n].time = 1000;
  n++;

  // reposition right front leg inward
  // lift leg
  z =  40.0;
  LegPos(b,1, 59.5     -x,-59.2-y, 0.0+z);
  m[n].body = b;
  m[n].time = 1000;
  n++;

  // reposition inwards
  LegPos(b,1, 59.5     -x,-59.2, 0.0+z);
  m[n].body = b;
  m[n].time = 1000;
  n++;

  // set down
  LegPos(b,1, 59.5     -x,-59.2, 0.0);
  m[n].body = b;
  m[n].time = 1000;
  n++;

  // reposition right back leg inwards
  // lift leg
  LegPos(b,3,-59.5+rb_x-x,-59.2-y, 0.0+z);
  m[n].body = b;
  m[n].time = 1000;
  n++;

  // reposition inwards
  y = 0.0;
  LegPos(b,3,-59.5+rb_x-x,-59.2-y, 0.0+z);
  m[n].body = b;
  m[n].time = 1000;
  n++;

  // set down
  LegPos(b,3,-59.5+rb_x-x,-59.2-y, 0.0);
  m[n].body = b;
  m[n].time = 1000;
  n++;

  // stand tall
  BodyPos(b, 130, 0.0);
  LegPos(b,1, 59.5     -x,-59.2-y, 0.0);
  LegPos(b,3,-59.5+rb_x-x,-59.2-y, 0.0);
  m[n].body = b;
  m[n].time = 1000;
  n++;

  // ** get left legs on board
  // unlock knees
  LegAng(b,0,-1.5, 1.5, 1.3);
  LegAng(b,2,-1.5, 1.5, 1.3);
  m[n].body = b;
  m[n].time = 1000;
  n++;
    
  // rotate around so not under board
  LegAng(b,0, 0.0, 1.5, 1.3);
  LegAng(b,2, 0.1, 1.5, 1.3);
  m[n].body = b;
  m[n].time = 1000;
  n++;
    
  // bend knees to get feet over board
  LegAng(b,0, 0.1, 1.5, 2.3);
  LegAng(b,2, 0.1, 1.5, 2.3);
  m[n].body = b;
  m[n].time = 1000;
  n++;
  
  // rotate legs to lift left side of robot
  LegAng(b,0,-1.5, 1.5, 2.3);
  LegAng(b,2,-1.5, 1.5, 2.3);
  m[n].body = b;
  m[n].time = 1000;
  n++;
    
  // ** prepare to jump sideways
  // stand taller
  BodyPos(b, 145, 0.0);
  LegPos(b,1, 59.5     -x,-59.2-y, 0.0);
  LegPos(b,3,-59.5+rb_x-x,-59.2-y, 0.0);
  m[n].body = b;
  m[n].time = 1000;
  n++;

  // shift backward to even weight distribution
  x = 10.0;
  LegPos(b,1, 59.5     -x,-59.2-y, 0.0);
  LegPos(b,3,-59.5+rb_x-x,-59.2-y, 0.0);
  m[n].body = b;
  m[n].time = 1000;
  n++;

  // reposition left leg ends roughly level and near edge of board
  LegAng(b,0,-1.5, 1.5, 1.8);
  LegAng(b,2,-1.5, 1.5, 1.8);
  m[n].body = b;
  m[n].time = 1000;
  n++;
    
  m[n].body = b;
  m[n].time = 0;
  n++;

  // ** jump left onto board
  x = 0.0;
  y = 80.0;
  LegPos(b,1, 59.5     -x,-59.2-y, 0.0);
  LegPos(b,3,-59.5+rb_x-x,-59.2-y, 0.0);
  m[n].body = b;
  m[n].time = 400;
  n++;

  // stand tall to lock onto board
  BodyPos(b, 135, 0.0);
  static const double xf_y = 20.0;
  y = 0.0;
  LegPos(b,0, 59.5     -x, 59.2-xf_y-y, 0.0);
  LegPos(b,1, 59.5     -x,-59.2+xf_y-y, 0.0);
  LegPos(b,2,-59.5     -x, 59.2     -y, 0.0);
  LegPos(b,3,-59.5+rb_x-x,-59.2     -y, 0.0);
  m[n].body = b;
  m[n].time = 5000;
  n++;

  // ** settle
  // crouch down to settle onto board
  BodyPos(b, 115, 0.0);
  LegPos(b,0, 59.5     -x, 59.2-xf_y-y, 0.0);
  LegPos(b,1, 59.5     -x,-59.2+xf_y-y, 0.0);
  LegPos(b,2,-59.5     -x, 59.2     -y, 0.0);
  LegPos(b,3,-59.5+rb_x-x,-59.2     -y, 0.0);
  m[n].body = b;
  m[n].time = 1000;
  n++;
    
  // ** fine tune position
  // * push off of handle bars
  // pick front legs up
  BodyPos(b, 115, 0.0);
  x =   0.0;
  y =   0.0;
  z =  60.0;
  LegPos(b,0, 59.5+x, 59.2+y, z);
  LegPos(b,1, 59.5+x,-59.2-y, z);
  m[n].body = b;
  m[n].time = 1000;
  n++;

  // move front legs forward and level with shoulder
  x =  40.0;
  z = 115.0;
  LegPos(b,0, 59.5+x, 59.2+y, z);
  LegPos(b,1, 59.5+x,-59.2-y, z);
  m[n].body = b;
  m[n].time = 1000;
  n++;

  // push off handlebars
  x = 140.0;
  LegPos(b,0, 59.5+x, 59.2+y, z);
  LegPos(b,1, 59.5+x,-59.2-y, z);
  m[n].body = b;
  m[n].time = 1000;
  n++;

  // *** this is only needed for testing after this point

  // back off handlebars so that stand won't push off
  x = 40.0;
  LegPos(b,0, 59.5+x, 59.2+y, z);
  LegPos(b,1, 59.5+x,-59.2-y, z);
  m[n].body = b;
  m[n].time = 1000;
  n++;

  x = 40.0;
  z = 20.0;
  LegPos(b,0, 59.5+x, 59.2+y, z);
  LegPos(b,1, 59.5+x,-59.2-y, z);
  m[n].body = b;
  m[n].time = 1000;
  n++;

  m[n].body = b;
  m[n].time = 0;
  n++;

  out = fopen("sb_climb.mot","wb");
  fwrite(&n,sizeof(n),1,out);
  fwrite(m,sizeof(Motion::BodyStateMotion),n,out);
  fclose(out);

  //==== Stand  ====//
  // Stands with feet under shoulders
  // useful for testing kinmeatics
  {
    mzero(b);

    n = 0;
    m[n].body = b;

    // starting neutral position
    //BodyPos(b, 93, RAD(8.0));
    //LegPos(b,0, 120, 68, 0);
    //LegPos(b,1, 120,-68, 0);
    //LegPos(b,2,-100, 70, 0);
    //LegPos(b,3,-100,-70, 0);

    m[n].time = 1000;
    n++;
    BodyPos(b, 125, RAD(0.0));
    HeadAng(b, -1.31, 0.0, -.2618);
    LegPos(b,0, 65, 62.5, 0);
    LegPos(b,1, 65,-62.5, 0);
    LegPos(b,2,-65, 62.5, 0);
    LegPos(b,3,-65,-62.5, 0);
    m[n].body = b;

    m[n].time = 30000;
    n++;
    m[n].body = b;
    m[n].time = 0;
    n++;

    out = fopen("kin_test.mot","wb");
    fwrite(&n,sizeof(n),1,out);
    fwrite(m,sizeof(Motion::BodyStateMotion),n,out);
    fclose(out);
  }
  
  //==== Roll  ====//
  // Rolls to left
  // Probably not terribly useful, but fun.
  {
    int t;

    mzero(b);

    n = 0;
    m[n].body = b;

    t = 1;

    // starting neutral position
    //BodyPos(b, 93, RAD(8.0));
    //LegPos(b,0, 120, 68, 0);
    //LegPos(b,1, 120,-68, 0);
    //LegPos(b,2,-100, 70, 0);
    //LegPos(b,3,-100,-70, 0);

    float lean1=20.0;
    float so1=20.0; // side offset 1
    float sp =-20.0; // spread
    float popl=50.0;
    float popr=30.0;

    // rotate flat, raise up, and lean left
    m[n].time = 100;
    n++;
    BodyPos(b, 130, RAD(0.0));
    HeadAng(b, -1.31, 0.0, -.2618);
    LegPos(b,0, 120, 68-lean1, 0);
    LegPos(b,1,  90,-68-lean1, 0);
    LegPos(b,2,-100, 70-lean1, 0);
    LegPos(b,3,- 70,-70-lean1, 0);
    m[n].body = b;
  
    // lean right
    m[n].time = 100;
    n++;
    LegPos(b,0, 120, 68+so1, 0);
    LegPos(b,1,  90,-68+so1, 0);
    LegPos(b,2,-100, 70+so1, 0);
    LegPos(b,3,- 70,-70+so1, 0);
    m[n].body = b;
  
    // spread feet out by moving left feet
    m[n].time = 100;
    n++;
    LegPos(b,0, 120, 68+sp+so1, 0);
    LegPos(b,1,  90,-68   +so1, 0);
    LegPos(b,2,-110, 70+sp+so1, 0);
    LegPos(b,3,- 70,-70   +so1, 0);
    m[n].body = b;
  
    // lean left and pop
    m[n].time = 100;
    n++;
    LegPos(b,0, 120, 68   -so1, popl);
    LegPos(b,1,  90,-68-sp-so1,-popr);
    LegPos(b,2,-110, 70   -so1, popl);
    LegPos(b,3,- 70,-70-sp-so1,-popr);
    m[n].body = b;
    m[n].time = 200;
    n++;
    m[n].body = b;

    // move right feet out of way of continue roll
    m[n].time = 100;
    n++;
    LegAng(b,1, 1.0, 0.0, 0.7);
    LegAng(b,3, 1.0, 0.0, 0.8);
    m[n].body = b;
    m[n].time = 200;
    n++;
    m[n].body = b;
    
    // move left out to add force
    m[n].time = 100;
    n++;
    LegAng(b,0, 0.0, 0.0, 0.0);
    LegAng(b,2, 0.0, 0.0, 0.0);
    m[n].body = b;
    m[n].time = 100;
    n++;
    m[n].body = b;
    
    // move right feet back to add to roll
    // move head to side a little to help leg get by ear
    m[n].time = 100;
    n++;
    HeadAng(b, -1.31, -0.3, -.2618);
    LegAng(b,1, 2.26, 0.0, 0.7);
    LegAng(b,3, 2.26, 0.0, 0.8);
    m[n].body = b;
    m[n].time = 100;
    n++;
    m[n].body = b;
    
    // move left feet out of danger
    m[n].time = 100;
    n++;
    LegAng(b,0, 0.0, 1.5, 0.0);
    LegAng(b,2, 0.0, 1.5, 0.0);
    m[n].body = b;

    // move left feet out to block
    // move right feet out to block
    m[n].time = 100;
    n++;
    LegAng(b,0, 1.5, 1.5, 0.0);
    LegAng(b,2, 1.5, 1.5, 0.0);
    LegAng(b,1, 1.5, 1.5, 0.0);
    LegAng(b,3, 1.5, 1.5, 0.0);
    m[n].body = b;
    m[n].time = 2000;
    n++;
    m[n].body = b;

    // Curl all four legs
    m[n].time = 100;
    n++;
    HeadAng(b, 0.5, 0.0, 0.0);
    LegAng(b,0, 1.5, 0.0, 2.13);
    LegAng(b,1, 1.5, 0.0, 2.13);
    LegAng(b,2, 1.5, 0.0, 2.13);
    LegAng(b,3, 1.5, 0.0, 2.13);
    m[n].body = b;
    
    // Curl all four legs
    m[n].time = 200;
    n++;
    LegAng(b,0, -0.7, 0.0, 2.13);
    LegAng(b,1, -0.7, 0.0, 2.13);
    LegAng(b,2, -0.4, 0.0, 2.13);
    LegAng(b,3, -0.4, 0.0, 2.13);
    m[n].body = b;
    
    // terminate kick
    m[n].time = 0;
    n++;

    out = fopen("k_roll.mot","wb");
    fwrite(&n,sizeof(n),1,out);
    fwrite(m,sizeof(Motion::BodyStateMotion),n,out);
    fclose(out);
  }
}


void make_turn_with_ball(){

  Motion::BodyStateMotion m[64];
  Motion::BodyState b;
  FILE *out;
  int n;

  //==== Turn with ball motion (initial grab) ====//

  mzero(b);

  n = 0;
  m[n].body = b;
  m[n].time = 100;
  n++;

  BodyPos(b,75,0.4);
  HeadAng(b, -1.4, 0.0, 0.8);
  LegPos(b, 0, 120+ 0, 68,0);
  LegPos(b, 1, 120+ 0,-68,0);
  LegPos(b, 2,-100+ 0, 70,0);
  LegPos(b, 3,-100+ 0,-70,0);
  m[n].body = b;
  m[n].time = 250;
  n++;

  // grab ball
  HeadAng(b, -1.2, 0.0, 0.3);
  LegAng(b, 0, 0.5, 0.0, 1.0);
  LegAng(b, 1, 0.5, 0.0, 1.0);
  m[n].body = b;
  m[n].time = 100;
  n++;

  LegAng(b, 0, 1.664971, -0.200000, 0.611458);
  LegAng(b, 1, 1.664971, -0.200000, 0.611458);
  LegAng(b, 3, -0.772425, 0.418935, 1.556821);
  LegAng(b, 2, -0.772425, 0.418935, 1.556821);
  HeadAng(b, -0.197119, 0.074799, 0.841936);
  m[n].body = b;
  m[n].time = 100;
  n++;
  m[n].body = b;
  m[n].time = 150;
  n++;

  out = fopen("tb_grab.mot","wb");
  fwrite(&n,sizeof(n),1,out);
  fwrite(m,sizeof(Motion::BodyStateMotion),n,out);
  fclose(out);


  //==== Turn with ball motion (leg steps)  ====//

  mzero(b);

  n = 0;
  m[n].body = b;
  m[n].time = 150;
  n++;

  BodyPos(b,75,0.5);
  HeadAng(b, -0.197119, 0.074799, 0.841936);
  LegAng(b, 0, 1.664971, -0.200000, 0.611458);
  LegAng(b, 1, 1.664971, -0.200000, 0.611458);
  LegAng(b, 3, -1.473728, 0.963168, 1.716568);
  LegAng(b, 2, -0.772425, 0.418935, 1.556821);
  m[n].body = b;
  m[n].time = 150;
  n++;

  HeadAng(b, -0.197119, 0.074799, 0.841936);
  LegAng(b, 0, 1.664971, -0.200000, 0.611458);
  LegAng(b, 1, 1.664971, -0.200000, 0.611458);
  LegAng(b, 3, -0.554096, 0.000000, 1.392764);
  LegAng(b, 2, -0.966529, -0.136590, 2.273761);
  m[n].body = b;
  m[n].time = 150;
  n++;


  out = fopen("tb_step.mot","wb");
  fwrite(&n,sizeof(n),1,out);
  fwrite(m,sizeof(Motion::BodyStateMotion),n,out);
  fclose(out);
}