main

2025-06-18 09:08:17 +08:00
commit 7999439a8f
363 changed files with 313769 additions and 0 deletions
--- a/USER/motorstat.c
+++ b/USER/motorstat.c
@ -0,0 +1,149 @@
+#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;
+    }
+	}
+    
+
+				
+}
+