电源稳压降压用LMT317 作为LDO芯片，采用直流无刷电机实现高转速保证动量轮高动量，内部集成TB6612驱动芯片和SY8120IABC稳压芯片保证动量轮处于正常控制状态。状态反馈传感器采用JY61陀螺仪。主控芯片采用CH32V103R8T6。

/*说明：片内外设使用了定时器和串口，定时器用来正交解码读取编码器的值，也用于pwm输出。串口1用来调试和调用finSH组件，串口2接收陀螺仪数据
内核使用了线程和信号量以及中断*/


//发送到VOFA+
void sendVofa(float data_1,float data_2)
{
    unsigned char tail[4]={0x00, 0x00, 0x80, 0x7f};
       float fdata[2];
       fdata[0]=data_1;
       fdata[1]=data_2;
       uart_putbuff(UART_1,(char*)fdata,sizeof(float)*2);
       uart_putbuff(UART_1,(char*)tail,4);
}


//数据滤波
#define dt 0.012  //这个单位是s，是采样周期
/**********************************************
* 卡尔曼滤波
***********************************************/
float KalmanFilter_Elect(float curr_elect_val,float last_elect_val)
{
  static float Q_curr = 1.0;//0.1           //Q增大，动态响应增大，过程噪声的协方差
  static float Q_last = 0.0001;         //过程噪声的协方差，过程噪声的协方差为一个一行两列矩阵
  static float R_elect = 10.0;                  //测量噪声的协方差 即测量偏差

  static float Pk[2][2] = { {1, 0}, {0, 1 }};

  static float Pdot[4] = {0,0,0,0};

  static float q_bias = 0.0;
  static float elect_err = 0.0;
  static float PCt_0 = 0.0;
  static float PCt_1 = 0.0;
  static float E = 0.0;
  static float K_0 = 0.0, K_1 = 0.0, t_0 = 0.0, t_1 = 0.0;

  Pdot[0] = Q_curr - Pk[0][1] - Pk[1][0];       //Pk-先验估计误差协方差的微分
  Pdot[1] = -Pk[1][1];
  Pdot[2] = -Pk[1][1];
  Pdot[3] = Q_last;

  Pk[0][0] += Pdot[0] * dt;             //Pk-先验估计误差的协方差微分的积分
  Pk[0][1] += Pdot[1] * dt;             //先验估计误差协方差
  Pk[1][0] += Pdot[2] * dt;
  Pk[1][1] += Pdot[3] * dt;

  elect_err = curr_elect_val - last_elect_val;          //偏差 = 测量值 - 预测值，先验估计

  PCt_0 = Pk[0][0];
  PCt_1 = Pk[1][0];

  E = R_elect + PCt_0;

  K_0 = PCt_0 / E;
  K_1 = PCt_1 / E;

  t_0 = PCt_0;
  t_1 = Pk[0][1];

  Pk[0][0] -= K_0 * t_0;                    //后验估计误差协方差
  Pk[0][1] -= K_0 * t_1;
  Pk[1][0] -= K_1 * t_0;
  Pk[1][1] -= K_1 * t_1;

  curr_elect_val += K_0 * elect_err;                //后验估计 更新最优电磁值 最优电磁值 = 预测值 + 卡尔曼增益*(测量值-预测值)
  q_bias += K_1 * elect_err;                //后验估计 更新误差

  return curr_elect_val;

}


//控制算法部分

static void calculatepwm(float actualang,float expectang,int32_t actualsped,int32_t expectsped)//控制算法运行
{
            //直立环经典pid+fuzzy：pd
            errbala.err=expectang-actualang;//当前误差
            errbala.derr=gyro;//当次误差与上一次误差之间的差值
            errbala.err=KalmanFilter_Elect(errbala.err,errbala.lasterr);//卡尔曼滤波
            errbala.derr=KalmanFilter_Elect(errbala.derr,errbala.lastderr);//卡尔曼滤波
            errbala.lasterr=errbala.err;
            errbala.lastderr=errbala.derr;
            errbala.out=kp*errbala.err+kd*errbala.derr;//算出结果

            //速度环pi
            errsped.err=expectsped-actualsped;//当前误差
            errsped.sum+=errsped.err;//误差积分
            if(errsped.sum>1000)//积分限幅
                errsped.sum=1000;
            else if(errsped.sum<-300)
                errsped.sum=-300;
            errsped.out=kp1*errsped.err+ki1*errsped.sum;//算出结果

            pwmout=errbala.out+errsped.out;//
            if(pwmout>=9999)pwmout=9999;
            else if(pwmout<-9999)pwmout=-9999;


            //这里做最后的输出控制
            if(pwmout>0)
            {
                pwm_duty(PWM2_CH2_A1, pwmout);
                pwm_duty(PWM3_CH1_A6,9000);
            }
            else
            {
                pwm_duty(PWM2_CH2_A1, -pwmout);
                pwm_duty(PWM3_CH1_A6,0);
            }
            //rt_kprintf("pwmout：%d\r\n",(int32_t)pwmout);
}

//编码器值获取及滤波
encoder =(int32_t)(0.7* timer_quad_get(TIMER_4)+0.3*lastenc);//一阶低通滤波去毛刺


//陀螺仪数据获取
if (ucRxBuffer[0]!=0x55) //数据头不对，则重新开始寻找0x55数据头
    {
        ucRxCnt=0;
        return;
    }
    if (ucRxCnt<11) {return;}//数据不满11个，则返回
    else
    {
        if(ucRxBuffer[1]==0x53)
        {
        int i=-1;
        while((i++)<11)
        {
            ANGLE[i]=ucRxBuffer[i];
        }
        }
        else if(ucRxBuffer[1]==0x52)
        {
            int i=-1;
            while((i++)<11)
        {
                GYRO[i]=ucRxBuffer[i];
        }
        }

        ucRxCnt=0;//清空缓存区

    }
    
    roll=((short)(ANGLE[3]<<8|ANGLE[2]))/32768.0*180;//得到角度
        gyro=((short)(GYRO[3]<<8|GYRO[2]))/32768.0*2000;//得到角速度

代码和3D模型见附件