树莓派 python 平衡小车

不知不觉忙活了个把月，现在小车终于可以慢速的前进和转向。网上大部分平衡小车都是基于stm32的，但是对于我这样其他从业者来说，树莓派python更容易上手，以后延伸其他功能也容易。

简单回归下
硬件
1.树莓派3b
2.mpu6050
3.L298n电机驱动，买的的6612电机驱动 因为疫情一直没有发出，不知道换上后会不会有提升
4.8811cu 双频网卡，板载的网卡不知道为什么坏了
5.DC降压模块给树莓派供电
6.带编码器130电机，现在的问题就是这个电机转速可能太低，参数是 12v 空载7800转/min，我用的1/48的减速箱，车轮理论最大160转，但是实测才120转每分，可能就是这个原因造成小车性能弱的问题，打算买个升压模块把电压提高试试
7.电源 3串18650电池加了保护板，为什么不用航模电池，钟爱18650加保护板不怕电池短路过放

软件
1.调试mpu6050 获取角度，这个网上有很多
2.对获取的角度进行滤波，我用了简单好理解的 一阶互补滤波

angle_P=0      #计算后的角度

def yijiehubu_P(angle_m,gyro_m): 
    global angle_P
    dt=0.005  #0.002   
    K1 =0.05   # K值是两则的比例
    angle_P= K1 * angle_m + (1-K1) * (angle_P + gyro_m * dt)
    #print(" 滤波:",str(angle_P)[:5],end='')
    return angle_P

3.调试自立环，速度环，角度环待调 现在用差数转弯

def zhilihuan(y2,gyro_yout_scaled,Vout):     #直立环   y2实际角度
    global zl,Kp,Kd,fre,err_last,flag_stop,flag_z
    
    print("\rKp",Kp,end='')
    print(" Kd",Kd,end='')     #最 gy大260  1000/260=3.8(Kd) 3.
 
    DC = Kp*(float(y2)-zl)+ Kd*gyro_yout_scaled    #zl 机械中立值     
    DC=DC+Vout
    DC=int(DC*1000)   #还剩1000档，k 125 开始调试    
    
    if flag_stop==1:     #电机停止
        DC=0
    
    if DC>1000000:    #设置限幅
        DC=1000000
    if DC<-1000000:
        DC=-1000000
    
    DC2=DC  # DC2发送给上位机查看数据用
    
    if flag_z==1:
        cha1=300000
        cha2=0
    elif flag_z==2:
        cha1=0
        cha2=300000
    else:
        cha1=0
        cha2=0
    
    if DC>=0:  
        DC_R=DC-cha1
        DC_L=DC-cha2
        if DC_R<0:
            DC_R=0
        if DC_L<0:
            DC_L=0

        pi.write(17,0) 
        pi.write(18,1) 
        pi.write(23,0)
        pi.write(24,1)  
        pi.hardware_PWM(12, fre, DC_R)    # 250000 25% 
        pi.hardware_PWM(19, fre, DC_L)    # 250000 25%
        #print(" #前进#",end='')
       
    if DC<=0:
        DC=abs(DC)    #取绝对值        
        pi.write(17,1)
        pi.write(18,0) 
        pi.write(23,1)
        pi.write(24,0)
        pi.hardware_PWM(12, fre, DC)    # 250000 25%
        pi.hardware_PWM(19, fre, DC)    # 250000 25%
    
    return DC2    
        
#++++++++++++++++++++++++++++++++++++++速度环++++++++++++++++++++++++++++++++++++++++++++++++++++++++

I_bias=0  
bias=0
Movement=0

def shuduhuan(Encoder_real_L,Encoder_real_R):   #10ms的值，10ms后要清零 M法
    global Kp2,Ki,geidingshudu,I_bias,bias,Movement         #观察下L+R=H和的最大值 50
      #Kp(MAX)=1000/(h*0.5)   kp max 40
    
    bias_least=(Encoder_real_L+Encoder_real_R-geidingshudu)   #两个编码器用 不用纠结/2
    bias*=0.7
    bias+=bias_least*0.3            #滤波
    I_bias+=bias      #对偏差积分
    I_bias=I_bias-Movement
    
    if I_bias>10000:
        I_bias=10000
    if I_bias<-10000:
        I_bias=-10000
    
    #print(" | bias_least",bias_least,end='')
    print(" | bias",str(bias)[:6],end='')
    print(" | I_bias",str(I_bias)[:6],end='')
    print(" Mo",Movement,end='')

    Vout=Kp2*bias+Ki*I_bias       #核心还是偏差控制，偏差逐向给定值 
                                #Kp Ki 记性相同                           
    print(" Vout",Vout,end='')
    
    return Vout
  

4.开发基于网络的窗口版上位机，图形话调整pid参数

5.下一步打算开发网页版的控制端