#include "DynamixelSerial.h"

//TLC5940NT pin definitions
#define VPRG 2
#define SIN 11
#define SCLK 13
#define XLAT 4
#define BLANK 5
#define DCPRG 6
#define GSCLK 7
uint8_t dataPin  = 8;
uint8_t latchPin = 9;
uint8_t clockPin = 10;

#define pin_dinamyxel 3
long int VEL = 100;
  
void setup(){
  pinMode(VPRG, OUTPUT);
  pinMode(SIN, OUTPUT);
  pinMode(SCLK, OUTPUT);
  pinMode(XLAT, OUTPUT);
  pinMode(BLANK, OUTPUT);
  pinMode(DCPRG, OUTPUT);
  pinMode(GSCLK, OUTPUT);
  pinMode(MISO, INPUT);
  pinMode(SS,OUTPUT);
  pinMode(dataPin, OUTPUT);
  pinMode(latchPin, OUTPUT);  
  pinMode(clockPin, OUTPUT); 
  digitalWrite(SS,HIGH); //disable device
  digitalWrite(SIN, LOW);
  digitalWrite(SCLK, LOW);
  digitalWrite(XLAT, LOW);
  digitalWrite(VPRG, LOW);
  digitalWrite(BLANK, HIGH);
  digitalWrite(GSCLK, HIGH);
  digitalWrite(DCPRG, LOW); // USE EEPROM DC register if LOW
  Dynamixel.begin(1000000,pin_dinamyxel);  // Inicialize the servo at 1Mbps and Pin Control 3
  delay(1000);
  for(int i=0; i<8; 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);
  
  for(int j=0; j<8; j++) Dynamixel.moveSpeed(j, 256, VEL); //moveSpeed(ID, Position, Speed);
  delay(5000);
  for(int j=0; j<8; 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);
//      }
}