Synchonous acceleration with Accelstepper library

Projects Programming
1

Hi,

For a robot I use the accelstepper library.
When accelerating two steppers I see a minimal difference between stepperL.speed() and stepperR.speed() during the acceleration phase – resulting in minimal difference in stepperL/R currentPosition(). As stated it’s minimal. But the dead reckoning calculation still results in an angle change which results in a correction by changing the MaxSpeed. I would like to prevent this.

I assume that this happens because the calculation of new speeds and next step time doesn’t take place at exactly the same time. So there most likely are differences in rounding calculation results. Anyhow – that’s only speculation.

Does someone have similar experiences and/or knows a way to do the acceleration exactly synchronous?

The code is too big to be posted as I use finite state machines. But the .run() functions for both steppers run directly after each other in the code.

Thanks
Robert

2

The code is too big to be posted

Then you need to create a smaller sketch that illustrates just the problem you are having, and post that.

If you can, the problem may be solvable. If you can't, then it is likely that your problem lies in the amount of code that you have, and the time it takes to get back around to the stepper instances.

3

Thanks for yor reply.

I'll try to extract the relevant code. Turnaround time sould be no problem as most of the sketch (beside odometry and the FSM for driving) is disabled at the moment for testing. In addition I would expect both steppers to be too slow/fast/unsteady - but not to differ.

What I would expect is that while beeing in state Drive_GoTo - speed and currentPosition for both steppers are always the same. But there is a minimal offset until it reaches full speed. Not big, just a few steps.

It works like this:

  1. via serial the coords of the new target are submitted.
  2. GoTo(String Coords) changes the state of the FSM to Drive_Goto
  3. the entry Function Drive_GoTo_Start_F() calculates the relevant steps to go to the target and sets the new target for the steppers. In theory they can differ between both wheels - but at the moment WHEEL_DIAMETERl and WHEEL_DIAMETERr are equal, so the amount of steps are the same. Also correcting the direction before and while driving straigt is disabled at the moment.
  4. the FSM stays in state Drive_GoTo until all stepps are processed

definitions:

#include <FiniteStateMachine.h>
#include <AccelStepper.h>

//stepper
AccelStepper stepperL(1,StepLStep,StepLDir);
AccelStepper stepperR(1,StepRStep,StepRDir);

#define stepper_highspeed 500.0
#define stepper_lowspeed 200.0
#define stepper_acceleration 500.0

//dead reckoning
float MUL_COUNTl;
float MUL_COUNTr;
float MUL_COUNT_avg;
#define WHEEL_DIAMETERl 8.05
#define WHEEL_DIAMETERr 8.05
#define PULSES_PER_REVOLUTION 800
#define AXLE_LENGTH 14.1

//FSM
State Drive_Idle = State(Drive_Idle_Entry_F , Drive_Idle_F, NULL);
State Drive_GoTo = State(GoTo_start_F, Drive_GoTo_F, NULL);
State Drive_Turn = State(Turn_start_F, Drive_Turn_F, NULL);
State Drive_Align = State(NULL, Drive_Align_F, NULL);
FSM FSM_Drive = FSM(Drive_Idle);

function to start driving staight:

void GoTo(String Coords){
X_pos_target=convertStringtoFloat(splitString(Coords,';',0));
Y_pos_target=convertStringtoFloat(splitString(Coords,';',1));
FSM_Drive.transitionTo(Drive_GoTo);
}

relevant FSM code:

void GoTo_start_F(){

	DriveMillis = millis();

	TargetDist = DistToTarget (Y_pos, X_pos, Y_pos_target, X_pos_target);
	stepperL.moveTo(stepperL.currentPosition()+TargetDist/MUL_COUNTl);
	stepperR.moveTo(stepperR.currentPosition()+TargetDist/MUL_COUNTr);}
}


void Drive_GoTo_F () {

	unsigned long lastCalc = millis() - DriveMillis;

	if (lastCalc >= 10) {

		/********* check if target reached **********/
		if(stepperL.distanceToGo() == 0 && stepperL.distanceToGo() == 0) {
			FSM_Drive.immediateTransitionTo(Drive_Idle);
		}

		DriveMillis = millis();
	}
}

function for moving the stepper (in main loop):

void stepperUpdate(){
	stepperL.run();
	stepperR.run();
}

Odometer:

/*********************/
/* define structures */
/*********************/

struct position
{
	float x;        /* meter */
	float y;        /* meter */
	float theta;    /* radian (counterclockwise from x-axis) */
};


/********************/
/* global variables */
/********************/

struct position current_position;


/********************/
/* define functions */
/********************/

void initialize_odometry()
{
	current_position.x = 0.0;
	current_position.y = 0.0;
	current_position.theta = 0.0;
}

void odometer_thread()
{
	if (millis() - odometersmillis >= 10) {

	float dist_left;
	float dist_right;
	int left_ticks;
	int right_ticks;
	float expr1;
	float cos_current;
	float sin_current;
	float right_minus_left;
	float right_minus_left_AXL = 0;
	float right_minus_left_AXL_theta;

	lsamp = stepperL.currentPosition();
	rsamp = stepperR.currentPosition(); 


	left_ticks = lsamp - last_left;
	right_ticks = rsamp - last_right;

	last_left = lsamp;
	last_right = rsamp;

	dist_left = (float)left_ticks * MUL_COUNTl;
	dist_right = (float)right_ticks * MUL_COUNTr;

	cos_current = cos(current_position.theta);
	sin_current = sin(current_position.theta);

	if (dist_left == dist_right)
	{
		current_position.x += dist_left * cos_current;
		current_position.y += dist_left * sin_current;
	}
	else
	{

		right_minus_left = dist_right - dist_left;

		expr1 = AXLE_LENGTH * (dist_right + dist_left) / 2 / right_minus_left;

		right_minus_left_AXL = right_minus_left / AXLE_LENGTH;
		right_minus_left_AXL_theta = right_minus_left_AXL + current_position.theta;

		current_position.x += expr1 * (sin(right_minus_left_AXL_theta) - sin_current);
		current_position.y -= expr1 * (cos(right_minus_left_AXL_theta) - cos_current);

		current_position.theta += right_minus_left_AXL;

		if (current_position.theta > PI)
			current_position.theta -= (2.0*PI);
		if (current_position.theta < -PI) 
			current_position.theta += (2.0*PI); 
	} 

	theta = current_position.theta;
	X_pos = current_position.x;
	Y_pos = current_position.y;

	odometersmillis = millis();

	}
}

relevant code from setup:

MUL_COUNTl  = PI * WHEEL_DIAMETERl / PULSES_PER_REVOLUTION;
MUL_COUNTr  = PI * WHEEL_DIAMETERr / PULSES_PER_REVOLUTION;
MUL_COUNT_avg = (MUL_COUNTl + MUL_COUNTr) / 2;

stepperL.setMaxSpeed(stepper_highspeed);
stepperL.setAcceleration(stepper_acceleration);

stepperR.setMaxSpeed(stepper_highspeed);
stepperR.setAcceleration(stepper_acceleration);

full main loop:

void loop() {

	odometer_thread();

	//PingMainLoop();

	FSM_Drive.update();
	//FSM_IR.update();

	stepperUpdate();

	//TIMER_UBAtest.update();
	//TIMER_FSM_IR.update();
	//TIMER_FSM_DRIVE.update(); -- no longer needed?
	//TIMER_StepperPowerOff.update();
	output(false);
}