RobotSimulation/pick_and_place/ROS/src/niryo_one_ros/niryo_one_driver/src/hw_comm/can_communication.cpp
int CanCommunication::relativeMoveMotor(StepperMotorState* motor, int steps, int delay, bool wait)
{
    if (!motor->isEnabled()) {
        return CAN_OK;
    }

    if (can->sendTorqueOnCommand(motor->getId(), true) != CAN_OK) {
        ROS_ERROR("Failed to send torque ON to motor %d", motor->getId());
        return CAN_FAIL;
    }

    if (can->sendRelativeMoveCommand(motor->getId(), steps, delay) != CAN_OK) {
        ROS_ERROR("Relative Move motor failed for motor %d", motor->getId());
        return CAN_FAIL;
    }
    if (wait) {
        ros::Duration(steps*delay/1000000 + 0.5).sleep(); // wait for 0.5 sec more to finish
    }

    return CAN_OK;
}
/*
 * To use only during calibration phase, or for debug purposes
 * - Move motor from whatever current position to current_steps + steps
 */
To use only during calibration phase, or for debug purposes
- Move motor from whatever current position to current_steps + steps
To use only during calibration phase or for debug purposes - Move motor from whatever current position to current_steps + steps
int CanCommunication::relativeMoveMotor(StepperMotorState* motor, int steps, int delay, bool wait)
{
    if (!motor->isEnabled()) {
        return CAN_OK;
    }
    if (can->sendTorqueOnCommand(motor->getId(), true) != CAN_OK) {
        ROS_ERROR("Failed to send torque ON to motor %d", motor->getId());
        return CAN_FAIL;
    }
    if (can->sendRelativeMoveCommand(motor->getId(), steps, delay) != CAN_OK) {
        ROS_ERROR("Relative Move motor failed for motor %d", motor->getId());
        return CAN_FAIL;
    }
    if (wait) {
        ros::Duration(steps*delay/1000000 + 0.5).sleep(); 
    }
    return CAN_OK;
}
int CanCommunication :: relativeMoveMotor ( StepperMotorState * motor , int steps , int delay , bool wait ) { if ( ! motor -> isEnabled ( ) ) { return CAN_OK ; } if ( can -> sendTorqueOnCommand ( motor -> getId ( ) , true ) != CAN_OK ) { ROS_ERROR ( " " , motor -> getId ( ) ) ; return CAN_FAIL ; } if ( can -> sendRelativeMoveCommand ( motor -> getId ( ) , steps , delay ) != CAN_OK ) { ROS_ERROR ( " " , motor -> getId ( ) ) ; return CAN_FAIL ; } if ( wait ) { ros :: Duration ( steps * delay / 1000000 + 0.5 ) . sleep ( ) ; } return CAN_OK ; }
To use only during calibration phase, or for debug purposes
 - Move motor from whatever current position to current_steps + steps
To use only during calibration phase or for debug purposes - Move motor from whatever current position to current_steps + steps
// wait for 0.5 sec more to finish