用stm32制作平衡小車，電機不動

ID:586056 · 發表于 2019-7-26 11:59

現在正在做平衡小車的直立環，程序沒有編輯錯誤，電機驅動和stm32板子也沒有問題（用其他程序試過可以動），原本程序是可以的，但改動了幾次后就驅動不了了，改回來也不行，所以請各位大神看看是哪里出了問題

單片機源程序如下:
#include "led.h"
#include "delay.h"
#include "key.h"
#include "sys.h"
#include "usart.h"
#include "timer.h"
#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h"
#include "mpu6050.h"
#include <math.h>
#include "mpuiic.h"

void qian(int x,int y);
void hou(int x,int y);
void zuo(int x,int y);
void you(int x,int y);
float pitch,roll,yaw;
short aacx,aacy,aacz;
short gyrox,gyroy,gyroz;
int angleA, omega, f_angle=0;
int last_angle=0,error,pwm;

int main(void)
{
delay_init();
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
uart_init(115200);
  LED_Init();
MPU_Init();
TIM3_Int_Init(49,7199);
TIM3_PWM_Init(7200,0);
qian(7000,7000);
  while(1)
{

}
}
void qian(int x,int y)
{
  LED0=1;
LED1=0;
LED2=0;
LED3=1;
TIM_SetCompare1( TIM3,x);
TIM_SetCompare2( TIM3,y);
}
void hou(int x,int y)
{
  LED0=0;
LED1=1;
LED2=1;
LED3=0;
TIM_SetCompare1( TIM3,x);
TIM_SetCompare2( TIM3,y);
}
void zuo(int x,int y)
{
  LED0=0;
LED1=1;
LED2=0;
LED3=1;
TIM_SetCompare1( TIM3,x);
TIM_SetCompare2( TIM3,y);
}
void you(int x,int y)
{
  LED0=1;
LED1=0;
LED2=1;
LED3=0;
TIM_SetCompare1( TIM3,x);
TIM_SetCompare2( TIM3,y);
}
void TIM3_IRQHandler(void)
{
if (TIM_GetITStatus(TIM3, TIM_IT_Update) != RESET)
  {
  TIM_ClearITPendingBit(TIM3, TIM_IT_Update  );

MPU_Get_Accelerometer(&aacx,&aacy,&aacz);
MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);

angleA = atan2(aacx , aacz) * 180 / 3.14 ;
omega =gyroy/131.00 ;
f_angle = 0.98 * (f_angle + omega * 0.005) + 0.2 * angleA;
error=f_angle-30;
pwm=error*80+omega;

if(pwm>7200)
pwm=7200;
if(pwm<-7200)
pwm=-7200;
   printf("%d\r\n",f_angle);
      printf("%d\r\n",pwm);
if(pwm>0)
hou(pwm,pwm);
else
qian(-1*pwm,-1*pwm);

}
}