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);

}