 
 void jointControl()
 {
-  uint16_t step_time = uint16_t(floor(MOVE_TIME/CONTROL_PERIOD) + 1.0);
+  uint16_t step_time = uint16_t(floor(MOVE_TIME/CONTROL_TIME) + 1.0);
   float tick_time = 0;
   static uint16_t step_cnt = 0;
   static float moving = true;

  goal_trajectory.push_back(goal);
  goal_trajectory.push_back(goal);

  joint_trajectory.init(start_trajectory, goal_trajectory, MOVE_TIME, CONTROL_PERIOD);
}

void loop() 
{
  uint32_t t = millis();

  if ((t-tTime[0]) >= CONTROL_PERIOD)
  {
    present_position = dxl.getAngle();

    tTime[0] = t;
  }

  if ((t-tTime[1]) >= CONTROL_PERIOD)
  {
    jointControl();
    
    tTime[1] = t;
  }

std::vector<float> goal_acceleration;
std::vector<float> present_position;

static float tTime[2] = {0.0, 0.0};

void setup()
{
