ezWheelSAS / swd_ros_controllers

ROS nodes to control motors powered by the ez-Wheel Safety Wheel Drive (SWD®) technology.
https://www.ez-wheel.com/
GNU Lesser General Public License v2.1
5 stars 4 forks source link

STO behavior #57

Closed Koma2yu closed 1 year ago

Koma2yu commented 1 year ago

The AMR starts running as soon as the e-stop(STO)is released. So, in commissioning, I set restart_ acknowledge_behavior to ”ture”. When I started AMR in that state, the red light on the LEFT side motor LED was always on and it was stop running. When I set the restart_acknowledge_behavior of the left side back to “false” again, it started working. However, the right motor did not return when the emergency stop was released after the STO , but the left motor would return and start running when the emergency stop was released.

What should I do to prevent the AMR starts running as soon as the e-stop(STO)is released (for both the right and left motors)? I would like to solve this problem with software.

GMezWheel commented 1 year ago

Hi @Koma2yu ,

You have to modify the following callback into DiffDriveController::cbTimerStateMachine().

...
                // If STO detected, exit
                if (sto_signal) {
                    return;
                }
...
                if (smccore::IPDSService::PDSState::OPERATION_ENABLED != pds_state_l) {
                    err_l = m_left_controller.enterInOperationEnabledState();
                }

                if (smccore::IPDSService::PDSState::OPERATION_ENABLED != pds_state_r) {
                    err_r = m_right_controller.enterInOperationEnabledState();
                }
...

Indeed, as soon as the STO is released, this callback try to change NMT & PDS state to OPER automatically (even if acknowledge_behavior is True) : => enterInOperationEnabledState()

As a suggestion, add you own pre-requesites before the call of enterInOperationEnabledState().