用TT电机做一个平衡小车(stm32)

咕咕咕~咕咕咕咕咕咕
(这个东西挺难的)

---

2020-08-02
程序我早就写好了,但是调参一直没调好,感觉小车重量设计有问题。但是也不能一直咕咕咕,所以我先放一下相关代码吧。

关于这个工程的结构以及其思路详见:
http://www.h13studio.com/一种关于STM32工程文件结构的想法/

然后用上述工程只需要修改Project/main.c即可

/**********C语言标准库**********/
#include "string.h"
#include "stdlib.h"
#include "math.h"

/**********STM32标准库**********/
#include "stm32f10x.h"
#include "stm32f10x_exti.h"

/**********项目所需库***********/
#include "delay.h"
#include "sys.h"
#include "usart.h"
#include "mpu6050.h"
#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h"
#include "PID.h"
#include "BlueRocker.h"
#include "ANO_tc.h"

/***********硬件连接************/
//标星为暂未实现在此直接更换引脚的方法
//*MPU6050
#define MPU6050_SDA_Port
#define MPU6050_SDA_Pin
#define MPU6050_SCL_Port
#define MPU6050_SCL_Pin

//*左光电门
#define SpeedometerL_Port GPIOB
#define SpeedometerL_Pin  GPIO_Pin_12

//*右光电门
#define SpeedometerR_Port GPIOB
#define SpeedometerR_Pin  GPIO_Pin_0

//速度滤波池
//容量
#define FILTER_POOL 5

//*L298N
//L
#define L298N_A1_Port GPIOA
#define L298N_A1_Pin  GPIO_Pin_11
#define L298N_A2_Port GPIOA
#define L298N_A2_Pin  GPIO_Pin_12
//R
#define L298N_B1_Port GPIOA
#define L298N_B1_Pin  GPIO_Pin_15
#define L298N_B2_Port GPIOB
#define L298N_B2_Pin  GPIO_Pin_3

//电机空域;两个电机空域不一样
#define MotorL_Airspace 30
#define MotorR_Airspace 41

//JDY-09蓝牙串口
#define JDY_09_TX 这还define个p啊
#define JDY_09_RX 直接照着串口改就完了

//RGB-LED
#define RED_Port      GPIOB
#define RED_Pin       GPIO_Pin_10
#define GREEN_Port    GPIOB
#define GREEN_Pin     GPIO_Pin_1
#define BLUE_Port     GPIOB
#define BLUE_Pin      GPIO_Pin_11

//项目逻辑常量
#define LIMIT_ANGLE 30

/**********项目全局变量**********/
//PID初始化
PIDtypedef PIDangle;    // + + 0
PIDtypedef PIDspeed;    // - + -？AT+Ps=-325.0  AT+Ds=99999999999999999999999 AT+Is=-0.000007
PIDtypedef PIDdamping;  //阻尼环
PIDtypedef PIDproduct_1;  //速度和角度乘积一次方环
PIDtypedef PIDproduct_4;

//定时函数参数初始化
uint32_t Time,time;       //系统时钟，过两天我改成Systick试试
short PIDtimes = 0;

//PWM参数初始化
int16_t PWML=0,PWMR=0;
uint8_t i = 0;            //PWM计数器

//MPU6050数据初始化
//uint16_t flag_6050 = 0;
//float pitch_sum,roll_sum,yaw_sum;                    //三轴滤波池***算了 PID频率比6050频率还高..没必要
float pitch,roll,yaw;         //欧拉角
short aacx,aacy,aacz;          //加速度传感器原始数据
short gyrox,gyroy,gyroz;    //陀螺仪原始数据

//测速光电门参数初始化
uint32_t LastTimeL = 0,LastTimeR = 0;
uint16_t FlagL = 0,FlagR = 0; //实际上用uint8_t就行了
uint16_t FlagL_Temp = 0,FlagR_Temp = 0;
double SpeedL,SpeedR;

//蓝牙摇杆上位机初始化
BlueRockerTypeDef BlueRocker;

//RGB初始化
typedef struct {
    uint8_t Red;
    uint8_t Green;
    uint8_t Blue;
} RGB_Valuetypedef;
RGB_Valuetypedef RGB_Value;
//PWM参数与电机的共用

//项目逻辑变量初始化
//平衡角度
//double balanceangle = 2.25;//-0.55
double balanceangle = -0.55;
//double limitangle = 30.0;    //请写绝对值
double speedthreshold = 100;   //注意是速度的平方
//速度滤波池
uint16_t DeltaTimeL[FILTER_POOL],DeltaTimeR[FILTER_POOL];
uint8_t FilterSpeedL = 0,FilterSpeedR = 0;
//累计误差衰减系数
double     Attenuations;
//上一次小车的状态
static uint8_t status = 0;
//(1: 上次PID时的速度    正_1 负_0)
//(2: 上次PID时的角度    正_1 负_0)
//(3: 上次测速时左轮速度 正_1 负_0)
//(4: 上次测速时右轮速度 正_1 负_0)
//余下 4 位未分配()()()()


/*******初始化回调函数指针*******/
//算了 不如直接判断返回值简单明了
//void USART1_interrupt(u8* Rec,u8 len);
//void (*CallBackFunction) (u8*,u8) = USART1_interrupt;

void TIM3_Int_Init(u16 arr,u16 psc)
{
    TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
    NVIC_InitTypeDef NVIC_InitStructure;

    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE); //时钟使能

    //定时器TIM3初始化
    TIM_TimeBaseStructure.TIM_Period = arr; //设置在下一个更新事件装入活动的自动重装载寄存器周期的值
    TIM_TimeBaseStructure.TIM_Prescaler =psc; //设置用来作为TIMx时钟频率除数的预分频值
    TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; //设置时钟分割:TDTS = Tck_tim
    TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;  //TIM向上计数模式
    TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure); //根据指定的参数初始化TIMx的时间基数单位
    TIM_ITConfig(TIM3,TIM_IT_Update,ENABLE ); //使能指定的TIM3中断,允许更新中断

    //中断优先级NVIC设置
    NVIC_InitStructure.NVIC_IRQChannel = TIM3_IRQn;  //TIM3中断
    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;  //先占优先级0级
    NVIC_InitStructure.NVIC_IRQChannelSubPriority = 3;  //从优先级3级
    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; //IRQ通道被使能
    NVIC_Init(&NVIC_InitStructure);  //初始化NVIC寄存器

    TIM_Cmd(TIM3, ENABLE);  //使能TIMx
}

void L298N_init()
{
    GPIO_InitTypeDef GPIO_InitStructure;
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOB,ENABLE);

    //A1
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
    GPIO_InitStructure.GPIO_Pin = L298N_A1_Pin;
    GPIO_Init(L298N_A1_Port,&GPIO_InitStructure);

    //A2
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
    GPIO_InitStructure.GPIO_Pin = L298N_A2_Pin;
    GPIO_Init(L298N_A2_Port,&GPIO_InitStructure);


    //B1
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
    GPIO_InitStructure.GPIO_Pin = L298N_B1_Pin;
    GPIO_Init(L298N_B1_Port,&GPIO_InitStructure);

    //B2
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
    GPIO_InitStructure.GPIO_Pin = L298N_B2_Pin;
    GPIO_Init(L298N_B2_Port,&GPIO_InitStructure);
}

void Speedometer_init()
{
    /*定义结构体*/
    GPIO_InitTypeDef GPIO_InitStructure;
    NVIC_InitTypeDef NVIC_InitStructure;
    EXTI_InitTypeDef EXTI_InitStructure;

    void GPIO_EXTILineConfig(uint8_t GPIO_PortSource, uint8_t GPIO_PinSource); //将GPIO和EXTI建立连接（映射）

    /*使能时钟*/
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB | RCC_APB2Periph_AFIO,ENABLE);//使能时钟

    /*配置中断源    */
    //EXTI_R
    EXTI_InitStructure.EXTI_Line=EXTI_Line0;             //配置EXTI中断线
    EXTI_InitStructure.EXTI_Mode=EXTI_Mode_Interrupt;    //配置EXTI模式为中断
    EXTI_InitStructure.EXTI_Trigger=EXTI_Trigger_Rising; //配置EXTI触发方式
    EXTI_InitStructure.EXTI_LineCmd=ENABLE;              //使能EXTI中断线
    EXTI_Init(&EXTI_InitStructure);                      //根据指定的参数初始化EXTI寄存器

    //EXTI_L
    EXTI_InitStructure.EXTI_Line=EXTI_Line12;            //配置EXTI中断线
    EXTI_InitStructure.EXTI_Mode=EXTI_Mode_Interrupt;    //配置EXTI模式为中断
    EXTI_InitStructure.EXTI_Trigger=EXTI_Trigger_Rising; //配置EXTI触发方式
    EXTI_InitStructure.EXTI_LineCmd=ENABLE;              //使能EXTI中断线
    EXTI_Init(&EXTI_InitStructure);                      //根据指定的参数初始化EXTI寄存器

    /*设置中断通道*/
    //NVIC_L
    NVIC_InitStructure.NVIC_IRQChannel = EXTI15_10_IRQn;    //EXTIx 中断通道
    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=0; //抢占最高优先级(反正占用时间少)
    NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;      //响应优先级
    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;         //IRQ通道使能
    NVIC_Init(&NVIC_InitStructure);                         //根据指定的参数初始化NVIC寄存器

    //NVIC_R
    NVIC_InitStructure.NVIC_IRQChannel = EXTI0_IRQn;        //EXTIx 中断通道
    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=0; //抢占最高优先级(反正占用时间少)
    NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;      //响应优先级
    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;         //IRQ通道使能
    NVIC_Init(&NVIC_InitStructure);                         //根据指定的参数初始化NVIC寄存器

    /*配置GPIO*/
    //GPIO_L
    GPIO_InitStructure.GPIO_Pin = SpeedometerL_Pin;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;           //上拉模式
    GPIO_Init(SpeedometerL_Port,&GPIO_InitStructure);       //设置GPIO
    GPIO_EXTILineConfig(GPIO_PortSourceGPIOB, GPIO_PinSource12);

    //GPIO_R
    GPIO_InitStructure.GPIO_Pin = SpeedometerR_Pin;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;           //上拉模式
    GPIO_Init(SpeedometerR_Port,&GPIO_InitStructure);       //设置GPIO
    GPIO_EXTILineConfig(GPIO_PortSourceGPIOB, GPIO_PinSource0);
}

/*******************************************************************/
//L298N PWM处理函数
//参数:
//  int16_t PWML  ->  左轮目标PWM高电平时间
//  int16_t PWMR  ->  右轮目标PWM高电平时间
//  int16_t Beats ->  系统心跳计数次数
//
//作用:光电测速传感器每触发一次 执行一次此函数
/*******************************************************************/
void L298N_PWM(int16_t PWML,int16_t PWMR,int16_t Beats)
{
    if(PWML <= 0)
    {
        if(PWML == 0)
            return;
        //PWM预处理
        if(PWML >= -32768 + MotorL_Airspace)
            PWML -= MotorL_Airspace;
        if(-PWML > Beats)
        {
            //反向转动, A1 = 0; A2 = 1;
            GPIO_ResetBits(L298N_A1_Port,L298N_A1_Pin);
            GPIO_SetBits(L298N_A2_Port,L298N_A2_Pin);
        } else {
            //待机, A1 = 0; A2 = 0;
            GPIO_ResetBits(L298N_A1_Port,L298N_A1_Pin);
            GPIO_ResetBits(L298N_A2_Port,L298N_A2_Pin);
        }

    } else {
        //PWM预处理
        if(PWML <= 32767 - MotorL_Airspace)
            PWML += MotorL_Airspace;
        if(PWML > Beats)
        {
            //正向转动, A1 = 1; A2 = 0;
            GPIO_SetBits(L298N_A1_Port,L298N_A1_Pin);
            GPIO_ResetBits(L298N_A2_Port,L298N_A2_Pin);
        } else {
            //待机, A1 = 0; A2 = 0;
            GPIO_ResetBits(L298N_A1_Port,L298N_A1_Pin);
            GPIO_ResetBits(L298N_A2_Port,L298N_A2_Pin);
        }
    }

    if(PWMR <= 0)
    {
        if(PWMR == 0)
            return;
        //PWM预处理
        if(PWMR >= -32768 + MotorR_Airspace)
            PWMR -= MotorR_Airspace;
        if(-PWMR > Beats)
        {
            //反向转动, B1 = 0; B2 = 1;
            GPIO_SetBits(L298N_B1_Port,L298N_B1_Pin);
            GPIO_ResetBits(L298N_B2_Port,L298N_B2_Pin);
        } else {
            //待机, B1 = 0; B2 = 0;
            GPIO_ResetBits(L298N_B1_Port,L298N_B1_Pin);
            GPIO_ResetBits(L298N_B2_Port,L298N_B2_Pin);
        }

    } else {
        //PWM预处理
        if(PWMR <= 32767 - MotorR_Airspace)
            PWMR += MotorR_Airspace;
        if(PWMR > Beats)
        {
            //正向转动, B1 = 1; B2 = 0;
            GPIO_ResetBits(L298N_B1_Port,L298N_B1_Pin);
            GPIO_SetBits(L298N_B2_Port,L298N_B2_Pin);
        } else {
            //待机, B1 = 0; B2 = 0;
            GPIO_ResetBits(L298N_B1_Port,L298N_B1_Pin);
            GPIO_ResetBits(L298N_B2_Port,L298N_B2_Pin);
        }
    }
}

/*******************************************************************/
//RGB PWM处理函数
//参数:
//  RGB_Valuetypedef RGB_Value -> 存有RGB值的结构体
//  uint8_t Beats               -> 用于PWM计数的心跳
//
//作用:PWM输出
/*******************************************************************/
void RGB(RGB_Valuetypedef RGB_Value,uint8_t Beats)
{
    if(Beats < RGB_Value.Red)
        GPIO_ResetBits(RED_Port,RED_Pin);
    else
        GPIO_SetBits(RED_Port,RED_Pin);

    if(Beats < RGB_Value.Green)
        GPIO_ResetBits(GREEN_Port,GREEN_Pin);
    else
        GPIO_SetBits(GREEN_Port,GREEN_Pin);

    if(Beats < RGB_Value.Blue)
        GPIO_ResetBits(BLUE_Port,BLUE_Pin);
    else
        GPIO_SetBits(BLUE_Port,BLUE_Pin);
}


/*******************************************************************/
//速度正负判断函数
//参数:
//  int16_t PWM
//
//返回值:
//  0 -> 负
//  1 -> 正
/*******************************************************************/
uint8_t Sign_judgment(int16_t PWM)
{
    if(PWM > 0)
        return 1;
    else
        return 0;
}

/*******************************************************************/
//右测速器外部IO中断函数
//参数:
//  无
//
//作用:右光电测速传感器每触发一次 执行一次此函数
/*******************************************************************/
void EXTI0_IRQHandler(void)
{
    uint8_t temp;
    uint32_t DeltaTimeSum = 0;
    if(EXTI_GetITStatus(EXTI_Line0) == 1) //读取中断标志位，确定中断真的发生
    {
        FlagR ++;

        FilterSpeedR ++;
        DeltaTimeR[FilterSpeedR] = time - LastTimeR;
        LastTimeR = time;
        if (FilterSpeedR >= FILTER_POOL -1)
            FilterSpeedR = 0;

        for(temp = 0; temp < FILTER_POOL; temp++)
            DeltaTimeSum += DeltaTimeR[temp];

        if(Sign_judgment(PWMR))
        {
            if ((status&0x10) == 0)       //速度正负未发生变化时 速度正常赋值
            {
                SpeedR = (double)(1000000/DeltaTimeSum);
            }
            status |= 0x10;
        } else {
            if ((status&0x10) != 0)       //速度正负未发生变化时 速度正常赋值
            {
                SpeedR = -(double)(1000000/DeltaTimeSum);
            }
            status &= 0xef;
        }
    }

    EXTI_ClearITPendingBit(EXTI_Line0); //清除中断标志位
}

/*******************************************************************/
//左测速器外部IO中断函数
//参数:
//  无
//
//作用:左光电测速传感器每触发一次 执行一次此函数
/*******************************************************************/
void EXTI15_10_IRQHandler(void)
{
    uint8_t temp;
    uint32_t DeltaTimeSum = 0;
    if(EXTI_GetITStatus(EXTI_Line12) == 1) //读取中断标志位，确定中断真的发生
    {
        FlagL ++;

        FilterSpeedL ++;
        DeltaTimeL[FilterSpeedL] = time - LastTimeL;
        LastTimeL = time;
        if (FilterSpeedL >= FILTER_POOL -1)
            FilterSpeedL = 0;

        for(temp = 0; temp < FILTER_POOL; temp++)
            DeltaTimeSum += DeltaTimeL[temp];


        if(Sign_judgment(PWML))
        {
            if ((status&0x20) == 0)       //速度正负未发生变化时 速度正常赋值
            {
                SpeedL = (double)(1000000/DeltaTimeSum);
            }
            status |= 0x20;           //将速度记录为正 本次速度先不变化
        } else {
            if ((status&0x20) != 0)       //速度正负未发生变化时 速度正常赋值
            {
                SpeedL = -(double)(1000000/DeltaTimeSum);
            }
            status &= 0xdf;           //将速度记录为负 本次速度先不变化
        }
    }

    EXTI_ClearITPendingBit(EXTI_Line12); //清除中断标志位
}

/*******************************************************************/
//定时器中断函数
//参数:
//  无
//
//作用:每0.1ms触发一次,提供PWM以及系统时钟,并一秒上报一次设备状态
/*******************************************************************/

void TIM3_IRQHandler(void)   //TIM3中断
{
    static double product_speedsum_error,itemp;

    //总速度
    double speed = 0,speed_2 = 0,product_1 = 0,product_4 = 0,temp = 0;

    //匿名上位机参数初始化
    //ANO_SENSER_Typedef ANO_SENSER_MSG;

    if (TIM_GetITStatus(TIM3, TIM_IT_Update) != RESET)  //检查TIM3更新中断发生与否
    {
        TIM_ClearITPendingBit(TIM3, TIM_IT_Update);  //清除TIMx更新中断标志
        time++;

        if(time%10 == 0)
        {
            Time++;                       //系统时钟 单位毫秒

            speed = SpeedL + SpeedR;

            if(speed > 0)
            {
                if ((status&0x80) == 0)
                {
                    //PIDspeed.sum_error = 0;
                    PIDdamping.sum_error = 0;
                }
                status |= 0x80;
            } else {
                if ((status&0x80) != 0)
                {
                    //PIDspeed.sum_error = 0;
                    PIDdamping.sum_error = 0;
                }
                status &= 0x7f;
            }

            if(pitch > PIDangle.setpoint)
            {
                if ((status&0x40) == 0)
                {
                    PIDangle.sum_error = 0;
                    product_speedsum_error = 0;
                    PIDproduct_1.sum_error = 0;
                    PIDproduct_4.sum_error = 0;
                    //PIDspeed.sum_error = 0;
                    PIDdamping.sum_error = 0;
                    PIDspeed.sum_error *= Attenuations;
                }
                status |= 0x40;
            } else {
                if ((status&0x40) != 0)
                {
                    PIDangle.sum_error = 0;
                    product_speedsum_error = 0;
                    PIDproduct_1.sum_error = 0;
                    PIDproduct_4.sum_error = 0;
                    //PIDspeed.sum_error = 0;
                    PIDdamping.sum_error = 0;
                    PIDspeed.sum_error *= Attenuations;
                }
                status &= 0xbf;
            }

            if(pitch > balanceangle + LIMIT_ANGLE)
            {
                PIDangle.sum_error = 0;
                product_speedsum_error = 0;
                PIDproduct_1.sum_error = 0;
                PIDproduct_4.sum_error = 0;
                PIDspeed.sum_error = 0;
                PIDdamping.sum_error = 0;
            }

            if(pitch < balanceangle - LIMIT_ANGLE)
            {
                PIDangle.sum_error = 0;
                product_speedsum_error = 0;
                PIDproduct_1.sum_error = 0;
                PIDproduct_4.sum_error = 0;
                PIDspeed.sum_error = 0;
                PIDdamping.sum_error = 0;
            }

            /*
            if(PIDspeed.sum_error*PIDspeed.integral >= 255)
                  PIDspeed.sum_error = 255/PIDspeed.integral;

            if(PIDspeed.sum_error*PIDspeed.integral <= -255)
                  PIDspeed.sum_error = -255/PIDspeed.integral;
            */
            //speed_2 = pow((speed*0.001),3);

            temp = incPIDcalc(&PIDspeed,speed);
            /*
            if(temp >= LIMIT_ANGLE)
                  temp = LIMIT_ANGLE;
            if(temp <= -LIMIT_ANGLE)
                  temp = -LIMIT_ANGLE;
            */
            PIDangle.setpoint = balanceangle + temp;

                        /*
            product_speedsum_error -= speed;

            product_1 = (-PIDangle.sum_error)*product_speedsum_error*0.00000001;

            product_4 = pow((-PIDangle.sum_error)*product_speedsum_error*0.00001,4)*0.000001    ;

            if(product_1 > 0)
            {
                if(pitch < 0)
                {
                    product_1 = - product_1;
                    product_4 = - product_4;
                }
            } else {
                product_1 = 0;
                product_4 = 0;
                //PIDproduct_1.sum_error = 0;
                //PIDproduct_4.sum_error = 0;
            }
            */
                        /*
                        if(pow(speed,2) < speedthreshold)
                        {
                              itemp = PIDspeed.integral;
                              PIDspeed.integral = 0;
                        } else {
                              PIDspeed.integral = itemp;
                        }
                        */
            temp = incPIDcalc(&PIDangle,pitch) + incPIDcalc(&PIDdamping,speed) + incPIDcalc(&PIDproduct_1,product_1) + incPIDcalc(&PIDproduct_4,product_4);

            if(temp > 255)
                temp = 255;
            if(temp < -255)
                temp = -255;
            /*
            if(speed > 0)
                speed_2 = pow((speed*0.001),3);
            else
                speed_2 = -pow((speed*0.001),3);
            */

            //PWML += incPIDcalc(&PIDspeed,speed_2);
            PWML = temp;
            PWMR = PWML;
            if(PWMR > 0)
                        {
                RGB_Value.Red = 255;
                              RGB_Value.Blue = 0;
                              RGB_Value.Green = 0;
            } else {
                RGB_Value.Red = 0;
                              RGB_Value.Blue = 255;
                              RGB_Value.Green = 0;
                        }
            //printf("PWML: %d\r\n",PWML);

            if(Time%100 == 0)
            {
                if(FlagL_Temp != FlagL)
                    FlagL_Temp = FlagL;
                else
                    SpeedL = 0;

                if(FlagR_Temp != FlagR)
                    FlagR_Temp = FlagR;
                else
                    SpeedR = 0;
            }

            if(Time%3000 == 0)
            {
                //printf("%lf %lf\r\n",SpeedL,SpeedR);
                //printf("%lf %lf %lf\r\n",PIDspeed.sum_error,PIDangle.setpoint,pitch);
                //printf("Product1: %lf %lf\r\n",product_1,product_4);
                //printf("%lf")
            }

            /*
            if(Time%200 == 0)
            {
                  printf("%d\r\n",status);
                  printf("%f\r\n",speed);
                  printf("FlagL:%d\r\n",FlagL);
                printf("SpeedL:%f\r\n",SpeedL);
                printf("FlagR:%d\r\n",FlagR);
                printf("SpeedR:%f\r\n",SpeedR);
            }
            */
            /*
              ANO_SENSER_MSG.ACC_X = 0;
              ANO_SENSER_MSG.ACC_Y = 0;
              ANO_SENSER_MSG.ACC_Z = 0;
              ANO_SENSER_MSG.GYRO_Y = 0;
              ANO_SENSER_MSG.GYRO_Z = 0;
              ANO_SENSER_MSG.MAG_X = 0;
              ANO_SENSER_MSG.MAG_Y = 0;
              ANO_SENSER_MSG.MAG_Z = 0;
              ANO_SENSER_MSG.GYRO_X = (int16_t)(((speedL_sum/flag_L) + (speedR_sum/flag_R))*0.002);
            */
            /*

            //ANO_SENSER_MSG.GYRO_Y = (int16_t)pitch*100;
            //ANO_SENSER(USART1,ANO_SENSER_MSG);
            }
            */
            /*
            if(Time%1000 == 0)            //不要尝试将本移到外面并改为if(time%10000 == 0),否则main的while里面的6050数据读取BOOM!
            {
                      printf("\r\n");
                      printf("Pangle: %lf\r\n",PIDangle.proportion);
                      printf("Iangle: %lf\r\n",PIDangle.integral);
                      printf("Dangle: %lf\r\n",PIDangle.derivative);
                      printf("Tangle: %lf\r\n",PIDangle.setpoint);
                      printf("Pspeed: %lf\r\n",PIDspeed.proportion);
                      printf("Ispeed: %lf\r\n",PIDspeed.integral);
                      printf("Dspeed: %lf\r\n",PIDspeed.derivative);

                      printf("Speed: %lf\r\n",speedL+speedR);

                        //printf("BlueRocker.rockerVal: %d\r\n",BlueRocker.rockerVal);
                        printf("pitch: %f\r\n",pitch);

                        //printf("roll: %f\r\n",roll);
                        //printf("yaw: %f\r\n",yaw);
                        printf("PWM: %d\r\n",PWML);
                        //printf("PID Frequence: %d\r\n",PIDtimes);
                        //printf("System Time: %lld\r\n",Time);
                      printf("Flag_L: %d\r\n",flag_L);
                      printf("Flag_R: %d\r\n",flag_R);
                        //PIDtimes = 0;
            }
            */
        }

        L298N_PWM(PWML,PWMR,i);
        RGB(RGB_Value,i);
        i++;
    }
}

/*******************************************************************/
//串口AT命令处理函数
//参数:
//  u8* Rec  ->  数据指针
//  u8  len  ->  数组长度
//
//作用:在串口接收中断程序中被调用
/*******************************************************************/

void AT_Process(u8* Rec,u8 len)
{
    char temp[USART_REC_LEN];
    if(!strncmp((char*)Rec,"AT+Pa=",6))
    {
        Rec += 6;
        strncpy(temp,(char*)Rec,len-6);
        PIDangle.proportion = atof(temp);
        printf("Set Pa to %lf\r\n",PIDangle.proportion);
        return;
    }

    if(!strncmp((char*)Rec,"AT+Ia=",6))
    {
        Rec += 6;
        strncpy(temp,(char*)Rec,len-6);
        PIDangle.integral = atof(temp);
        printf("Set Ia to %lf\r\n",PIDangle.integral);
        return;
    }

    if(!strncmp((char*)Rec,"AT+Da=",6))
    {
        Rec += 6;
        strncpy(temp,(char*)Rec,len-6);
        PIDangle.derivative = atof(temp);
        printf("Set Da to %lf\r\n",PIDangle.derivative);
        return;
    }

    if(!strncmp((char*)Rec,"AT+Targeta=",11))
    {
        Rec += 11;
        strncpy(temp,(char*)Rec,len-11);
        PIDangle.setpoint = atof(temp);
        printf("Set Targeta to %lf\r\n",PIDangle.setpoint);
        return;
    }


    if(!strncmp((char*)Rec,"AT+Ps=",6))
    {
        Rec += 6;
        strncpy(temp,(char*)Rec,len-6);
        PIDspeed.proportion = atof(temp);
        printf("Set Ps to %lf\r\n",PIDspeed.proportion);
        return;
    }

    if(!strncmp((char*)Rec,"AT+Is=",6))
    {
        Rec += 6;
        strncpy(temp,(char*)Rec,len-6);
        PIDspeed.integral = atof(temp);
        printf("Set Is to %lf\r\n",PIDspeed.integral);
        return;
    }

    if(!strncmp((char*)Rec,"AT+Ds=",6))
    {
        Rec += 6;
        strncpy(temp,(char*)Rec,len-6);
        PIDspeed.derivative = atof(temp);
        printf("Set Ds to %lf\r\n",PIDspeed.derivative);
        return;
    }

    if(!strncmp((char*)Rec,"AT+Targets=",11))
    {
        Rec += 11;
        strncpy(temp,(char*)Rec,len-11);
        PIDspeed.setpoint = atof(temp);
        printf("Set Targets to %lf\r\n",PIDspeed.setpoint);
        return;
    }

    if(!strncmp((char*)Rec,"AT+balanceangle=",16))
    {
        Rec += 16;
        strncpy(temp,(char*)Rec,len-16);
        balanceangle = atof(temp);
        printf("Set balanceangle to %lf\r\n",balanceangle);
        return;
    }

    if(!strncmp((char*)Rec,"AT+Pd=",6))
    {
        Rec += 6;
        strncpy(temp,(char*)Rec,len-6);
        PIDdamping.proportion = atof(temp);
        printf("Set Pd to %lf\r\n",PIDdamping.proportion);
        return;
    }

    if(!strncmp((char*)Rec,"AT+Id=",6))
    {
        Rec += 6;
        strncpy(temp,(char*)Rec,len-6);
        PIDdamping.integral = atof(temp);
        printf("Set Id to %lf\r\n",PIDdamping.integral);
        return;
    }

    if(!strncmp((char*)Rec,"AT+Dd=",6))
    {
        Rec += 6;
        strncpy(temp,(char*)Rec,len-6);
        PIDdamping.derivative = atof(temp);
        printf("Set Dd to %lf\r\n",PIDdamping.derivative);
        return;
    }

    if(!strncmp((char*)Rec,"AT+Pp1=",7))
    {
        Rec += 7;
        strncpy(temp,(char*)Rec,len-7);
        PIDproduct_1.proportion = atof(temp);
        printf("Set Pp1 to %lf\r\n",PIDproduct_1.proportion);
        return;
    }

    if(!strncmp((char*)Rec,"AT+Ip1=",7))
    {
        Rec += 7;
        strncpy(temp,(char*)Rec,len-7);
        PIDproduct_1.integral = atof(temp);
        printf("Set Ip1 to %lf\r\n",PIDproduct_1.integral);
        return;
    }

    if(!strncmp((char*)Rec,"AT+Dp1=",7))
    {
        Rec += 7;
        strncpy(temp,(char*)Rec,len-7);
        PIDproduct_1.derivative = atof(temp);
        printf("Set Dp1 to %lf\r\n",PIDproduct_1.derivative);
        return;
    }

    if(!strncmp((char*)Rec,"AT+Pp4=",7))
    {
        Rec += 7;
        strncpy(temp,(char*)Rec,len-7);
        PIDproduct_4.proportion = atof(temp);
        printf("Set Pp4 to %lf\r\n",PIDproduct_4.proportion);
        return;
    }

    if(!strncmp((char*)Rec,"AT+Ip4=",7))
    {
        Rec += 7;
        strncpy(temp,(char*)Rec,len-7);
        PIDproduct_4.integral = atof(temp);
        printf("Set Ip4 to %lf\r\n",PIDproduct_4.integral);
        return;
    }

    if(!strncmp((char*)Rec,"AT+Dp4=",7))
    {
        Rec += 7;
        strncpy(temp,(char*)Rec,len-7);
        PIDproduct_4.derivative = atof(temp);
        printf("Set Dp4 to %lf\r\n",PIDproduct_4.derivative);
        return;
    }

        if(!strncmp((char*)Rec,"AT+As=",6))
    {
        Rec += 6;
        strncpy(temp,(char*)Rec,len-6);
        Attenuations = atof(temp);
        printf("Set As to %lf\r\n",Attenuations);
        return;
    }
}

/*******************************************************************/
//串口中断函数:
//参数:
//  无
//
//作用:每次串口收到数据时被调用
/*******************************************************************/
void USART1_IRQHandler(void)
{
    u8 Res;
    if(USART_GetITStatus(USART1, USART_IT_RXNE) != RESET)  //接收中断(接收到的数据必须是0x0d 0x0a结尾)
    {
        /****************************在这里可以输出原生数据****************************/
        Res = USART_ReceiveData(USART1);    //读取接收到的数据
        Usart_Receive(&BlueRocker,Res);   //0x0A 0x0D结尾是蓝摇

        if((USART_RX_STA&0x8000)==0)//接收未完成
        {
            if(USART_RX_STA&0x4000)//接收到了0x0d
            {
                if(Res!=0x0a)
                    USART_RX_STA=0;//接收错误,重新开始
                else
                {
                    USART_RX_STA|=0x8000;    //接收完成了
                    AT_Process(USART_RX_BUF,(USART_RX_STA&0x3fff));
                    USART_RX_STA=0;
                }
            }
            else //还没收到0X0D
            {
                if(Res==0x0d)USART_RX_STA|=0x4000;
                else
                {
                    USART_RX_BUF[USART_RX_STA&0X3FFF]=Res ;
                    USART_RX_STA++;
                    if(USART_RX_STA>(USART_REC_LEN-1))USART_RX_STA=0;//接收数据错误,重新开始接收
                }
            }
        }
    }
}

void RGB_init()
{
    GPIO_InitTypeDef GPIO_InitStructure;

    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);

    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;

    GPIO_InitStructure.GPIO_Pin = RED_Pin;
    GPIO_Init(RED_Port,&GPIO_InitStructure);

    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;

    GPIO_InitStructure.GPIO_Pin = GREEN_Pin;
    GPIO_Init(GREEN_Port,&GPIO_InitStructure);

    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;

    GPIO_InitStructure.GPIO_Pin = BLUE_Pin;
    GPIO_Init(BLUE_Port,&GPIO_InitStructure);
}

int main(void)
{
    /****************************************变量初始化**********************************************/
    //逻辑变量初始化
    unsigned char times;
      Attenuations = 0.65;
    //PID初始化
    PIDangle.setpoint = 0;
    PIDspeed.setpoint = 0;
    PIDdamping.setpoint = 0;
    PIDproduct_1.setpoint = 0;
    PIDproduct_4.setpoint = 0;

    PIDangle.proportion = 50.0;//50
    //PIDangle.integral = 0.5;//0.1650  0.1250-> 速度环--
    PIDangle.derivative = 1900.0;//1900

    PIDspeed.proportion = 0.001;
    PIDspeed.integral = -0.000017;
    PIDspeed.derivative = 0.0;
    /*
        PIDproduct_1.proportion = 150;
      PIDproduct_1.integral = 0;
      PIDproduct_1.derivative = 220;

        PIDproduct_4.proportion = 1.0;
      PIDproduct_4.integral = 300.0;
      PIDproduct_4.derivative = 15000.0;



      PIDdamping.proportion = 0;
      PIDdamping.integral = 0;
      PIDdamping.derivative = 0;

      //PIDspeed.proportion = -325.0;
      //PIDspeed.derivative = (double)99999999999999999999999.0;
    */

    /*
    PIDangle.proportion = 55.0;//50
    PIDangle.integral = 0.0000002;//0.5
    PIDangle.derivative = 1500.0;//1900
    */

    /*****************************************系统初始化*********************************************/
    RGB_init();
    RGB_Value.Red = 0;
    RGB_Value.Green = 0;
    RGB_Value.Blue = 0;

    L298N_init();             //L298N初始化
    GPIO_ResetBits(L298N_A1_Port,L298N_A1_Pin);
    GPIO_ResetBits(L298N_A2_Port,L298N_A2_Pin);
    GPIO_ResetBits(L298N_B1_Port,L298N_B1_Pin);
    GPIO_ResetBits(L298N_B2_Port,L298N_B2_Pin);

    NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);     //设置NVIC中断分组2:2位抢占优先级，2位响应优先级
    delay_init();         //延时初始化
    MPU_Init();                    //初始化MPU6050
    while(mpu_dmp_init()!=0)
        delay_ms(400);        //dmp初始化
    uart_init(9600);               //串口初始化为9600
    Speedometer_init();       //光电测速初始化
    TIM3_Int_Init(9,719);     //系统时钟初始化
    BlueRocker_init(&BlueRocker);        //蓝摇上位机初始化

    //RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO|RCC_APB2Periph_GPIOB|RCC_APB2Periph_GPIOA, ENABLE);
    //GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable, ENABLE);

    /*****************************************进入主程序*********************************************/
    while(1)
    {
        if(mpu_dmp_get_data(&pitch,&roll,&yaw)==0)
        {
            //MPU_Get_Accelerometer(&aacx,&aacy,&aacz);    //得到加速度传感器数据
            //MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);    //得到陀螺仪数据
        }

        if(times % 5 == 0)
        {
            times = 0;
        }

        times ++;
    }
}

Video

其实目前的效果比这个好不少 但是还是不行。