synthetos / TinyG

Affordable Industrial Grade Motion Control
https://github.com/synthetos/TinyG/wiki
887 stars 293 forks source link

Motors can run in the incorrect direction when the motor steps per turn parameter is set after the motor has already moved. #267

Open jay-sheldon-Nortech opened 3 years ago

jay-sheldon-Nortech commented 3 years ago

Motors can run in the incorrect direction when the motor steps per turn parameter is set after the motor has already moved. This can occur when motor parameter setup commands are re-issued on a system that has already been running. In my case, the controller driving TinyG re-ran initialization without resetting TinyG.

The sequence to reproduce the issue is as follows: Setup motor parameters Move motor opposite the initial direction. ( Currently STEP_INITIAL_DIRECTION is defined to DIRECTION_CW, so this would be CCW). Issue the command to update the motor steps per revolution. ( xtr, eg 1tr for motor 1 ) Move motor in the initial direction, due to the bug the motor will move opposite of the direction in the command.

set_motor_steps_perunit calls _streset, which updates _st_pre.mot[motor].prevdirection to STEP_INITIAL_DIRECTION but the motor output polarity in the VPORT is not updated. When a move command to move is issued, loadmove checks the _prevdirection to determine if the direction bit in the VPORT needs to be updated and bypasses the update if _prevdirection matches the present direction. Because the VPORT and prev direction do not match and the update is skipped, the motor moves in the incorrect direction.

jay-sheldon-Nortech commented 3 years ago

I tested a fix locally by always updating the VPORT direction bit when st_reset is called. This worked for my use case. An alternative fix would be to initialize prev_direction to something other than DIRECTION_CW or DIRECTION_CCW to always trigger an update of the direction setting when _load_move is called after the st_reset.

void st_reset() { for (uint8_t motor=0; motor<MOTORS; motor++) { st_pre.mot[motor].prev_direction = STEP_INITIAL_DIRECTION; st_run.mot[motor].substep_accumulator = 0; // will become max negative during per-motor setup; st_pre.mot[motor].corrected_steps = 0; // diagnostic only - no action effect }

// STEP_INITIAL_DIRECTION  = DIRECTION_CW. This fixes an issue where the motor could run in the incorrect direction when st_reset is called.

PORT_MOTOR_1_VPORT.OUT &= ~DIRECTION_BIT_bm;
PORT_MOTOR_2_VPORT.OUT &= ~DIRECTION_BIT_bm;
PORT_MOTOR_3_VPORT.OUT &= ~DIRECTION_BIT_bm;
PORT_MOTOR_4_VPORT.OUT &= ~DIRECTION_BIT_bm;
mp_set_steps_to_runtime_position();

}

jay-sheldon-Nortech commented 3 years ago

I tested a fix locally by always updating the VPORT direction bit when st_reset is called. This worked for my use case. An alternative fix would be to initialize prev_direction to something other than DIRECTION_CW or DIRECTION_CCW to always trigger an update of the direction setting when _load_move is called after the st_reset.

void st_reset()
{
    for (uint8_t motor=0; motor<MOTORS; motor++) {
        st_pre.mot[motor].prev_direction = STEP_INITIAL_DIRECTION;
        st_run.mot[motor].substep_accumulator = 0;  // will become max negative during per-motor setup;
        st_pre.mot[motor].corrected_steps = 0;      // diagnostic only - no action effect
    }

    // STEP_INITIAL_DIRECTION  = DIRECTION_CW. This fixes an issue where the motor could run in the incorrect direction when st_reset is called.

    PORT_MOTOR_1_VPORT.OUT &= ~DIRECTION_BIT_bm;
    PORT_MOTOR_2_VPORT.OUT &= ~DIRECTION_BIT_bm;
    PORT_MOTOR_3_VPORT.OUT &= ~DIRECTION_BIT_bm;
    PORT_MOTOR_4_VPORT.OUT &= ~DIRECTION_BIT_bm;
    mp_set_steps_to_runtime_position();
}