#include "DynamixelSerial1.h"

#define pin_dinamyxel 4
long int VEL = 100;
  
void setup(){
  Serial.begin(9600);
  Dynamixel.begin(1000000,pin_dinamyxel);  // Inicialize the servo at 1Mbps and Pin Control 3
  delay(1000);
  for(int i=0; i<6; i++) Dynamixel.setEndless(i, OFF);
}

void loop(){
  
//  for(int j=0; j<8; j++) Dynamixel.moveSpeed(j,500, VEL); //moveSpeed(ID, Position, Speed);
//  delay(5000);
  Serial.println("#######################################");
    for (int i=0; i<6; i++) 
    {
      if (Dynamixel.ping(i)!=-1) 
        {
          Serial.print("Motor ");
          Serial.print(i);
          Serial.println(" ON");
        }
      else 
        {
          Serial.print("Motor ");
          Serial.print(i);
          Serial.println(" OFF");
        }
    }
  for(int j=0; j<6; j++) Dynamixel.moveSpeed(j, 256, VEL); //moveSpeed(ID, Position, Speed);
  delay(5000);
  for(int j=0; j<6; j++) Dynamixel.moveSpeed(j, 768, VEL); //moveSpeed(ID, Position, Speed);
  delay(5000);

//    for(int i=256; i<768; i=i+10) 
//      {
//        delay(1); 
//        for(int j=0; j<8; j++) Dynamixel.moveSpeed(j, i, VEL); //moveSpeed(ID, Position, Speed);
//      }
}