/**
* @file MOTOR_DC_ATY.c
*
* @param Project DEVICE_GENERAL_ATY_LIB
*
* @author ATY
*
* @copyright
* - Copyright 2017 - 2025 MZ-ATY
* - This code follows:
* - MZ-ATY Various Contents Joint Statement -
*
* https://mengze.top/MZ-ATY_VCJS
* - CC 4.0 BY-NC-SA -
*
* https://creativecommons.org/licenses/by-nc-sa/4.0/
* - Your use will be deemed to have accepted the terms of this statement.
*
* @brief Familiar 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 *********************************/