Current-based Position Control Mode

Hi, I am currently using the current-based position control mode to control some dynamxiel MX64-AR(2.0) actuators., which offers us the capability to control the motion of the motor while setting the max torque during motion.
However, I noticed that the torque feedback from Dynamixel actuators is always not stable.
I would like to test the min torque that is required to drive the actuator but found that every time the robot would start to move at different torque values (unit mA). As the attached screenshot shows, the motor starts to rotate at each peak. I tried to reset torque values to zero after each iteration but it didn’t really work and gave similar results.
The main functions I am using is:;

I also have my testing code attached here. Please have a look and provide some suggestions and I would appreciate it a lot!

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)

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.
// Set Port baudrate to 57600bps. This has to match with DYNAMIXEL baudrate.
// Set Port Protocol Version. This has to match with DYNAMIXEL protocol version.
// Get DYNAMIXEL information;

// Turn off torque when configuring items in EEPROM area, set PWM mode
dxl.setOperatingMode(activeChanel_ID, OP_CURRENT_BASED_POSITION);
// dxl.writeControlTableItem(PROFILE_VELOCITY, activeChanel_ID, 50);



void loop(){
CP = dxl.getPresentPosition(activeChanel_ID,UNIT_DEGREE);
DP = CP + 30;
for (torque = 0; torque < 1000; torque = torque + 1){
DEBUG_SERIAL.print("Present Torque(mA) : ");