Here is my testing code:
#include <Dynamixel2Arduino.h>
#include <RC100.h>
#if defined(ARDUINO_AVR_UNO) || defined(ARDUINO_AVR_MEGA2560) // When using DynamixelShield
#include <SoftwareSerial.h>
SoftwareSerial soft_serial(7, 8); // DYNAMIXELShield UART RX/TX
#define DXL_SERIAL Serial
#define DEBUG_SERIAL soft_serial
const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN
#elif 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 DEBUG_SERIAL Serial
const uint8_t DXL_DIR_PIN = 22; //OpenCM9.04 EXP Board’s DIR PIN. (28 for the DXL port on the OpenCM 9.04 board)
#endif
const uint8_t activeChanel_ID = 6;
float torque;
float CP, DP, initial;
const float DXL_PROTOCOL_VERSION = 2.0;
unsigned long previousMillis = 0;
const long interval = 1400;
double K8P_level;
double PVQ_level;
RC100 Controller;
uint16_t RcvData = 0;
Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN);
using namespace ControlTableItem;
void setup(){
// Use UART port of DYNAMIXEL Shield to debug.
DEBUG_SERIAL.begin(57600);
// Set Port baudrate to 57600bps. This has to match with DYNAMIXEL baudrate.
dxl.begin(57600);
// Set Port Protocol Version. This has to match with DYNAMIXEL protocol version.
dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION);
// Get DYNAMIXEL information
dxl.ping(activeChanel_ID);
// Turn off torque when configuring items in EEPROM area, set PWM mode
dxl.torqueOff(activeChanel_ID);
dxl.setOperatingMode(activeChanel_ID, OP_CURRENT_BASED_POSITION);
dxl.torqueOn(activeChanel_ID);
// dxl.writeControlTableItem(PROFILE_VELOCITY, activeChanel_ID, 50);
Controller.begin(2);
Serial.begin(9600);
}
void loop(){
dxl.setGoalCurrent(activeChanel_ID,0,UNIT_MILLI_AMPERE);
CP = dxl.getPresentPosition(activeChanel_ID,UNIT_DEGREE);
DP = CP + 30;
for (torque = 0; torque < 1000; torque = torque + 1){
dxl.setGoalCurrent(activeChanel_ID,torque,UNIT_MILLI_AMPERE);
dxl.setGoalPosition(activeChanel_ID,DP,UNIT_DEGREE);
delay(200);
DEBUG_SERIAL.print("Present Torque(mA) : ");
DEBUG_SERIAL.println(dxl.getPresentCurrent(activeChanel_ID,UNIT_MILLI_AMPERE));
}
}