86Hexapod_with_Speech_Recognition.cpp

#include <time.h>
#include <math.h>
#include <Servo86.h>
#include <ros.h>
#include <std_msgs/String.h>

double _86ME_var[50] = {0};
double _roll = 0;
double _pitch = 0;
double _comp_range = 180;
double _IMU_val[9] = {0};
double _IMU_Q[4] = {1, 0, 0, 0};
double _omega[2] = {0};
int _IMU_init_status;
int servo_mask[12] = {0};
Servo used_servos[12];

enum {_FORWARD, _LEFT, _RIGHT, _HOME, _TITLE, _NONE};
int _last_motion[2] = {_NONE, _NONE};
int _curr_motion[2] = {_NONE, _NONE};
bool internal_trigger[6] = {0};
bool external_trigger[6] = {0};
int Serial1_Command = 0xFFF;

ros::NodeHandle  nh;

void messageCb( const std_msgs::String& cmsg)
{
  Serial1_Command = (int)cmsg.data[0];
}

ros::Subscriber<std_msgs::String> sub("/recognizer/output", &messageCb );

ServoOffset offsets;

ServoFrame _86ME_HOME;
ServoFrame _86ME_RUN;

ServoFrame forward_frm[6];
ServoFrame left_frm[6];
ServoFrame right_frm[6];
ServoFrame home_frm[1];
int title_title = 1;
ServoFrame title_frm[2];


namespace forward
{
  enum {IDLE, FRAME_0, WAIT_FRAME_0, FRAME_1, WAIT_FRAME_1, FRAME_2, WAIT_FRAME_2, FRAME_3, WAIT_FRAME_3, FRAME_4, WAIT_FRAME_4, FRAME_5, WAIT_FRAME_5};
  int state = IDLE;
  unsigned long time;
  double comp_range = 180;
}
namespace left
{
  enum {IDLE, FRAME_0, WAIT_FRAME_0, FRAME_1, WAIT_FRAME_1, FRAME_2, WAIT_FRAME_2, FRAME_3, WAIT_FRAME_3, FRAME_4, WAIT_FRAME_4, FRAME_5, WAIT_FRAME_5};
  int state = IDLE;
  unsigned long time;
  double comp_range = 180;
}
namespace right
{
  enum {IDLE, FRAME_0, WAIT_FRAME_0, FRAME_1, WAIT_FRAME_1, FRAME_2, WAIT_FRAME_2, FRAME_3, WAIT_FRAME_3, FRAME_4, WAIT_FRAME_4, FRAME_5, WAIT_FRAME_5};
  int state = IDLE;
  unsigned long time;
  double comp_range = 180;
}
namespace home
{
  enum {IDLE, FRAME_0, WAIT_FRAME_0};
  int state = IDLE;
  unsigned long time;
  double comp_range = 180;
}
namespace title
{
  enum {IDLE, FLAG_0, FRAME_1, WAIT_FRAME_1, FRAME_2, WAIT_FRAME_2, GOTO_3};
  int state = IDLE;
  unsigned long time;
  int aaa_3 = 0;
  double comp_range = 180;
}
bool isBlocked(int layer)
{
  if(layer == 0)
  {
    if(external_trigger[_TITLE]) return true;
  }
  return false;
}
void closeTriggers(int layer)
{
  if(layer == 0)
  {
    external_trigger[_FORWARD]= false; internal_trigger[_FORWARD]= false;
    external_trigger[_LEFT]= false; internal_trigger[_LEFT]= false;
    external_trigger[_RIGHT]= false; internal_trigger[_RIGHT]= false;
    external_trigger[_HOME]= false; internal_trigger[_HOME]= false;
    external_trigger[_TITLE]= false; internal_trigger[_TITLE]= false;
  }
}
void updateCompRange()
{
  _comp_range = -1;
  if((external_trigger[_FORWARD] || internal_trigger[_FORWARD]) && forward::comp_range >= _comp_range)
    _comp_range = forward::comp_range;
  else if((external_trigger[_LEFT] || internal_trigger[_LEFT]) && left::comp_range >= _comp_range)
    _comp_range = left::comp_range;
  else if((external_trigger[_RIGHT] || internal_trigger[_RIGHT]) && right::comp_range >= _comp_range)
    _comp_range = right::comp_range;
  else if((external_trigger[_HOME] || internal_trigger[_HOME]) && home::comp_range >= _comp_range)
    _comp_range = home::comp_range;
  else if((external_trigger[_TITLE] || internal_trigger[_TITLE]) && title::comp_range >= _comp_range)
    _comp_range = title::comp_range;
  else
    _comp_range = 180;
}
void updateTrigger()
{
  if(isBlocked(0)) goto L1;
L0:
  if(title_title == 1) {_curr_motion[0] = _TITLE; title_title--;}
  else if(Serial1_Command == 'f') {_curr_motion[0] = _FORWARD;}
  else if(Serial1_Command == 'l') {_curr_motion[0] = _LEFT;}
  else if(Serial1_Command == 'r') {_curr_motion[0] = _RIGHT;}
  else if(Serial1_Command == 's') {_curr_motion[0] = _HOME;}
  else _curr_motion[0] = _NONE;
  if(_last_motion[0] != _curr_motion[0] && _curr_motion[0] != _NONE)
  {
    closeTriggers(0);
    external_trigger[_curr_motion[0]] = true;
    forward::state = 0;
    left::state = 0;
    right::state = 0;
    home::state = 0;
    title::state = 0;
    title::aaa_3 = 0;
  }
  external_trigger[_curr_motion[0]] = true;
  _last_motion[0] = _curr_motion[0];
L1:
  if(isBlocked(1)) return;
  _curr_motion[1] = _NONE;
  if(_last_motion[1] != _curr_motion[1] && _curr_motion[1] != _NONE)
  {
    closeTriggers(1);
    external_trigger[_curr_motion[1]] = true;
  }
  external_trigger[_curr_motion[1]] = true;
  _last_motion[1] = _curr_motion[1];
}
void forwardUpdate()
{
  switch(forward::state)
  {
  case forward::IDLE:
    if(external_trigger[_FORWARD] || internal_trigger[_FORWARD]) forward::state = forward::FRAME_0;
    else break;
  case forward::FRAME_0:
    for(int i = 12; i-- > 0; )
      _86ME_RUN.positions[i] = forward_frm[0].positions[i] & (~servo_mask[i]);
    _86ME_RUN.playPositions((unsigned long)300);
    forward::time = millis();
    forward::state = forward::WAIT_FRAME_0;
  case forward::WAIT_FRAME_0:
    if(millis() - forward::time >= 300)
      forward::state = forward::FRAME_1;
    break;
  case forward::FRAME_1:
    for(int i = 12; i-- > 0; )
      _86ME_RUN.positions[i] = forward_frm[1].positions[i] & (~servo_mask[i]);
    _86ME_RUN.playPositions((unsigned long)150);
    forward::time = millis();
    forward::state = forward::WAIT_FRAME_1;
  case forward::WAIT_FRAME_1:
    if(millis() - forward::time >= 150)
      forward::state = forward::FRAME_2;
    break;
  case forward::FRAME_2:
    for(int i = 12; i-- > 0; )
      _86ME_RUN.positions[i] = forward_frm[2].positions[i] & (~servo_mask[i]);
    _86ME_RUN.playPositions((unsigned long)150);
    forward::time = millis();
    forward::state = forward::WAIT_FRAME_2;
  case forward::WAIT_FRAME_2:
    if(millis() - forward::time >= 150)
      forward::state = forward::FRAME_3;
    break;
  case forward::FRAME_3:
    for(int i = 12; i-- > 0; )
      _86ME_RUN.positions[i] = forward_frm[3].positions[i] & (~servo_mask[i]);
    _86ME_RUN.playPositions((unsigned long)300);
    forward::time = millis();
    forward::state = forward::WAIT_FRAME_3;
  case forward::WAIT_FRAME_3:
    if(millis() - forward::time >= 300)
      forward::state = forward::FRAME_4;
    break;
  case forward::FRAME_4:
    for(int i = 12; i-- > 0; )
      _86ME_RUN.positions[i] = forward_frm[4].positions[i] & (~servo_mask[i]);
    _86ME_RUN.playPositions((unsigned long)150);
    forward::time = millis();
    forward::state = forward::WAIT_FRAME_4;
  case forward::WAIT_FRAME_4:
    if(millis() - forward::time >= 150)
      forward::state = forward::FRAME_5;
    break;
  case forward::FRAME_5:
    for(int i = 12; i-- > 0; )
      _86ME_RUN.positions[i] = forward_frm[5].positions[i] & (~servo_mask[i]);
    _86ME_RUN.playPositions((unsigned long)150);
    forward::time = millis();
    forward::state = forward::WAIT_FRAME_5;
  case forward::WAIT_FRAME_5:
    if(millis() - forward::time >= 150)
    {
      forward::state = forward::IDLE;
      internal_trigger[_FORWARD] = false;
      external_trigger[_FORWARD] = false;
    }
    break;
  default:
    break;
  }
}
void leftUpdate()
{
  switch(left::state)
  {
  case left::IDLE:
    if(external_trigger[_LEFT] || internal_trigger[_LEFT]) left::state = left::FRAME_0;
    else break;
  case left::FRAME_0:
    for(int i = 12; i-- > 0; )
      _86ME_RUN.positions[i] = left_frm[0].positions[i] & (~servo_mask[i]);
    _86ME_RUN.playPositions((unsigned long)300);
    left::time = millis();
    left::state = left::WAIT_FRAME_0;
  case left::WAIT_FRAME_0:
    if(millis() - left::time >= 300)
      left::state = left::FRAME_1;
    break;
  case left::FRAME_1:
    for(int i = 12; i-- > 0; )
      _86ME_RUN.positions[i] = left_frm[1].positions[i] & (~servo_mask[i]);
    _86ME_RUN.playPositions((unsigned long)150);
    left::time = millis();
    left::state = left::WAIT_FRAME_1;
  case left::WAIT_FRAME_1:
    if(millis() - left::time >= 150)
      left::state = left::FRAME_2;
    break;
  case left::FRAME_2:
    for(int i = 12; i-- > 0; )
      _86ME_RUN.positions[i] = left_frm[2].positions[i] & (~servo_mask[i]);
    _86ME_RUN.playPositions((unsigned long)150);
    left::time = millis();
    left::state = left::WAIT_FRAME_2;
  case left::WAIT_FRAME_2:
    if(millis() - left::time >= 150)
      left::state = left::FRAME_3;
    break;
  case left::FRAME_3:
    for(int i = 12; i-- > 0; )
      _86ME_RUN.positions[i] = left_frm[3].positions[i] & (~servo_mask[i]);
    _86ME_RUN.playPositions((unsigned long)300);
    left::time = millis();
    left::state = left::WAIT_FRAME_3;
  case left::WAIT_FRAME_3:
    if(millis() - left::time >= 300)
      left::state = left::FRAME_4;
    break;
  case left::FRAME_4:
    for(int i = 12; i-- > 0; )
      _86ME_RUN.positions[i] = left_frm[4].positions[i] & (~servo_mask[i]);
    _86ME_RUN.playPositions((unsigned long)150);
    left::time = millis();
    left::state = left::WAIT_FRAME_4;
  case left::WAIT_FRAME_4:
    if(millis() - left::time >= 150)
      left::state = left::FRAME_5;
    break;
  case left::FRAME_5:
    for(int i = 12; i-- > 0; )
      _86ME_RUN.positions[i] = left_frm[5].positions[i] & (~servo_mask[i]);
    _86ME_RUN.playPositions((unsigned long)150);
    left::time = millis();
    left::state = left::WAIT_FRAME_5;
  case left::WAIT_FRAME_5:
    if(millis() - left::time >= 150)
    {
      left::state = left::IDLE;
      internal_trigger[_LEFT] = false;
      external_trigger[_LEFT] = false;
    }
    break;
  default:
    break;
  }
}
void rightUpdate()
{
  switch(right::state)
  {
  case right::IDLE:
    if(external_trigger[_RIGHT] || internal_trigger[_RIGHT]) right::state = right::FRAME_0;
    else break;
  case right::FRAME_0:
    for(int i = 12; i-- > 0; )
      _86ME_RUN.positions[i] = right_frm[0].positions[i] & (~servo_mask[i]);
    _86ME_RUN.playPositions((unsigned long)300);
    right::time = millis();
    right::state = right::WAIT_FRAME_0;
  case right::WAIT_FRAME_0:
    if(millis() - right::time >= 300)
      right::state = right::FRAME_1;
    break;
  case right::FRAME_1:
    for(int i = 12; i-- > 0; )
      _86ME_RUN.positions[i] = right_frm[1].positions[i] & (~servo_mask[i]);
    _86ME_RUN.playPositions((unsigned long)150);
    right::time = millis();
    right::state = right::WAIT_FRAME_1;
  case right::WAIT_FRAME_1:
    if(millis() - right::time >= 150)
      right::state = right::FRAME_2;
    break;
  case right::FRAME_2:
    for(int i = 12; i-- > 0; )
      _86ME_RUN.positions[i] = right_frm[2].positions[i] & (~servo_mask[i]);
    _86ME_RUN.playPositions((unsigned long)150);
    right::time = millis();
    right::state = right::WAIT_FRAME_2;
  case right::WAIT_FRAME_2:
    if(millis() - right::time >= 150)
      right::state = right::FRAME_3;
    break;
  case right::FRAME_3:
    for(int i = 12; i-- > 0; )
      _86ME_RUN.positions[i] = right_frm[3].positions[i] & (~servo_mask[i]);
    _86ME_RUN.playPositions((unsigned long)300);
    right::time = millis();
    right::state = right::WAIT_FRAME_3;
  case right::WAIT_FRAME_3:
    if(millis() - right::time >= 300)
      right::state = right::FRAME_4;
    break;
  case right::FRAME_4:
    for(int i = 12; i-- > 0; )
      _86ME_RUN.positions[i] = right_frm[4].positions[i] & (~servo_mask[i]);
    _86ME_RUN.playPositions((unsigned long)150);
    right::time = millis();
    right::state = right::WAIT_FRAME_4;
  case right::WAIT_FRAME_4:
    if(millis() - right::time >= 150)
      right::state = right::FRAME_5;
    break;
  case right::FRAME_5:
    for(int i = 12; i-- > 0; )
      _86ME_RUN.positions[i] = right_frm[5].positions[i] & (~servo_mask[i]);
    _86ME_RUN.playPositions((unsigned long)150);
    right::time = millis();
    right::state = right::WAIT_FRAME_5;
  case right::WAIT_FRAME_5:
    if(millis() - right::time >= 150)
    {
      right::state = right::IDLE;
      internal_trigger[_RIGHT] = false;
      external_trigger[_RIGHT] = false;
    }
    break;
  default:
    break;
  }
}
void homeUpdate()
{
  switch(home::state)
  {
  case home::IDLE:
    if(external_trigger[_HOME] || internal_trigger[_HOME]) home::state = home::FRAME_0;
    else break;
  case home::FRAME_0:
    for(int i = 12; i-- > 0; )
      _86ME_RUN.positions[i] = home_frm[0].positions[i] & (~servo_mask[i]);
    _86ME_RUN.playPositions((unsigned long)200);
    home::time = millis();
    home::state = home::WAIT_FRAME_0;
  case home::WAIT_FRAME_0:
    if(millis() - home::time >= 200)
    {
      home::state = home::IDLE;
      internal_trigger[_HOME] = false;
      external_trigger[_HOME] = false;
    }
    break;
  default:
    break;
  }
}
void titleUpdate()
{
  switch(title::state)
  {
  case title::IDLE:
    if(external_trigger[_TITLE] || internal_trigger[_TITLE]) title::state = title::FLAG_0;
    else break;
  case title::FLAG_0:
    flag_title_aaa_0:
  case title::FRAME_1:
    for(int i = 12; i-- > 0; )
      _86ME_RUN.positions[i] = title_frm[0].positions[i] & (~servo_mask[i]);
    _86ME_RUN.playPositions((unsigned long)300);
    title::time = millis();
    title::state = title::WAIT_FRAME_1;
  case title::WAIT_FRAME_1:
    if(millis() - title::time >= 300)
      title::state = title::FRAME_2;
    break;
  case title::FRAME_2:
    for(int i = 12; i-- > 0; )
      _86ME_RUN.positions[i] = title_frm[1].positions[i] & (~servo_mask[i]);
    _86ME_RUN.playPositions((unsigned long)300);
    title::time = millis();
    title::state = title::WAIT_FRAME_2;
  case title::WAIT_FRAME_2:
    if(millis() - title::time >= 300)
      title::state = title::GOTO_3;
    break;
  case title::GOTO_3:
    if(title::aaa_3++ < 1) goto flag_title_aaa_0;
    else
    {
      title::aaa_3 = 0;
      internal_trigger[_TITLE] = false;
      external_trigger[_TITLE] = false;
      title::state = title::IDLE;
    }
    break;
  default:
    break;
  }
}
void setup()
{
  Serial.begin(9600);
  nh.getHardware()->setESP8266(Serial1, 115200);
  nh.getHardware()->setWiFi("SSID", "PASSWORD");
  nh.initNode("ROSSERIAL_SERVER_IP");
  nh.subscribe(sub);

  used_servos[0].attach(2, 500, 2500);
  used_servos[1].attach(3, 500, 2500);
  used_servos[2].attach(4, 500, 2500);
  used_servos[3].attach(5, 500, 2500);
  used_servos[4].attach(6, 500, 2500);
  used_servos[5].attach(7, 500, 2500);
  used_servos[6].attach(8, 500, 2500);
  used_servos[7].attach(9, 500, 2500);
  used_servos[8].attach(10, 500, 2500);
  used_servos[9].attach(11, 500, 2500);
  used_servos[10].attach(12, 500, 2500);
  used_servos[11].attach(13, 500, 2500);

  offsets.offsets[1] = -140;
  offsets.offsets[3] = -50;
  offsets.offsets[5] = -50;
  offsets.offsets[7] = -90;
  offsets.offsets[9] = -110;
  offsets.offsets[11] = -100;

  forward_frm[0].positions[0] = 1700;
  forward_frm[0].positions[1] = 1500;
  forward_frm[0].positions[2] = 1300;
  forward_frm[0].positions[3] = 1750;
  forward_frm[0].positions[4] = 1700;
  forward_frm[0].positions[5] = 1500;
  forward_frm[0].positions[6] = 1700;
  forward_frm[0].positions[7] = 1750;
  forward_frm[0].positions[8] = 1300;
  forward_frm[0].positions[9] = 1500;
  forward_frm[0].positions[10] = 1700;
  forward_frm[0].positions[11] = 1750;

  forward_frm[1].positions[0] = 1700;
  forward_frm[1].positions[1] = 1500;
  forward_frm[1].positions[2] = 1300;
  forward_frm[1].positions[3] = 1500;
  forward_frm[1].positions[4] = 1700;
  forward_frm[1].positions[5] = 1500;
  forward_frm[1].positions[6] = 1700;
  forward_frm[1].positions[7] = 1500;
  forward_frm[1].positions[8] = 1300;
  forward_frm[1].positions[9] = 1500;
  forward_frm[1].positions[10] = 1700;
  forward_frm[1].positions[11] = 1500;

  forward_frm[2].positions[0] = 1700;
  forward_frm[2].positions[1] = 1750;
  forward_frm[2].positions[2] = 1300;
  forward_frm[2].positions[3] = 1500;
  forward_frm[2].positions[4] = 1700;
  forward_frm[2].positions[5] = 1750;
  forward_frm[2].positions[6] = 1700;
  forward_frm[2].positions[7] = 1500;
  forward_frm[2].positions[8] = 1300;
  forward_frm[2].positions[9] = 1750;
  forward_frm[2].positions[10] = 1700;
  forward_frm[2].positions[11] = 1500;

  forward_frm[3].positions[0] = 1300;
  forward_frm[3].positions[1] = 1750;
  forward_frm[3].positions[2] = 1700;
  forward_frm[3].positions[3] = 1500;
  forward_frm[3].positions[4] = 1300;
  forward_frm[3].positions[5] = 1750;
  forward_frm[3].positions[6] = 1300;
  forward_frm[3].positions[7] = 1500;
  forward_frm[3].positions[8] = 1700;
  forward_frm[3].positions[9] = 1750;
  forward_frm[3].positions[10] = 1300;
  forward_frm[3].positions[11] = 1500;

  forward_frm[4].positions[0] = 1300;
  forward_frm[4].positions[1] = 1500;
  forward_frm[4].positions[2] = 1700;
  forward_frm[4].positions[3] = 1500;
  forward_frm[4].positions[4] = 1300;
  forward_frm[4].positions[5] = 1500;
  forward_frm[4].positions[6] = 1300;
  forward_frm[4].positions[7] = 1500;
  forward_frm[4].positions[8] = 1700;
  forward_frm[4].positions[9] = 1500;
  forward_frm[4].positions[10] = 1300;
  forward_frm[4].positions[11] = 1500;

  forward_frm[5].positions[0] = 1300;
  forward_frm[5].positions[1] = 1500;
  forward_frm[5].positions[2] = 1700;
  forward_frm[5].positions[3] = 1750;
  forward_frm[5].positions[4] = 1300;
  forward_frm[5].positions[5] = 1500;
  forward_frm[5].positions[6] = 1300;
  forward_frm[5].positions[7] = 1750;
  forward_frm[5].positions[8] = 1700;
  forward_frm[5].positions[9] = 1500;
  forward_frm[5].positions[10] = 1300;
  forward_frm[5].positions[11] = 1750;

  left_frm[0].positions[0] = 1700;
  left_frm[0].positions[1] = 1750;
  left_frm[0].positions[2] = 1300;
  left_frm[0].positions[3] = 1500;
  left_frm[0].positions[4] = 1700;
  left_frm[0].positions[5] = 1750;
  left_frm[0].positions[6] = 1300;
  left_frm[0].positions[7] = 1500;
  left_frm[0].positions[8] = 1700;
  left_frm[0].positions[9] = 1750;
  left_frm[0].positions[10] = 1300;
  left_frm[0].positions[11] = 1500;

  left_frm[1].positions[0] = 1700;
  left_frm[1].positions[1] = 1500;
  left_frm[1].positions[2] = 1300;
  left_frm[1].positions[3] = 1500;
  left_frm[1].positions[4] = 1700;
  left_frm[1].positions[5] = 1500;
  left_frm[1].positions[6] = 1300;
  left_frm[1].positions[7] = 1500;
  left_frm[1].positions[8] = 1700;
  left_frm[1].positions[9] = 1500;
  left_frm[1].positions[10] = 1300;
  left_frm[1].positions[11] = 1500;

  left_frm[2].positions[0] = 1700;
  left_frm[2].positions[1] = 1500;
  left_frm[2].positions[2] = 1300;
  left_frm[2].positions[3] = 1750;
  left_frm[2].positions[4] = 1700;
  left_frm[2].positions[5] = 1500;
  left_frm[2].positions[6] = 1300;
  left_frm[2].positions[7] = 1750;
  left_frm[2].positions[8] = 1700;
  left_frm[2].positions[9] = 1500;
  left_frm[2].positions[10] = 1300;
  left_frm[2].positions[11] = 1750;

  left_frm[3].positions[0] = 1300;
  left_frm[3].positions[1] = 1500;
  left_frm[3].positions[2] = 1700;
  left_frm[3].positions[3] = 1750;
  left_frm[3].positions[4] = 1300;
  left_frm[3].positions[5] = 1500;
  left_frm[3].positions[6] = 1700;
  left_frm[3].positions[7] = 1750;
  left_frm[3].positions[8] = 1300;
  left_frm[3].positions[9] = 1500;
  left_frm[3].positions[10] = 1700;
  left_frm[3].positions[11] = 1750;

  left_frm[4].positions[0] = 1300;
  left_frm[4].positions[1] = 1500;
  left_frm[4].positions[2] = 1700;
  left_frm[4].positions[3] = 1500;
  left_frm[4].positions[4] = 1300;
  left_frm[4].positions[5] = 1500;
  left_frm[4].positions[6] = 1700;
  left_frm[4].positions[7] = 1500;
  left_frm[4].positions[8] = 1300;
  left_frm[4].positions[9] = 1500;
  left_frm[4].positions[10] = 1700;
  left_frm[4].positions[11] = 1500;

  left_frm[5].positions[0] = 1300;
  left_frm[5].positions[1] = 1750;
  left_frm[5].positions[2] = 1700;
  left_frm[5].positions[3] = 1500;
  left_frm[5].positions[4] = 1300;
  left_frm[5].positions[5] = 1750;
  left_frm[5].positions[6] = 1700;
  left_frm[5].positions[7] = 1500;
  left_frm[5].positions[8] = 1300;
  left_frm[5].positions[9] = 1750;
  left_frm[5].positions[10] = 1700;
  left_frm[5].positions[11] = 1500;

  right_frm[0].positions[0] = 1300;
  right_frm[0].positions[1] = 1750;
  right_frm[0].positions[2] = 1700;
  right_frm[0].positions[3] = 1500;
  right_frm[0].positions[4] = 1300;
  right_frm[0].positions[5] = 1750;
  right_frm[0].positions[6] = 1700;
  right_frm[0].positions[7] = 1500;
  right_frm[0].positions[8] = 1300;
  right_frm[0].positions[9] = 1750;
  right_frm[0].positions[10] = 1700;
  right_frm[0].positions[11] = 1500;

  right_frm[1].positions[0] = 1300;
  right_frm[1].positions[1] = 1500;
  right_frm[1].positions[2] = 1700;
  right_frm[1].positions[3] = 1500;
  right_frm[1].positions[4] = 1300;
  right_frm[1].positions[5] = 1500;
  right_frm[1].positions[6] = 1700;
  right_frm[1].positions[7] = 1500;
  right_frm[1].positions[8] = 1300;
  right_frm[1].positions[9] = 1500;
  right_frm[1].positions[10] = 1700;
  right_frm[1].positions[11] = 1500;

  right_frm[2].positions[0] = 1300;
  right_frm[2].positions[1] = 1500;
  right_frm[2].positions[2] = 1700;
  right_frm[2].positions[3] = 1750;
  right_frm[2].positions[4] = 1300;
  right_frm[2].positions[5] = 1500;
  right_frm[2].positions[6] = 1700;
  right_frm[2].positions[7] = 1750;
  right_frm[2].positions[8] = 1300;
  right_frm[2].positions[9] = 1500;
  right_frm[2].positions[10] = 1700;
  right_frm[2].positions[11] = 1750;

  right_frm[3].positions[0] = 1700;
  right_frm[3].positions[1] = 1500;
  right_frm[3].positions[2] = 1300;
  right_frm[3].positions[3] = 1750;
  right_frm[3].positions[4] = 1700;
  right_frm[3].positions[5] = 1500;
  right_frm[3].positions[6] = 1300;
  right_frm[3].positions[7] = 1750;
  right_frm[3].positions[8] = 1700;
  right_frm[3].positions[9] = 1500;
  right_frm[3].positions[10] = 1300;
  right_frm[3].positions[11] = 1750;

  right_frm[4].positions[0] = 1700;
  right_frm[4].positions[1] = 1500;
  right_frm[4].positions[2] = 1300;
  right_frm[4].positions[3] = 1500;
  right_frm[4].positions[4] = 1700;
  right_frm[4].positions[5] = 1500;
  right_frm[4].positions[6] = 1300;
  right_frm[4].positions[7] = 1500;
  right_frm[4].positions[8] = 1700;
  right_frm[4].positions[9] = 1500;
  right_frm[4].positions[10] = 1300;
  right_frm[4].positions[11] = 1500;

  right_frm[5].positions[0] = 1700;
  right_frm[5].positions[1] = 1750;
  right_frm[5].positions[2] = 1300;
  right_frm[5].positions[3] = 1500;
  right_frm[5].positions[4] = 1700;
  right_frm[5].positions[5] = 1750;
  right_frm[5].positions[6] = 1300;
  right_frm[5].positions[7] = 1500;
  right_frm[5].positions[8] = 1700;
  right_frm[5].positions[9] = 1750;
  right_frm[5].positions[10] = 1300;
  right_frm[5].positions[11] = 1500;

  home_frm[0].positions[0] = 1500;
  home_frm[0].positions[1] = 1500;
  home_frm[0].positions[2] = 1500;
  home_frm[0].positions[3] = 1500;
  home_frm[0].positions[4] = 1500;
  home_frm[0].positions[5] = 1500;
  home_frm[0].positions[6] = 1500;
  home_frm[0].positions[7] = 1500;
  home_frm[0].positions[8] = 1500;
  home_frm[0].positions[9] = 1500;
  home_frm[0].positions[10] = 1500;
  home_frm[0].positions[11] = 1500;

  title_frm[0].positions[0] = 1729;
  title_frm[0].positions[1] = 1950;
  title_frm[0].positions[2] = 1500;
  title_frm[0].positions[3] = 1500;
  title_frm[0].positions[4] = 1500;
  title_frm[0].positions[5] = 1500;
  title_frm[0].positions[6] = 1500;
  title_frm[0].positions[7] = 1500;
  title_frm[0].positions[8] = 1500;
  title_frm[0].positions[9] = 1500;
  title_frm[0].positions[10] = 1500;
  title_frm[0].positions[11] = 1500;

  title_frm[1].positions[0] = 1381;
  title_frm[1].positions[1] = 1950;
  title_frm[1].positions[2] = 1500;
  title_frm[1].positions[3] = 1500;
  title_frm[1].positions[4] = 1500;
  title_frm[1].positions[5] = 1500;
  title_frm[1].positions[6] = 1500;
  title_frm[1].positions[7] = 1500;
  title_frm[1].positions[8] = 1500;
  title_frm[1].positions[9] = 1500;
  title_frm[1].positions[10] = 1500;
  title_frm[1].positions[11] = 1500;

  _86ME_HOME.positions[0] = 1500;
  _86ME_HOME.positions[1] = 1500;
  _86ME_HOME.positions[2] = 1500;
  _86ME_HOME.positions[3] = 1500;
  _86ME_HOME.positions[4] = 1500;
  _86ME_HOME.positions[5] = 1500;
  _86ME_HOME.positions[6] = 1500;
  _86ME_HOME.positions[7] = 1500;
  _86ME_HOME.positions[8] = 1500;
  _86ME_HOME.positions[9] = 1500;
  _86ME_HOME.positions[10] = 1500;
  _86ME_HOME.positions[11] = 1500;


  offsets.setOffsets();

  _86ME_HOME.playPositions((unsigned long)0);
}

void loop()
{
  updateTrigger();
  forwardUpdate();
  leftUpdate();
  rightUpdate();
  homeUpdate();
  titleUpdate();
  nh.spinOnce();
}