本帖最后由 ColeYao 于 2025-7-22 10:36 编辑
已找到新算法其速度曲线接近理想的S曲线，如图从上到下依次为目标位置，实际电机位置，电机速度（图二使用一个定时器实时控制四路电机速度），电机加速度!
代码：
void TestMove(uchar MtNo)
{
switch(MtNo)
{
case 1:
Mt1.MoveT0(200*Mt1.RadioV,800);
while(Mt1.Runing) osDelay(1);
osDelay(500);
Mt1.MoveT0(400*Mt1.RadioV,200);
while(Mt1.Runing) osDelay(1);
osDelay(500);
Mt1.MoveT0(1600*Mt1.RadioV,800);
while(Mt1.Runing) osDelay(1);
osDelay(500);
Mt1.MoveT0(800*Mt1.RadioV,800); //220转速配合32细分时共振明显
while(Mt1.Runing) osDelay(1);
osDelay(500);
break;
case 2:
Mt2.MoveT0(200*Mt2.RadioV,800);
while(Mt2.Runing) osDelay(1);
osDelay(500);
Mt2.MoveT0(400*Mt2.RadioV,200);
while(Mt2.Runing) osDelay(1);
osDelay(500);
Mt2.MoveT0(1600*Mt2.RadioV,800);
while(Mt2.Runing) osDelay(1);
osDelay(500);
Mt2.MoveT0(800*Mt2.RadioV,800); //220转速配合32细分时共振明显
while(Mt2.Runing) osDelay(1);
osDelay(500);
break;
case 3:
Mt3.MoveT0(200*Mt3.RadioV,800);
while(Mt3.Runing) osDelay(1);
osDelay(500);
Mt3.MoveT0(400*Mt3.RadioV,200);
while(Mt3.Runing) osDelay(1);
osDelay(500);
Mt3.MoveT0(1600*Mt3.RadioV,800);
while(Mt3.Runing) osDelay(1);
osDelay(500);
Mt3.MoveT0(800*Mt3.RadioV,800); //220转速配合32细分时共振明显
while(Mt3.Runing) osDelay(1);
osDelay(500);
break;
case 4:
Mt4.MoveT0(200*Mt4.RadioV,800);
while(Mt4.Runing) osDelay(1);
osDelay(500);
Mt4.MoveT0(400*Mt4.RadioV,200);
while(Mt4.Runing) osDelay(1);
osDelay(500);
Mt4.MoveT0(1600*Mt4.RadioV,800);
while(Mt4.Runing) osDelay(1);
osDelay(500);
Mt4.MoveT0(800*Mt4.RadioV,800); //220转速配合32细分时共振明显
while(Mt4.Runing) osDelay(1);
osDelay(500);
break;
}
}
void MonMtr(uchar MtNo)
{
switch(MtNo)
{
case 1:
Mon1.DesPos[0]=Mt1.Pos_Des;
Mon1.Pos[0]=Mt1.Pos_Pul;
Mon1.ACpm[0]=Mt1.v_CirpMin-Mon1.VCpm[0];
Mon1.VCpm[0]=Mt1.v_CirpMin;
break;
case 2:
Mon1.DesPos[1]=Mt2.Pos_Des;
Mon1.Pos[1]=Mt2.Pos_Pul;
Mon1.ACpm[1]=Mt2.v_CirpMin-Mon1.VCpm[1];
Mon1.VCpm[1]=Mt2.v_CirpMin;
break;
case 3:
Mon1.DesPos[2]=Mt3.Pos_Des;
Mon1.Pos[2]=Mt3.Pos_Pul;
Mon1.ACpm[2]=Mt3.v_CirpMin-Mon1.VCpm[2];
Mon1.VCpm[2]=Mt3.v_CirpMin;
break;
case 4:
Mon1.DesPos[3]=Mt4.Pos_Des;
Mon1.Pos[3]=Mt4.Pos_Pul;
Mon1.ACpm[3]=Mt4.v_CirpMin-Mon1.VCpm[3];
Mon1.VCpm[3]=Mt4.v_CirpMin;
break;
}
}
/* USER CODE END Header_StartDefaultTask */
void StartDefaultTask(void *argument)
{
/* USER CODE BEGIN StartDefaultTask */
/* Infinite loop */
for(;;)
{
osDelay(10);
TestMove(4);
}
/* USER CODE END StartDefaultTask */
}
/* USER CODE BEGIN Header_StartTask02 */
/**
* @brief Function implementing the myTask02 thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_StartTask02 */
void StartTask02(void *argument)
{
/* USER CODE BEGIN StartTask02 */
/* Infinite loop */
for(;;)
{
osDelay(5);
Mt1.Move();
Mt2.Move();
Mt3.Move();
Mt4.Move();
MonMtr(1);
MonMtr(2);
MonMtr(3);
MonMtr(4);
}
/* USER CODE END StartTask02 */
}
/* USER CODE BEGIN Header_StartTask03 */
/**
* @brief Function implementing the myTask03 thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_StartTask03 */
void StartTask03(void *argument)
{
/* USER CODE BEGIN StartTask03 */
/* Infinite loop */
for(;;)
{
osDelay(10);
TestMove(3);
}
/* USER CODE END StartTask03 */
}
/* Private application code --------------------------------------------------*/
/* USER CODE BEGIN Application */
void StartTask04(void *argument)
{
/* USER CODE BEGIN StartTask04 */
/* Infinite loop */
for(;;)
{
osDelay(10);
TestMove(2);
}
/* USER CODE END StartTask04 */
}
void StartTask05(void *argument)
{
/* USER CODE BEGIN StartTask05 */
/* Infinite loop */
for(;;)
{
osDelay(10);
TestMove(1);
}
/* USER CODE END StartTask05 */
}
|
楼主
