ATY_LIB/Doing/MOTOR_DC_ATY.c

/**
* @file MOTOR_DC_ATY.c
*
* @param Project DEVICE_GENERAL_ATY_LIB
*
* @author ATY
*
* @copyright
*       - Copyright 2017 - 2026 MZ-ATY
*       - This code follows:
*           - MZ-ATY Various Contents Joint Statement -
*               <a href="https://mengze.top/MZ-ATY_VCJS">
*                        https://mengze.top/MZ-ATY_VCJS</a>
*           - CC 4.0 BY-NC-SA -
*               <a href="https://creativecommons.org/licenses/by-nc-sa/4.0/">
*                        https://creativecommons.org/licenses/by-nc-sa/4.0/</a>
*       - Your use will be deemed to have accepted the terms of this statement.
*
* @brief functions of DCMotor control
*
* @version
*       - 1_01_230426 > ATY
*           -# Preliminary version, first Release
*       - 1_01_240112 > ATY
*           -# add multy addr and channel
*           -# add lock
********************************************************************************
*/

#ifndef __MOTOR_DC_ATY_C
#define __MOTOR_DC_ATY_C

#include "MOTOR_DC_ATY.h"

/******************************* For user *************************************/

/******************************************************************************/

/**
 * @brief   start motor with direction set
 *
 * @param   dir direction to move
 * @param   speed direction to move
 * @param   dev
 * @return  uint8_t
 */
uint8_t MOTOR_DC_Start(uint8_t dir, uint32_t speed, struct MOTOR_DC_ATY_Dev* dev)
{
    __ATY_LOCK(dev);
    dev->motorEn = 1;
    dev->motorDir = dir;
    if(dir){
        dev->signalSet('A', 0);
        dev->signalSet('B', speed);
    }
    else{
        dev->signalSet('A', speed);
        dev->signalSet('B', 0);
    }
    __ATY_UNLOCK(dev);
    return 0;
}

/**
 * @brief   stop motor
 *
 * @param   dev
 * @return  uint8_t
 */
uint8_t MOTOR_DC_Stop(struct MOTOR_DC_ATY_Dev* dev)
{
    if(dev->motorEn == 0)
        return 1;
    __ATY_LOCK(dev);
    dev->motorEn = 0;
    dev->signalSet('A', 0);
    dev->signalSet('B', 0);
    dev->motorSpeedCounter = 0;
    // MOTOR_DC_PARAM_SET(0, 0, 0, 0, 0, 0, dev);
    __ATY_UNLOCK(dev);
    return 0;
}


/**
 * @brief   deal step motor state
 *
 * @param   dev
 * @note    put at 1ms cycle;
 *          set motorSetState = 4 to change motor dir with speed no changed
 *          set motorSetState = 3 to start motor directly, motorSoftSteps need to be 0
 *          set motorSetState = 2 to strat motor with soft, motorDir/0/motorSpeed/motorSoftSteps/motorSteps need to be set
 *          set motorSetState = 0 to stop motor flow
 *          set motorSetState = 10 to scram motor
 *          when motorSetState = 5 means last cycle finished, set others to start a new cycle
 *          when motorSetState = 1 means motor running and state not changing
 */
void MOTOR_DC_StateMachine_PWM(struct MOTOR_DC_ATY_Dev* dev)
{
    if(dev->motorSetState == MOTOR_DC_SET_REVERSE){
        dev->motorStepCounter = 0;
        dev->motorSetState = MOTOR_DC_SET_START_SOFT;
        dev->motorDir = !dev->motorDir;
    }
    else if(dev->motorSetState == MOTOR_DC_SET_START_DIRECTLY){
        dev->motorStepCounter = dev->motorSoftSteps;
        MOTOR_DC_Start(dev->motorDir, dev->motorSpeed, dev);
        dev->motorSetState = MOTOR_DC_SET_RUNNING;
    }
    else if(dev->motorSetState == MOTOR_DC_SET_START_SOFT){
        dev->motorStepCounter = 0;
        MOTOR_DC_Start(dev->motorDir, 0, dev);
        dev->motorSetState = MOTOR_DC_SET_RUNNING;
    }
    else if(dev->motorSetState == MOTOR_DC_SET_RUNNING){
        // if(dev->motorStepCounter < dev->motorSoftSteps){
        //     if(dev->lineProType == 'T'){
        //         dev->speedIncreaseStep = ((float)(0 - dev->motorSpeed) / (float)dev->motorSoftSteps);  // at 1ms cycle
        //         MOTOR_DC_Start(dev->motorDir,
        //             dev->motorSpeed < (0 - (dev->speedIncreaseStep * dev->motorStepCounter)) ?
        //             (0 - (dev->speedIncreaseStep * dev->motorStepCounter)) : dev->motorSpeed, dev);
        //     }
        //     else if(dev->lineProType == 'S'){
        //         // S line: y = ax^3 + cx, a in 0-1, c >= 0, lower c cause lower increase at start or end, c for users
        //         float S_param_A = (((float)(0 - dev->motorSpeed) / 2)
        //             - (dev->lineProParam * (float)((float)dev->motorSoftSteps / 2) * (float)((float)dev->motorSoftSteps / 2) / 2))
        //             / ((float)((float)dev->motorSoftSteps / 2) * (float)((float)dev->motorSoftSteps / 2)
        //                 * (float)((float)dev->motorSoftSteps / 2) * (float)((float)dev->motorSoftSteps / 2)) * 4;
        //         // mbG[27] = S_param_A * 10000000;
        //         if(dev->motorStepCounter < (dev->motorSoftSteps / 2))
        //             dev->speedIncreaseStep =
        //             ((float)(S_param_A * dev->motorStepCounter * dev->motorStepCounter * dev->motorStepCounter * dev->motorStepCounter) / 4)
        //             + ((float)(dev->lineProParam * dev->motorStepCounter * dev->motorStepCounter) / 2);
        //         // (S_param_A * dev->motorStepCounter * dev->motorStepCounter * dev->motorStepCounter)
        //         // + (lineProParam * dev->motorStepCounter);
        //         else
        //             dev->speedIncreaseStep =
        //             (float)((float)(0 - dev->motorSpeed) / 2)
        //             + ((float)(S_param_A * dev->motorSoftSteps * dev->motorSoftSteps * dev->motorSoftSteps * dev->motorSoftSteps) / 64)
        //             + ((float)(dev->lineProParam * dev->motorSoftSteps * dev->motorSoftSteps) / 8)
        //             - ((float)(S_param_A * (dev->motorSoftSteps - dev->motorStepCounter) * (dev->motorSoftSteps - dev->motorStepCounter)
        //                 * (dev->motorSoftSteps - dev->motorStepCounter) * (dev->motorSoftSteps - dev->motorStepCounter)) / 4)
        //             - ((float)(dev->lineProParam * (dev->motorSoftSteps - dev->motorStepCounter) * (dev->motorSoftSteps - dev->motorStepCounter)) / 2);
        //             // 0
        //             // - (S_param_A * (float)((float)dev->motorStepCounter - (float)dev->motorSoftSteps)
        //             //     * (float)((float)dev->motorStepCounter - (float)dev->motorSoftSteps)
        //             //     * (float)((float)dev->motorStepCounter - (float)dev->motorSoftSteps))
        //             // - (dev->lineProParam * (float)((float)dev->motorStepCounter - (float)dev->motorSoftSteps));
        //         MOTOR_DC_Start(dev->motorDir,
        //             dev->motorSpeed < (0 - dev->speedIncreaseStep) ?
        //             (0 - dev->speedIncreaseStep) : dev->motorSpeed, dev);
        //     }
        //     else if(dev->lineProType == '2'){}
        // }

        if(dev->motorStepCounter == dev->motorSoftSteps)
            dev->motorSetState = MOTOR_DC_SET_START_DIRECTLY;
        else if(dev->motorStepCounter >= (dev->motorSoftSteps + dev->motorSteps))
            dev->motorSetState = MOTOR_DC_SET_FINISHED;
    }
    else if(dev->motorSetState == MOTOR_DC_SET_SCRAM){
        MOTOR_DC_Stop(dev);
        dev->motorSetState = MOTOR_DC_SET_STOP;
    }
    else{ MOTOR_DC_Stop(dev); }    // 5 0

    if(dev->motorEn == 0){
        dev->motorStepCounter = 0;
    }
    else{
        dev->motorStepCounter++;
    }
}


/**
 * @brief   deal step motor state
 *
 * @param   dev
 * @note    put at 1ms cycle;
 *          set motorSetState = 4 to change motor dir with speed no change
 *          set motorSetState = 3 to start motor directly, motorSoftSteps need to be 0
 *          set motorSetState = 2 to strat motor with soft, motorDir/0/motorSpeed/motorSoftSteps/motorTime need to be set
 *          set motorSetState = 0 to stop motor
 *          set motorSetState = 10 to scram motor
 *          when motorSetState = 5 means last cycle finished, set others to start a new cycle
 *          when motorSetState = 1 means motor running and state not changing
 */
void MOTOR_DC_StateMachine(struct MOTOR_DC_ATY_Dev* dev)
{
    if(dev->motorSetState == 4){
        dev->motorSpeedCounter = 0;
        dev->motorSetState = 1;
        dev->motorDir = !dev->motorDir;
    }
    else if(dev->motorSetState == 3){
        dev->motorSpeedCounter = dev->motorSoftSteps;
        dev->motorSetState = 1;
        MOTOR_DC_Start(dev->motorDir, dev->motorSpeed, dev);
    }
    else if(dev->motorSetState == 2){
        dev->motorSpeedCounter = 0;
        dev->motorSetState = 1;
        MOTOR_DC_Start(dev->motorDir, 0, dev);
    }
    else if(dev->motorSetState == 1){
        if(dev->motorSpeedCounter == dev->motorSoftSteps)
            dev->motorSetState = 3;
        else if(dev->motorSpeedCounter == (dev->motorSoftSteps + dev->motorSteps))
            dev->motorSetState = 5;
    }
    else if(dev->motorSetState == 10){
        dev->motorSetState = 0;
        MOTOR_DC_Stop(dev);
    }
    else{ MOTOR_DC_Stop(dev); }    // 5 0

    if(dev->motorEn == 0){
        dev->motorSpeedCounter = 0;
    }
    else{
        dev->motorSpeedCounter++;
    }
}


#endif /* __MOTOR_DC_ATY_C */

/******************************** End Of File *********************************/