#ifndef SERVO_H_
#define SERVO_H_


#include <Dynamixel2Arduino.h>
#include "actuator.h" //needed to be able to access ControlTableItem::

#if defined(ARDUINO_OpenCM904) // When using official ROBOTIS board with DXL circuit.
  //#define DXL_SERIAL   Serial3 //OpenCM9.04 EXP Board's DXL port Serial. (Serial1 for the DXL port on the OpenCM 9.04 board)
  #define DXL_SERIAL   Serial1 //OpenCM9.04 EXP Board's DXL port Serial. (Serial1 for the DXL port on the OpenCM 9.04 board)
  #define DEBUG_SERIAL Serial
  //const int DXL_DIR_PIN = 22; //OpenCM9.04 EXP Board's DIR PIN. (28 for the DXL port on the OpenCM 9.04 board)
  const int DXL_DIR_PIN = 28; //OpenCM9.04 EXP Board's DIR PIN. (28 for the DXL port on the OpenCM 9.04 board)
#endif

#define DXL_SERIAL   Serial1

#define SERVO_UPDATE_INTERVAL 1000/50


#define SERVO_COUNT 3




struct dxl_servo {
	int id;
	//int initial_speed;
	float offset_angle;
  float angle;
	float min_angle;
	float max_angle;
	int orientation;
  uint8_t mode=OP_CURRENT_BASED_POSITION;
  float current;
  uint16_t modelnumber;
};



void scanDynamixel(int32_t baud, uint8_t protocol,uint8_t maxid);
void scanDynamixel();
void changeID(uint8_t present_id, uint8_t new_id);
void initServos();
void loopServos(unsigned long millis);
void printServoDebug();


#endif