150 lines
4.1 KiB
C
150 lines
4.1 KiB
C
#include "motorstat.h"
|
||
#include "timer.h"
|
||
#include "usart.h"
|
||
#include "main.h"
|
||
//extern struct StructMotorStat MotorStat;
|
||
struct StructMotorStat MotorStat;
|
||
|
||
//#include <math.h> // 包含数学库
|
||
|
||
|
||
|
||
|
||
int temp_speed;
|
||
void managespeed()
|
||
{
|
||
if ((MotorStat.speed>MotorStat.Targetspeed)&&(MotorStat.decflag==1))//dec
|
||
{
|
||
temp_speed=MotorStat.dec*0.001;
|
||
MotorStat.speed=MotorStat.speed-temp_speed;
|
||
if (MotorStat.speed<MotorStat.Targetspeed)
|
||
{
|
||
MotorStat.speed=MotorStat.Targetspeed;
|
||
}
|
||
setfreq(MotorStat.speed);
|
||
//MotorStat.set_speed(MotorStat.speed);
|
||
//printf("dec.speed=%d\r\n",MotorStat.speed);
|
||
|
||
}
|
||
|
||
|
||
if ((MotorStat.speed<MotorStat.Targetspeed)&&(MotorStat.accflag==1))
|
||
{
|
||
MotorStat.speed=MotorStat.speed+MotorStat.acc*1.0/1000.0;
|
||
if (MotorStat.speed>MotorStat.Targetspeed)
|
||
{
|
||
MotorStat.speed=MotorStat.Targetspeed;
|
||
}
|
||
MotorStat.set_speed(MotorStat.speed);
|
||
//printf("acc.speed=%d\r\n",MotorStat.speed);
|
||
}
|
||
|
||
|
||
}
|
||
|
||
|
||
|
||
//void motorloop(void)
|
||
//{
|
||
// if(MotorStat.speedflag==1){
|
||
// managespeed();
|
||
// }
|
||
// if((MotorStat.speed==MotorStat.Targetspeed)&&(MotorStat.speed!=0)){//当目标速度等于现在速度时
|
||
// MotorStat.speedflag=0;
|
||
// MotorStat.accflag=0;
|
||
// MotorStat.decflag=0;
|
||
// // MotorStat.set_speed(MotorStat.speed);
|
||
// //printf("target =%d\r\n",MotorStat.Targetspeed);
|
||
// }
|
||
//
|
||
//}
|
||
|
||
|
||
// 中断频率为1kHz（根据实际TIM4中断配置调整）
|
||
float dt = 1.0 / 1000; // 时间间隔 = 1ms
|
||
|
||
void motorloop(void)
|
||
{
|
||
if (MotorStat.moveflag==1)//如果正在转动
|
||
{
|
||
|
||
if (MotorStat.direction==0)//正转
|
||
{
|
||
MotorStat.NmberoNow++;
|
||
}else if (MotorStat.direction==1)//反转
|
||
{
|
||
MotorStat.NmberoNow--;
|
||
}
|
||
//printf("NmberoNow = %d\r\n",MotorStat.NmberoNow);
|
||
//printf("Stats: %d, Pos: %d, Speed: %.0f\r\n", MotorStat.move_phase, MotorStat.NmberoNow, MotorStat.current_speed);
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
|
||
if(MotorStat.speedflag==1){
|
||
|
||
if(!MotorStat.moveflag) return;
|
||
|
||
//int STEP_PER_REV = 200; // 每转200步
|
||
|
||
// 计算下一步延迟
|
||
float delay;
|
||
|
||
MotorStat.step_counter++;
|
||
printf("step_counter = %d\r\n",MotorStat.step_counter);
|
||
|
||
switch(MotorStat.move_phase){
|
||
case 0: // 加速阶段
|
||
MotorStat.current_speed += MotorStat.accel * dt; // 修正单位：steps/s² * s = steps/s
|
||
if(MotorStat.current_speed > MotorStat.max_speed){
|
||
MotorStat.current_speed = MotorStat.max_speed;
|
||
MotorStat.move_phase = 1;
|
||
}
|
||
if(MotorStat.step_counter >= MotorStat.accel_steps){//
|
||
MotorStat.move_phase = 1;
|
||
}
|
||
break;
|
||
|
||
case 1: // 匀速
|
||
if(MotorStat.step_counter >= (MotorStat.total_steps - MotorStat.decel_steps)){//
|
||
MotorStat.move_phase = 2;
|
||
}
|
||
break;
|
||
|
||
case 2: // 减速
|
||
MotorStat.current_speed -= MotorStat.decel * dt;
|
||
// if(MotorStat.current_speed < MotorStat.decel) MotorStat.current_speed = 0;
|
||
break;
|
||
}
|
||
|
||
// printf("mx: %d \r\n", MotorStat.max_speed);
|
||
|
||
// 更新PWM频率（需转换为Hz）
|
||
float steps_per_sec = MotorStat.current_speed ;
|
||
//float freq = steps_per_sec / (MotorStat.step_angle / 360.0f * STEP_PER_REV);
|
||
float freq = steps_per_sec * 1.0;
|
||
setfreq((u32)freq);
|
||
|
||
//printf("Mode : %d, Step: %u,\r\n", MotorStat.move_phase, MotorStat.step_counter); //运动模式，步数
|
||
// printf("acc = %d, dec = %d, Step: %u,\r\n", MotorStat.accel_steps,MotorStat.decel_steps,MotorStat.step_counter);
|
||
// printf("Step: %u, Pos: %d, Speed: %. printf("Dir : %d\r\n", MotorStat.direction);
|
||
|
||
|
||
// 检查是否到达目标位置
|
||
if (MotorStat.step_counter >= MotorStat.total_steps ||
|
||
MotorStat.NmberoNow == MotorStat.target_position) {
|
||
stopmotormove();
|
||
MotorStat.NmberoNow = MotorStat.target_position; // 确保位置准确
|
||
|
||
MotorStat.move_phase = -1;
|
||
}
|
||
}
|
||
|
||
|
||
|
||
}
|
||
|