#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_READUPDATE_INTERVAL 1000/5
#define SERVO_MIN_COMMAND_TIME 1 //should be less than any of the other servo intervals/SERVO_COUNT


#define SERVO_COUNT 3




struct dxl_servo {
	uint8_t id;
	//int initial_speed;
  int32_t velocity;
	float offset_angle; //constant offset. set this to match angle=0 to a straight arm
  float angle; //commanded angle
  float actual_angle; //angle read from servo
	float min_angle; //soft angle limit
	float max_angle; //soft angle limit
	int orientation; //1.0 = forward. -1.0 = backwards
  uint8_t mode=OP_CURRENT_BASED_POSITION; // OP_POSITION, OP_CURRENT_BASED_POSITION
  float current; //when mode=OP_CURRENT_BASED_POSITION, set current for servo
  uint16_t modelnumber; //Servo Model number for connection check at startup. use 0 to deactivate
};



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);
float getAverageServoPositionerror();
void printServoDebug();


#endif