|
|
2黑幣
現(xiàn)在正在做平衡小車的直立環(huán)，程序沒有編輯錯(cuò)誤，電機(jī)驅(qū)動和stm32板子也沒有問題（用其他程序試過可以動），原本程序是可以的，但改動了幾次后就驅(qū)動不了了，改回來也不行，所以請各位大神看看是哪里出了問題
單片機(jī)源程序如下:
#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);
}
}
|
|
