My apologies. I'm trying to move a carriage with 3 stepper motors. But I need acceleration and decelaration so the carriage does not thump/skip steps when either changing directions or stopping. The code I am using has acceleration when starting a move or when changing directions but not when stopping. I have been stumped by this problem for a solid couple of days now.
main.cpp
#include <Arduino.h>
#include "variables.h" //Contains most of the variables
#include "Wire.h"
#include "SPI.h"
#include "SerialTransfer.h"
#include <LiquidCrystal_I2C.h>
#include <LiquidMenu.h> //change liquidmenu to liquidmenu_i2c in liquidmenu config
#include <AccelStepper.h>
#include <MultiStepper.h>
#include "functiondeclaration.h"
AccelStepper lenghtStepper = AccelStepper(1, LENGHT_STEP_PIN, LENGHT_DIR_PIN);
AccelStepper turnStepper = AccelStepper(1, TURN_STEP_PIN, TURN_DIR_PIN);
AccelStepper tiltStepper = AccelStepper(1, TILT_STEP_PIN, TILT_DIR_PIN);
MultiStepper steppers; // Manages multiple AccelSteppers
//NOTE: THIS IS NOT THE PART OF THE CODE I'M HAVING PROBLEMS WITH
LiquidCrystal_I2C lcd(lcd_address, lcd_width, lcd_height);
///////////////////////////////////////////////////////////////////
//Main menu
//LINES
LiquidLine welcome_line1(6, 0, "Main Menu");
LiquidLine welcome_line2(0, 1, "Set pos");
LiquidLine welcome_line3(0, 2, "Drive positions");
LiquidLine welcome_line4(0, 3, "Drive pos loop");
//SCREENS
// You can add 4 line items here but the rest would then be required to be put into setup
LiquidScreen welcome_screen1(welcome_line1, welcome_line2, welcome_line3, welcome_line4);
//MENU
LiquidMenu welcome_menu(lcd, welcome_screen1);
///////////////////////////////////////////////////////////////////
//Drive menu
//setIndexAmountMenu where you set how many positions you want
//LINES setIndexAmountMenu
LiquidLine setIndexAmountMenu_line0(0, 0, "<- back");
LiquidLine setIndexAmountMenu_line1(2, 1, "Set index amount");
LiquidLine setIndexAmountMenu_line2(4, 2, "Index : ", positionCount);
LiquidLine setIndexAmountMenu_line3(7, 3, "Accept");
//SCREEN
LiquidScreen setIndexAmountMenu_screen1(setIndexAmountMenu_line0, setIndexAmountMenu_line1 ,setIndexAmountMenu_line2, setIndexAmountMenu_line3);
//Ask if wanna reset positions
//LINES
LiquidLine resetIndex_line1(0, 0, "<- back");
LiquidLine resetIndex_line2(4,1,"Reset positions?");
LiquidLine resetIndex_line3(0,2,"Yes No");
LiquidScreen resetIndex_screen1(resetIndex_line1,resetIndex_line2,resetIndex_line3);
//runMotorsSetPositionsMenu where you drive stepper motors into positions
//LINES
LiquidLine runMotorsSetPositionsMenu_line1(0, 0, "Index: ", indexValueLCD, " / ", positionCount);
LiquidLine runMotorsSetPositionsMenu_line2(0, 1, "Speed: ", speed);
LiquidLine runMotorsSetPositionsMenu_line3(0, 2);
LiquidLine runMotorsSetPositionsMenu_line4(0, 3, "Accept");
//SCREENrunMotorsSetPositionsMenu
LiquidScreen runMotorsSetPositionsMenu_screen1(runMotorsSetPositionsMenu_line1, runMotorsSetPositionsMenu_line2, runMotorsSetPositionsMenu_line3, runMotorsSetPositionsMenu_line4);
//MENU
//LiquidMenu drive_menu2(lcd,drive_screen2);
LiquidMenu drive_menu(lcd, setIndexAmountMenu_screen1, runMotorsSetPositionsMenu_screen1,resetIndex_screen1);
///////////////////////////////////////////////////////////////////
//Menu system
LiquidSystem menu_system(welcome_menu, drive_menu);
///////////////////////////////////////////////////////////////////
SerialTransfer myTransfer; // remember to use myTransfer.tick(); to get callback
/*
void debugPrintPositions(){
for(int i = 0; i < 10; i++){
for(int j = 0; j < 3; j++){
Serial.print(positions[i][j]);
Serial.print(" ");
}
Serial.println();
}
}
*/
void drivemotors()
{
menu_system.change_screen(&runMotorsSetPositionsMenu_screen1);
//Serial.println("drive");
indexValue = indexValueLCD-1;
///Serial.printf("poscount value %d \n", positionCount);
trueLoop = true;
while (trueLoop)
{
int oldspeed = speed;
myTransfer.tick(); //This gets the serial data and button values
if (inputArray[xbLB])
{
//Serial.printf("index value %d \n", indexValue);
if (indexValue - 1 >= 0)
{
indexValue--;
//Serial.printf("index value %d \n", indexValue);
indexValueLCD = indexValue + 1;
menu_system.update();
moveToIndex(indexValue);
}
inputArray[xbLB] = 0;
debugHandle();
}
else if (inputArray[xbRB])
{
//Serial.printf("index value %d \n", indexValue);
if (indexValue + 1 < positionCount)
{
indexValue++;
//Serial.printf("index value %d \n", indexValue);
indexValueLCD = indexValue + 1;
menu_system.update();
moveToIndex(indexValue);
}
inputArray[xbRB] = 0;
debugHandle();
}
lenghtStepper.setSpeed(inputArray[xbLSTICKAX]); //gamepad analog values
turnStepper.setSpeed(inputArray[xbRSTICKAX]); //gamepad analog values
tiltStepper.setSpeed(inputArray[xbRSTICKAY]); //gamepad analog values
lenghtStepper.runSpeed();
turnStepper.runSpeed();
tiltStepper.runSpeed();
if (oldspeed != speed)
{
menu_system.update();
}
if (inputArray[xbX])
{
//Serial.printf("index value %d \n", indexValue);
positions[indexValue][0] = lenghtStepper.currentPosition();
positions[indexValue][1] = turnStepper.currentPosition();
positions[indexValue][2] = tiltStepper.currentPosition();
inputArray[xbX] = 0; //Set the button value to not re trigger
if (indexValue + 1 < positionCount)//try to make it so that does not increment if almost at last position
{
if (positions[indexValue + 1][0] == 0 || positions[indexValue + 1][2] == 0 || positions[indexValue + 1][2] == 0)
{//Try to check if next value is zeroes maybe not correct
indexValue++;
indexValueLCD = indexValue + 1;
}
}
//Serial.printf("index value %d \n", indexValue);
menu_system.update();
}
}
gotoMain();
}
void moveToIndex(int index)
{
if (index < positionCount && index >= 0)
{
target_position[0] = positions[index][0];
target_position[1] = positions[index][1];
target_position[2] = positions[index][2];
turnStepper.setMaxSpeed(speed);
tiltStepper.setMaxSpeed(speed);
lenghtStepper.setMaxSpeed(speed);
float panInitialSpeed = turnStepper.speed();
float tiltInitialSpeed = tiltStepper.speed();
float sliderInitialSpeed = lenghtStepper.speed();
steppers.moveTo(target_position); //Sets new target positions //sets speeds
float sliderDeltaSpeed = lenghtStepper.speed() - sliderInitialSpeed;
float panDeltaSpeed = turnStepper.speed() - panInitialSpeed;
float tiltDeltaSpeed = tiltStepper.speed() - tiltInitialSpeed;
float sliderAccel = lenghtStepper.speed() / (slider_accel_increment_us * 0.0002);
float panAccel = turnStepper.speed() / (pan_accel_increment_us * 0.0002); //Equation is arbitrary and was deterined through empirical testing. The acceleration value does NOT correspond to mm/s/s
float tiltAccel = tiltStepper.speed() / (tilt_accel_increment_us * 0.0002);
long panDist = 0;
long tiltDist = 0;
long sliderDist = 0;
if (panAccel != 0)
{
panDist = pow(turnStepper.speed(), 2) / (5 * panAccel); //Equation is arbitrary and was deterined through empirical testing.
}
if (tiltAccel != 0)
{
tiltDist = pow(tiltStepper.speed(), 2) / (5 * tiltAccel); //Equation is arbitrary and was deterined through empirical testing.
}
if (sliderAccel != 0)
{
sliderDist = pow(lenghtStepper.speed(), 2) / (5 * sliderAccel); //Equation is arbitrary and was deterined through empirical testing.
}
if (index + 1 < positionCount)//What if when stopping this ain't called?
{ //makes sure there is a valid next keyframe
debugD("if index + 1 < positionCount \n");
long sliderStepDiff = positions[index + 1][0] - positions[index][0];
long panStepDiff = positions[index + 1][1] - positions[index][1];
long tiltStepDiff = positions[index + 1][2] - positions[index][2];
if ((sliderStepDiff == 0 && lenghtStepper.speed() != 0) || (sliderStepDiff > 0 && lenghtStepper.speed() < 0) || (sliderStepDiff < 0 && lenghtStepper.speed() > 0))
{ //if stopping or changing direction
target_position[0] = positions[index][0] - sliderDist; //If changing dir
}
if ((panStepDiff == 0 && turnStepper.speed() != 0) || (panStepDiff > 0 && turnStepper.speed() < 0) || (panStepDiff < 0 && turnStepper.speed() > 0))
{ //if stopping or changing direction
target_position[1] = positions[index][1] - panDist; //Set the target position slightly before the actual target to allow for the distance traveled while decelerating.
}
if ((tiltStepDiff == 0 && tiltStepper.speed() != 0) || (tiltStepDiff > 0 && tiltStepper.speed() < 0) || (tiltStepDiff < 0 && tiltStepper.speed() > 0))
{ //if stopping or changing direction
target_position[2] = positions[index][2] - tiltDist;
}
}
if (index > 0) //What if when stopping this ain't called?
{
long sliderStepDiffPrev = target_position[0] - positions[index - 1][0];
long panStepDiffPrev = target_position[1] - positions[index - 1][1]; //Change in position from the privious target to the current target position.
long tiltStepDiffPrev = target_position[2] - positions[index - 1][2];
//Making it negative ramps the speed down in the acceleration portion of the movement.
//The multiplication factor is arbitrary and was deterined through empirical testing.
if (sliderStepDiffPrev == 0 && sliderDeltaSpeed == 0)
{ //Movement stopping
sliderDeltaSpeed = -(2 * lenghtStepper.speed());
}
if (panStepDiffPrev == 0 && panDeltaSpeed == 0) //Movement stopping
{
panDeltaSpeed = -(2 * turnStepper.speed());
}
if (tiltStepDiffPrev == 0 && tiltDeltaSpeed == 0)
{ //Movement stopping
tiltDeltaSpeed = -(2 * tiltStepper.speed());
}
//debugD("Delta speeds lenght %ld turn %ld tilt %ld \n",sliderDeltaSpeed,panDeltaSpeed,tiltDeltaSpeed);
}
steppers.moveTo(target_position); //Sets new target positions and calculates new speeds.
if (turnStepper.currentPosition() != target_position[1] || tiltStepper.currentPosition() != target_position[2] || lenghtStepper.currentPosition() != target_position[0])
{ //Prevents issues caused when the motor target positions and speeds not being updated becuase they have not changed.
//Impliments the acceleration/deceleration. This implimentation feels pretty bad and should probably be updated but it works well enough so I'm not going to...
float panInc = 0;
float tiltInc = 0;
float sliderInc = 0;
unsigned long pan_last_us = 0;
unsigned long tilt_last_us = 0;
unsigned long slider_last_us = 0;
//debugD("panInc %f tiltInc %f sliderInc %f \n",panInc,tiltInc,sliderInc);
//debugD("pan_last_us %lu tilt_last_us %lu slider_last_us %lu \n",pan_last_us,tilt_last_us,slider_last_us);
while (((panInc < 1) || (tiltInc < 1) || (sliderInc < 1)) && steppers.run())
{
unsigned long usTime = micros();
if (usTime - pan_accel_increment_us >= pan_last_us)
{
panInc = (panInc < 1) ? (panInc + 0.01) : 1;
pan_last_us = micros();
turnStepper.setSpeed(panInitialSpeed + (panDeltaSpeed * panInc));
}
if (usTime - tilt_accel_increment_us >= tilt_last_us)
{
tiltInc = (tiltInc < 1) ? (tiltInc + 0.01) : 1;
tilt_last_us = micros();
tiltStepper.setSpeed(tiltInitialSpeed + (tiltDeltaSpeed * tiltInc));
}
if (usTime - slider_accel_increment_us >= slider_last_us)
{
sliderInc = (sliderInc < 1) ? (sliderInc + 0.01) : 1;
slider_last_us = micros();
lenghtStepper.setSpeed(sliderInitialSpeed + (sliderDeltaSpeed * sliderInc));
}
}
steppers.moveTo(target_position); //Sets all speeds to reach the target
steppers.runSpeedToPosition(); //Moves and blocks until complete
}
}
}
void executeMoves(int repeat){
lenghtStepper.setSpeed(0);
turnStepper.setSpeed(0);
tiltStepper.setSpeed(0);
for(int i = 0; i < repeat; i++){
for(int row = 0; row < positionCount; row++){
moveToIndex(row);
}
}
}
void runPositions()
{
executeMoves(1);
}
/*
//NOTE: THIS IS NOT THE PART OF THE CODE I'M HAVING PROBLEMS WITH
void runPositions()
{
//Go to first position
lenghtStepper.setMaxSpeed(4000);
turnStepper.setMaxSpeed(4000);
tiltStepper.setMaxSpeed(4000);
steppers.moveTo(positions[0]);
steppers.runSpeedToPosition();
delay(100);
for (indexValue = 0; indexValue < positionCount; indexValue++)
{
lenghtStepper.setMaxSpeed(speedpositions[indexValue]);
turnStepper.setMaxSpeed(speedpositions[indexValue]);
tiltStepper.setMaxSpeed(speedpositions[indexValue]);
steppers.moveTo(positions[indexValue]);
steppers.runSpeedToPosition();
}
}
*/
//NOTE: THIS IS NOT THE PART OF THE CODE I'M HAVING PROBLEMS WITH
void runPositionsLoop()
{
Serial.println("loop start");
lenghtStepper.setMaxSpeed(4000);
steppers.moveTo(positions[0]);
steppers.runSpeedToPosition();
delay(100);
while (true)
{
myTransfer.tick();
if (inputArray[xbY])
{
// Serial.println("break");
inputArray[xbY] = 0;
break;
}
// Serial.println("true");
for (int indVal = 0; indVal < positionCount; indVal++)
{
myTransfer.tick();
if (inputArray[xbY])
{
// Serial.println("break");
inputArray[xbY] = 0;
break;
}
Serial.print("In for 1st indval : ");
Serial.println(indVal);
lenghtStepper.setMaxSpeed(speed);
turnStepper.setMaxSpeed(speed);
tiltStepper.setMaxSpeed(speed);
steppers.moveTo(positions[indVal]);
steppers.runSpeedToPosition();
}
// Serial.println("2nd for");
myTransfer.tick();
if (inputArray[xbY])
{
// Serial.println("break");
inputArray[xbY] = 0;
break;
}
for (int indVal = positionCount - 1; indVal >= 0; indVal--)
{
myTransfer.tick();
if (inputArray[xbY])
{
// Serial.println("break");
inputArray[xbY] = 0;
break;
}
// Serial.print("In for 2nd indval : ");
// Serial.println(indVal);
lenghtStepper.setMaxSpeed(speed);
turnStepper.setMaxSpeed(speed);
tiltStepper.setMaxSpeed(speed);
steppers.moveTo(positions[indVal]);
steppers.runSpeedToPosition();
}
}
}
void buttonsCheckMenuNavigation() //Menu navigation code
{
if (inputArray[xbA])
{
menu_system.call_function(1); //Select
inputArray[xbA] = 0;
}
if (inputArray[xbRB])
{
}
if (inputArray[xbLB])
{
}
if (inputArray[xbDR])
{
menu_system.call_function(2); //Increase
inputArray[xbDR] = 0;
}
if (inputArray[xbDL])
{
menu_system.call_function(3); //Increase
inputArray[xbDL] = 0;
}
if (inputArray[xbDU])
{
menu_system.switch_focus(false); //Up in menu
inputArray[xbDU] = 0;
}
if (inputArray[xbDD])
{
menu_system.switch_focus(true); //Down in menu
inputArray[xbDD] = 0;
}
}
void callbackController()
{
//Serial.println("callback");
uint16_t recSize = 0;
recSize = myTransfer.rxObj(inputArray, recSize);
buttonsCheckMenuNavigation();
if (inputArray[xbLBA] < inputArray[xbRBA])
{
//Right trigger code
speedmodifier = map(inputArray[xbRBA], 0, 1000, 0, 250);
speed = speed + speedmodifier;
//Checks if speed is over max
if (speed > MaxTotalSpeed)
{
speed = MaxTotalSpeed;
}
}
else
{
//Left trigger code
speedmodifier = map(inputArray[xbLBA], 0, 1000, 0, 250);
speed = speed - speedmodifier;
//If speed is negative
if (speed < 0)
{
speed = 0;
}
}
//Serial.println(speed);
//Maps stick analog values to the speed
for (int i = 0; i < 4; i++)
{
inputArray[i] = map(inputArray[i], 0, 1000, 0, speed);
}
/*
for(int i = 0; i < xboxInputAmount; i++){
Serial.print(inputArray[i]);
Serial.print("|");
}
Serial.println();
*/
}
// supplied as a reference - persistent allocation required
const functionPtr callbackArr[] = {callbackController};
#include "functions.h"
void setup()
{
Serial1.begin(115200);
Serial.begin(115200);
Serial.println();
lcd.init();
lcd.backlight();
welcome_menu.init();
drive_menu.init();
welcome_line2.attach_function(1, gotodrive);
welcome_line3.attach_function(1, runPositions);
welcome_line4.attach_function(1, runPositionsLoop);
setIndexAmountMenu_line0.attach_function(1, gotoMain);
setIndexAmountMenu_line2.attach_function(2, increaseIndex);
setIndexAmountMenu_line2.attach_function(3, decreaseIndex);
setIndexAmountMenu_line3.attach_function(1, drivemotors);
resetIndex_line3.attach_function(2, resetIndexFunc1Increase);
resetIndex_line3.attach_function(3, resetIndexFunc1Decrease);
runMotorsSetPositionsMenu_line4.attach_function(1, breakWhileTrueDriveMotors);
//drive2_line4.attach_function(1, gotoMain);
menu_system.update();
///////////////////////////////////////////////////////////////////
configST myConfig;
myConfig.debug = true;
myConfig.callbacks = callbackArr;
myConfig.callbacksLen = sizeof(callbackArr) / sizeof(functionPtr);
///////////////////////////////////////////////////////////////////
myTransfer.begin(Serial1, myConfig);
lenghtStepper.setMaxSpeed(LENGHTMAXSPEED);
turnStepper.setMaxSpeed(TURNMAXSPEED);
tiltStepper.setMaxSpeed(TILTMAXSPEED);
lenghtStepper.setAcceleration(LENGHTMAXACCEL);
turnStepper.setAcceleration(TURNMAXACCEL);
tiltStepper.setAcceleration(TILITMAXACCEL);
steppers.addStepper(lenghtStepper);
steppers.addStepper(turnStepper);
steppers.addStepper(tiltStepper);
//Make sure that the position and speedposition arrays are zero
zeroPositions();
}
/*
//[Number of positions] [Number of axises]
// [lenght,turn,tilt]
int maxPositions = 10;
long positions[10][3];
long speedpositions[10]; //Speed value array
*/
void loop()
{
myTransfer.tick();
}
functions.h
void breakWhileTrueDriveMotors(){
trueLoop = false;
}
void zeroPositions()
{
for (int i = 0; i < maxPositions; i++)
{
for (int j = 0; j < 3; j++)
{
positions[i][j] = 0;
isModdedArray[i] = 0;
}
}
}
//Increase indexValue variable and checks if it is under the max value
void increaseIndex()
{
positionCount++;
if (positionCount > positionCount)
{
positionCount = maxPositions;
}
//Serial.printf("In inc ind %d", positionCount);
menu_system.update();
}
//Decreases indexValue variable and checks if it is over the min value
void decreaseIndex()
{
positionCount--;
if (positionCount < 2)
{
positionCount = 2;
}
//Serial.printf("In dec ind %d", positionCount);
menu_system.update();
}
//Called if increased *Increase is attached function number 3
//Don't reset positions
void resetIndexFunc1Increase(){
drivemotors();
}
//Called if decreased *decrease is attached function number 2
//Reset positions
void resetIndexFunc1Decrease(){
zeroPositions();
indexValue = 0;
indexValueLCD = indexValue + 1;
menu_system.change_screen(&setIndexAmountMenu_screen1);
}
void gotoMain()
{
menu_system.change_menu(welcome_menu);
}
void gotodrive()
{
if(firstSetPos == false){
menu_system.change_menu(drive_menu);
menu_system.change_screen(&setIndexAmountMenu_screen1);
zeroPositions();
firstSetPos = true;
}else{
menu_system.change_menu(drive_menu);
menu_system.change_screen(&resetIndex_screen1);
}
}
functiondeclaration.h
void drivemotors();
void zeroPositions();
void increaseIndex();
void decreaseIndex();
void gotoMain();
void moveToIndex(int index);
void breakWhileTrueDriveMotors();
variables.h
/*
Blueprint to changing speed like I want to
if(analogRead(inPin1) > analogRead(inPin2)){
int speedmodifier = map(analogRead(inPin1),0,1023,0,80);
speed = speed + speedmodifier;
}else
{
int speedmodifier = map(analogRead(inPin2),0,1023,0,80);
speed = speed - speedmodifier;
}
*/
/*
* SerialTransfer variables
*/
const int xboxInputAmount = 21; //How many xbox values there will be
int32_t inputArray[xboxInputAmount]; //This will contain every xbox controller outputs
const int xbLSTICKAX = 0;//Left stick X axis
const int xbLSTICKAY = 1;//Left stick Y axis
const int xbLBA = 4;//Left trigger axis
const int xbRSTICKAX = 2;//Right stick X axis
const int xbRSTICKAY = 3; //Right stick Y axis
const int xbRBA = 5;//Right trigger axis
const int xbA = 6;//Button A
const int xbB = 7;//Button B
const int xbX = 8;//Button X
const int xbY = 9;//Button Y
const int xbLB = 10;//Button Left Bumber
const int xbRB = 11;//Button Right Bumber
const int xbSHARE = 12;//Button Share
const int xbSETTINGS = 13;//Button Settings
const int xbEMBLEM = 14;//Button Emblem
const int xbLSTICKD = 15;//Left stick down
const int xbRSTICKD = 16;//Right stick down
const int xbDL = 17;//Dpad left
const int xbDR = 18;//Dpad Right
const int xbDU = 19;//Dpad up
const int xbDD = 20;//Dpad down
/* If the serialtransfer needs data to be sent back
for(uint16_t i=0; i < myTransfer.bytesRead; i++)
myTransfer.packet.txBuff[i] = myTransfer.packet.rxBuff[i];
myTransfer.sendData(myTransfer.bytesRead);
*/
/*
* End of SerialTransfer variables
*/
/*
*Start of LiquidMenu variables
*/
#define lcd_address 0x27
#define lcd_width 20
#define lcd_height 4
/*
*End of LiquidMenu variables
*/
/*
*Start of Accel stepper
*/
#define LENGHT_DIR_PIN 16
#define LENGHT_STEP_PIN 17
#define TURN_DIR_PIN 14
#define TURN_STEP_PIN 15
#define TILT_DIR_PIN 12
#define TILT_STEP_PIN 11
int LENGHTMMSTEP = 100; // Multiply steps by this number to get mm travel
int TURNDEGSTEP = 50; // Multiply steps by this number to get degrees travel
int TILTDEGSTEP = 30; //Multiply steps by this number to get degrees travel
long MaxTotalSpeed = 8000;
long speed = MaxTotalSpeed/2; //Default step speed
long speedmodifier = 0; //Used to set speed variable
int LENGHTMAXSPEED = MaxTotalSpeed;
int LENGHTMAXACCEL = 400;
int TURNMAXSPEED = MaxTotalSpeed;
int TURNMAXACCEL = 400;
int TILTMAXSPEED = MaxTotalSpeed;
int TILITMAXACCEL = 400;
bool firstSetPos = false;
bool trueLoop = true;
//[Number of positions] [Number of axises]
// [lenght,turn,tilt]
int maxPositions = 10;
long positions[10][3];
byte positionCount = 2;
byte indexValue = 0;
byte indexValueLCD = indexValue + 1;
/*
*End of accel stepper
*/
int slider_accel_increment_us = 2000; //Lower = tighter
int pan_accel_increment_us = 2000;
int tilt_accel_increment_us = 2000;
byte acceleration_enable_state = 0;
long target_position[3] = {0,0,0};