有人能看懂基于stm32的四轴飞行器代码吗？

Jay_Qi 2019-08-26 09:16:05

#include "control.h" //垂直方向电机速度设置 void SetUpDowm(float front_val,float back_val)//min--max 升降运动 { __HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_1,front_mid+front_val); //1 __HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_1,back_mid-back_val);//3 // 电机三根线反了 static uint16_t last_value_front = mid; static uint16_t last_value_back = mid; uint16_t front = mid + front_val; uint16_t back = mid - back_val; // 电机三根线反了 front = CONSTRAIN(front, last_value_front+max_speed_delta, last_value_front-max_speed_delta); #define CONSTRAIN(x,max,min) (x>max?max:(x<min?min:x)) back = CONSTRAIN(back, last_value_back+max_speed_delta, last_value_back-max_speed_delta); front = CONSTRAIN(front, max_speed_value, min_speed_value); back = CONSTRAIN(back, max_speed_value, min_speed_value); __HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_1,front); //1 __HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_1,back);//3 last_value_front = front; last_value_back = back; } float temp_l = 0; float temp_r =0; //水平方向电机速度设置 void SetHorizontal(float f_b, float rotate)//水平运动 { temp_l = f_b+left_mid+rotate; temp_r = f_b+right_mid-rotate; //控制方案一 __HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_3,temp_r);//2 __HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_3,temp_r);//4 //控制方案二 //static uint16_t last_value_left = left_mid; //static uint16_t last_value_right = right_mid; //float right = right_mid + f_b - rotate;//右 //float left = left_mid + f_b + rotate;//左 //right = CONSTRAIN(right, last_value_right+max_speed_delta, last_value_right-max_speed_delta); //left = CONSTRAIN(left, last_value_left+max_speed_delta, last_value_left-max_speed_delta); // left = CONSTRAIN(left, max_speed_value, min_speed_value); // right = CONSTRAIN(right, max_speed_value, min_speed_value); // //__HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_3,right);//2 // __HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_3,left);//4 //last_value_left = left; //last_value_right = right; } u16 xCounterPi_To_Stm32 = 5; void Clear_Control_Disable_Counter(void) { xCounterPi_To_Stm32

...全文