【开源STM32自平衡小车】 教你如何自己DIY一辆双轮自平衡小车

【请认准:OpenSir开源达人】

开源STM32自平衡小车

平衡小车开源资料网盘链接: 平衡小车百度网盘资料链接,点击进入
【哔站视频一键三连后,评论区留言邮箱获取提取码(3天内发至邮箱)】

哔站播放视频链接如下:
链接: 【开源STM32自平衡小车】哔站播放视频效果,点击进行播放

ps:如有不愿意进行自主diy的小伙伴们,我们也为其提供有推荐的套件
链接: STM32自平衡小车开源整车套件,点击即可进行查看

文章目录

提示:以下是本篇文章正文内容

一、硬件篇(附淘宝器件链接,也可自行选购)

1.STM32F103C8T6最小系统板(核心板)

【开源STM32自平衡小车】 教你如何自己DIY一辆双轮自平衡小车

【开源STM32自平衡小车】 教你如何自己DIY一辆双轮自平衡小车
推荐链接:STM32F103C8T6最小系统核心板,点击进入!

; 2.MPU6050姿态传感器(六轴)

【开源STM32自平衡小车】 教你如何自己DIY一辆双轮自平衡小车
推荐链接:MPU6050姿态传感器模块,点击进入!
传感器资料链接:

https://pan.baidu.com/s/1dNDqcp76L9QdM7iSZYfz_A 提取码: 4eum

https://pan.baidu.com/s/13ZoMZ-lpAtDRhey4lKEcFQ 提取码: p7fr

3.TB6612电机驱动(双通道)

【开源STM32自平衡小车】 教你如何自己DIY一辆双轮自平衡小车
【开源STM32自平衡小车】 教你如何自己DIY一辆双轮自平衡小车
【开源STM32自平衡小车】 教你如何自己DIY一辆双轮自平衡小车
推荐链接:TB6612电机驱动(双通道),点击进入!

; 4.0.96寸OLED显示屏

【开源STM32自平衡小车】 教你如何自己DIY一辆双轮自平衡小车
推荐链接:0.96寸OLED显示屏模块(128×64),点击进入!
模块资料链接:

https://pan.baidu.com/s/108smkVOLg-23cAnr37ZeGQ 密码:91t7

https://pan.baidu.com/s/1Lr8lw_6vt_VdM1UWe9CGsA 密码:p0un

5.蓝牙无线通信模块

【开源STM32自平衡小车】 教你如何自己DIY一辆双轮自平衡小车
推荐链接:JDY-31蓝牙无线通信模块,点击进入!
DY-31带底板资料:

https://pan.baidu.com/s/16qyiOO05whOXqYtBVBGs2Q

; 6.电源降压模块(双电压输出)

【开源STM32自平衡小车】 教你如何自己DIY一辆双轮自平衡小车
推荐链接:5V和3.3V双通道输出降压模块,点击进入!(咨询客服)

7.超声波测距模块

【开源STM32自平衡小车】 教你如何自己DIY一辆双轮自平衡小车
推荐链接:HC-SR04超声波测距模块,点击进入!
HC-SR04资料下载:

http://pan.baidu.com/s/1sjHKDI1

HC-SR04单芯片版本资料下载接:

https://pan.baidu.com/s/1sSah9PvLBrmbA7So-6YcSw 提取码: qq35

; 8.动力锂电池(双节18650)

【开源STM32自平衡小车】 教你如何自己DIY一辆双轮自平衡小车
【开源STM32自平衡小车】 教你如何自己DIY一辆双轮自平衡小车
推荐链接:3.7V 18650锂电池,点击进入!

9.小车橡胶车轮(65mm)

【开源STM32自平衡小车】 教你如何自己DIY一辆双轮自平衡小车

; 10.STLINK代码仿真下载器

【开源STM32自平衡小车】 教你如何自己DIY一辆双轮自平衡小车
推荐链接:STLINK V2代码烧录器,点击进入!

STLINK资料网盘链接:

https://pan.baidu.com/s/1gxzJeDe7CJaCCl4pGnwdNQ 提取码: an2m

WIN10系统驱动解决方案

https://pan.baidu.com/s/1OQosxeUMnCe5CZkwKRKhOA 提取码: h9ii

二、软件篇

点击文章顶部 B站视频链接 或者 点击此处 一键三连+关注,留言邮箱即可获取本小车Keil源代码文件

1.主函数代码

代码如下(main.c):


#include "stm32f10x.h"
#include "systick.h"
#include "led.h"
#include "iic.h"
#include "mpu6050.h"
#include "timer.h"
#include "nvic.h"
#include "imu.h"
#include "pwm.h"
#include "flash.h"
#include "pid.h"
#include "usart1.h"
#include "bluetooth.h"
#include "key.h"
#include "ps2.h"
#include "oled.h"
#include "adc.h"
#include "ultrasonic.h"
#include "imath.h"
#include "Infrared.h"

static void _SWJ_Config(void)
{
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE);
    GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable, ENABLE);
}

int main(void)
{
    NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
    _SWJ_Config();
    usart1_init(115200);
    printf("usart1 is ok\r\n");
    BluetoothUsart_init(9600);
    pwm_init();
    driver_pin_init();
    Encoder_A_init();
    Encoder_B_init();
    tdelay_ms(100);
    IIC_Init();
    mpu6050_init();
    while(1)
    {
        uint8_t mpuId;
        mpuId = get_mpu_id();
        if(mpuId==0x68||mpuId==0x98)
            break;
        tdelay_ms(50);
    }
    get_iir_factor(&Mpu.att_acc_factor,0.005f,25);
    OLED_Init();
    adc_init();
    ReadCalData();
    tdelay_ms(100);
    timer_init();
    HCSR04_Init();
    systick_init();
    NVIC_config();

    while(1)
    {
        if(softTimes[0] == 0)
        {
            softTimes[0] = 20;
        }
        if(softTimes[1] == 0)
        {
            softTimes[1] = 4;
            UltrasonicWave_StartMeasure();
            ParseBluetoothMessage(USARTxBlueTooth_RX_DATA, &BluetoothParseMsg);
        }
        if(softTimes[2] == 0)
        {
            softTimes[2] = 20;
            voltage_detection();
            OledShowDatas();
        }
    }
}

2.systick滴答定时器中断任务

代码如下(SysTick_Handler() ):


void SysTick_Handler(void)
{
    softTimesCountdown();
    UltrasonicCheck();
    get_gyro_raw();
    get_deg_s(&gyro_raw_f,&Mpu.deg_s);
    get_rad_s(&gyro_raw_f,&Mpu.rad_s);
    get_acc_raw();
    acc_iir_lpf(&acc_raw_f,&acc_att_lpf,Mpu.att_acc_factor);
    get_acc_g(&acc_att_lpf,&Mpu.acc_g);

    mahony_update(Mpu.rad_s.x,Mpu.rad_s.y,Mpu.rad_s.z,Mpu.acc_g.x,Mpu.acc_g.y,Mpu.acc_g.z);
    Matrix_ready();

    encoder_val_a = read_encoder(ENCODER_A);
    encoder_manage(&encoder_val_a);
    encoder_val_b = read_encoder(ENCODER_B);
    encoder_manage(&encoder_val_b);

    ctr.pwm[0] = ctr_bal(att.rol,Mpu.deg_s.y);
    ctr.pwm[1] = ctr_vel(encoder_val_a,-encoder_val_b);
    ctr.pwm[2] = ctr_turn(encoder_val_a,-encoder_val_b,Mpu.deg_s.z);

    ctr.out[0] = ctr.pwm[0] + ctr.pwm[1] + ctr.pwm[2];
    ctr.out[1] = ctr.pwm[0] + ctr.pwm[1] - ctr.pwm[2];

    i_limit(&(ctr.out[0]),AMPLITUDE);
    i_limit(&(ctr.out[1]),AMPLITUDE);

    dir_config(&(ctr.out[0]),&(ctr.out[1]));

    _ctr_out();

    ANO_DMA_SEND_DATA();
    gyro_cal(&gyro_raw_cal);
}

3.滴答定时器5ms中断配置

代码如下( systick_init ):


void systick_init(void)
{
    SystemCoreClockUpdate();

    if (SysTick_Config(SystemCoreClock / 200))
    {

        while (1);
    }

    SysTick->CTRL |=  SysTick_CTRL_ENABLE_Msk;
}

4.调试串口发送数据至上位机

代码如下( ANO_DMA_SEND_DATA ):

vvoid ANO_DMA_SEND_DATA(void)
{
    static uint8_t ANO_debug_cnt = 0;
    ANO_debug_cnt++;
    if(ANO_debug_cnt==1)
    {
        ANO_DT_Send_Status();
    }
    else if(ANO_debug_cnt==2)
    {
        ANO_DT_Send_Senser((int16_t)acc_raw.x,(int16_t)acc_raw.y,(int16_t)acc_raw.z,
                           (int16_t)gyro_raw.x,(int16_t)gyro_raw.y,(int16_t)gyro_raw.z,
                           (int16_t)0,(int16_t)0,(int16_t)0);
    }
    else if(ANO_debug_cnt==3)
    {
        ANO_DT_Send_RCData(1100,1200,1300,1400,1500,1600,1700,1800,1900,1100);
    }
    else if(ANO_debug_cnt==4)
    {
        ANO_DT_Send_Power(13.52,57.63);
    }
    else if(ANO_debug_cnt==5)
    {
        ANO_DT_Send_User(0,0,0,0,0,
                        encoder_val_a, encoder_val_b,ctr.pwm[0],ctr.pwm[1],acc_raw_f.z,
                        0,0,0,0,0);
    }
    else if(ANO_debug_cnt==6)
    {
        if(f.send_pid1)
        {
            f.send_pid1 = 0;

            ANO_DT_Send_PID(1,  vel.kp*0.1f,
                                vel.ki,
                                vel.kd,
                                1.5f,
                                2.5f,
                                1.5f,
                                2.5f,
                                1.5f,
                                2.5f);
        }
        if(f.send_pid2)
        {
            f.send_pid2 = 0;

            ANO_DT_Send_PID(2,  bal.kp*0.1f,
                                bal.ki,
                                bal.kd,
                                2.6f,
                                1.5f,
                                2.4f,
                                1.1f,
                                2.2f,
                                1.1f);
        }
        if(CalGyro.success==1)
        {
            CalGyro.success = 0;
            ANO_DT_Send_MSG(SENSOR_GYRO,CAL_SUCCESS);
        }
        else if(CalGyro.success==2)
        {
            CalGyro.success = 0;
            ANO_DT_Send_MSG(SENSOR_GYRO,CAL_SUCCESS);
        }
    }
    else if(ANO_debug_cnt==7)
    {
        ANO_debug_cnt = 0;
    }
}

5.ADC采用DMA方式进行数据采集

代码如下:


#include "adc.h"

_ADC_VALUE bat = {0 ,0 };

uint16_t adc_value[1];

void adc_gpio_init(void)
{
    GPIO_InitTypeDef GPIO_initStructure;
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE);

    GPIO_initStructure.GPIO_Pin = GPIO_Pin_2;
    GPIO_initStructure.GPIO_Mode = GPIO_Mode_AIN;
    GPIO_Init(GPIOA,&GPIO_initStructure);
}

void adc_config(void)
{
    ADC_InitTypeDef ADC_initStructure;

    RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1,ENABLE);

    ADC_initStructure.ADC_ContinuousConvMode = ENABLE;
    ADC_initStructure.ADC_DataAlign = ADC_DataAlign_Right;
    ADC_initStructure.ADC_ExternalTrigConv = ADC_ExternalTrigConv_None;
    ADC_initStructure.ADC_Mode = ADC_Mode_Independent;
    ADC_initStructure.ADC_NbrOfChannel = 1;
    ADC_initStructure.ADC_ScanConvMode = DISABLE;
    ADC_Init(ADC1,&ADC_initStructure);

    ADC_Cmd(ADC1,ENABLE);

    ADC_DMACmd(ADC1,ENABLE);

    RCC_ADCCLKConfig(RCC_PCLK2_Div8);

    ADC_RegularChannelConfig(ADC1,ADC_Channel_2,1,ADC_SampleTime_239Cycles5);

    ADC_ResetCalibration(ADC1);
    while(ADC_GetCalibrationStatus(ADC1));
    ADC_StartCalibration(ADC1);
    while(ADC_GetCalibrationStatus(ADC1));

    ADC_SoftwareStartConvCmd(ADC1,ENABLE);
}

void dma_config(void)
{
    DMA_InitTypeDef DMA_initStructure;

    RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1,ENABLE);

    DMA_initStructure.DMA_BufferSize = 1;
    DMA_initStructure.DMA_DIR = DMA_DIR_PeripheralSRC;
    DMA_initStructure.DMA_M2M = DMA_M2M_Disable;
    DMA_initStructure.DMA_MemoryBaseAddr = (u32)adc_value;
    DMA_initStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_HalfWord;
    DMA_initStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
    DMA_initStructure.DMA_Mode = DMA_Mode_Circular;
    DMA_initStructure.DMA_PeripheralBaseAddr = ((u32)&ADC1->DR);
    DMA_initStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_HalfWord;
    DMA_initStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
    DMA_initStructure.DMA_Priority = DMA_Priority_Medium;
    DMA_Init(DMA1_Channel1,&DMA_initStructure);

    DMA_Cmd(DMA1_Channel1,ENABLE);
}

void adc_init(void)
{
    adc_gpio_init();
    adc_config();
    dma_config();
}

void voltage_detection(void)
{
    bat.adc = adc_value[0];
    bat.voltage = (float)(bat.adc)/4096.0f*3.3f*11.0f;

    if(bat.voltage7.0f)
    {
        bat.danger_flag = 1;
    }
    else
    {
        bat.danger_flag = 0;
    }
}

6.陀螺仪数据校准

代码如下:


void gyro_cal(SI_F_XYZ *gyro_in)
{
    if(CalGyro.flag==1 && 1)
    {
        if(CalGyro.i < GyroCalSumValue)
        {
            CalGyro.offset.x += gyro_in->x;
            CalGyro.offset.y += gyro_in->y;
            CalGyro.offset.z += gyro_in->z;
            CalGyro.i++;
        }
        else
        {
            CalGyro.i = 0;
            CalGyro.OffsetFlashWrite.x = CalGyro.offset.x / GyroCalSumValue;
            CalGyro.OffsetFlashWrite.y = CalGyro.offset.y / GyroCalSumValue;
            CalGyro.OffsetFlashWrite.z = CalGyro.offset.z / GyroCalSumValue;

            FlashWriteNineFloat(SENSOR_CAL_ADDRESS, 0,0,0,
                                                    0,0,0,
                                                    CalGyro.OffsetFlashWrite.x,CalGyro.OffsetFlashWrite.y,CalGyro.OffsetFlashWrite.z);

            flash_flag.sensor = FlashReadNineFloat(SENSOR_CAL_ADDRESS,  &CalGyro.None.x,
                                                                        &CalGyro.None.y,
                                                                        &CalGyro.None.z,
                                                                        &CalGyro.None.x,
                                                                        &CalGyro.None.y,
                                                                        &CalGyro.None.z,

                                                                        &CalGyro.OffsetFlashRead.x,
                                                                        &CalGyro.OffsetFlashRead.y,
                                                                        &CalGyro.OffsetFlashRead.z);

            if(flash_flag.sensor!=0x01ff && flash_flag.sensor!=0x01C0)
            {
                _set_val(&gyro_offset,&CalGyro.OffsetFlashRead);
                CalGyro.success = 1;
                _led.sta = 0;
            }
            else
            {
                CalGyro.success = 2;
            }
            CalGyro.offset.x = 0;
            CalGyro.offset.y = 0;
            CalGyro.offset.z = 0;
            CalGyro.flag = 0;
        }
    }
}

int ReadCalData(void)
{
    ErrorStatus err;
    flash_flag.sensor = FlashReadNineFloat(SENSOR_CAL_ADDRESS,  &CalGyro.None.x,
                                                                &CalGyro.None.y,
                                                                &CalGyro.None.z,
                                                                &CalGyro.None.x,
                                                                &CalGyro.None.y,
                                                                &CalGyro.None.z,

                                                                &CalGyro.OffsetFlashRead.x,
                                                                &CalGyro.OffsetFlashRead.y,
                                                                &CalGyro.OffsetFlashRead.z);

    if(flash_flag.sensor!=0x01ff && flash_flag.sensor!=0x01C0)
    {
        _set_val(&gyro_offset, &CalGyro.OffsetFlashRead);
        err = SUCCESS ;
    }
    else
    {
        err = ERROR;
    }

    return err;
}

7.超声波测距控制

代码如下:

float getDistance;
uint16_t EchoFlag = 0;
uint16_t disCounter;
void EXTI15_10_IRQHandler(void)
{
    disCounter = 0;
    tdelay_us(3);
    if(EXTI_GetITStatus(EXTI_Line13) != RESET)
    {
        EchoFlag = 1;
        TIM_SetCounter(TIM1,0);
        TIM_Cmd(TIM1, ENABLE);

        while(GPIO_ReadInputDataBit(HCSR04_ECHO_PORT,HCSR04_ECHO_PIN))
        {
            disCounter = TIM_GetCounter(TIM1);
            if(disCounter>5000)
                break;
        }

        TIM_Cmd(TIM1, DISABLE);

        getDistance = TIM_GetCounter(TIM1)/1000000.0f * 340.0f / 2.0f * 100.0f;

        EXTI_ClearITPendingBit(EXTI_Line13);
    }
}

8.欧拉角姿态解析

代码如下( mahony_update(float gx, float gy, float gz, float ax, float ay, float az) ):


#include "imu.h"
#include "imath.h"
#include "math.h"
#include "mpu6050.h"
#include "timer.h"

_Matrix Mat = {0};
_Attitude att = {0};
#define mahonyPERIOD        5.0f
#define kp      0.5f
#define ki      0.0001f

float q0 = 1, q1 = 0, q2 = 0, q3 = 0;
float exInt = 0, eyInt = 0, ezInt = 0;

void mahony_update(float gx, float gy, float gz, float ax, float ay, float az)
{
    float norm;
    float vx, vy, vz;
    float ex, ey, ez;

    if(ax*ay*az==0)
        return;

    norm = invSqrt(ax*ax + ay*ay + az*az);
    ax = ax * norm;
    ay = ay * norm;
    az = az * norm;

    vx = Mat.DCM_T[0][2];
    vy = Mat.DCM_T[1][2];
    vz = Mat.DCM_T[2][2];

    ex = ay*vz - az*vy;
    ey = az*vx - ax*vz;
    ez = ax*vy - ay*vx;

    exInt = exInt + ex*ki;
    eyInt = eyInt + ey*ki;
    ezInt = ezInt + ez*ki;

    gx = gx + kp*ex + exInt;
    gy = gy + kp*ey + eyInt;
    gz = gz + kp*ez + ezInt;

    q0 = q0 + (-q1*gx - q2*gy - q3*gz)* mahonyPERIOD * 0.0005f;
    q1 = q1 + ( q0*gx + q2*gz - q3*gy)* mahonyPERIOD * 0.0005f;
    q2 = q2 + ( q0*gy - q1*gz + q3*gx)* mahonyPERIOD * 0.0005f;
    q3 = q3 + ( q0*gz + q1*gy - q2*gx)* mahonyPERIOD * 0.0005f;

    norm = invSqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
    q0 = q0 * norm;
    q1 = q1 * norm;
    q2 = q2 * norm;
    q3 = q3 * norm;

    att.pit =  atan2(2.0f*(q0*q1 + q2*q3),q0*q0 - q1*q1 - q2*q2 + q3*q3) * rad_to_angle;
    att.rol =  asin(2.0f*(q0*q2 - q1*q3)) * rad_to_angle;

    att.yaw += Mpu.deg_s.z  * mahonyPERIOD * 0.001f;
}

void rotation_matrix(void)
{
    Mat.DCM[0][0] = 1.0f - 2.0f * q2*q2 - 2.0f * q3*q3;
    Mat.DCM[0][1] = 2.0f * (q1*q2 -q0*q3);
    Mat.DCM[0][2] = 2.0f * (q1*q3 +q0*q2);

    Mat.DCM[1][0] = 2.0f * (q1*q2 +q0*q3);
    Mat.DCM[1][1] = 1.0f - 2.0f * q1*q1 - 2.0f * q3*q3;
    Mat.DCM[1][2] = 2.0f * (q2*q3 -q0*q1);

    Mat.DCM[2][0] = 2.0f * (q1*q3 -q0*q2);
    Mat.DCM[2][1] = 2.0f * (q2*q3 +q0*q1);
    Mat.DCM[2][2] = 1.0f - 2.0f * q1*q1 - 2.0f * q2*q2;
}

void rotation_matrix_T(void)
{
    Mat.DCM_T[0][0] = 1.0f - 2.0f * q2*q2 - 2.0f * q3*q3;
    Mat.DCM_T[0][1] = 2.0f * (q1*q2 +q0*q3);
    Mat.DCM_T[0][2] = 2.0f * (q1*q3 -q0*q2);

    Mat.DCM_T[1][0] = 2.0f * (q1*q2 -q0*q3);
    Mat.DCM_T[1][1] = 1.0f - 2.0f * q1*q1 - 2.0f * q3*q3;
    Mat.DCM_T[1][2] = 2.0f * (q2*q3 +q0*q1);

    Mat.DCM_T[2][0] = 2.0f * (q1*q3 +q0*q2);
    Mat.DCM_T[2][1] = 2.0f * (q2*q3 -q0*q1);
    Mat.DCM_T[2][2] = 1.0f - 2.0f * q1*q1 - 2.0f * q2*q2;
}

void Matrix_ready(void)
{
    rotation_matrix();
    rotation_matrix_T();
}

9.PID控制器

代码如下:


int ctr_bal(float angle,float gyro)
{
    ctr.exp[0] = 0;
    ctr.bais[0] = (float)(angle - ctr.exp[0]);
    ctr.balance = bal.kp * ctr.bais[0] + gyro * bal.kd;

    return ctr.balance;
}
int CarStepForwardOrBackward;

int ctr_vel(int encoder_left,int encoder_right)
{
    static float encoder_cur,encoder_last;

    CarStepForwardOrBackward = CarSpeedCtrlForwardOrBackward(1);
    remote.ForwardOrBackward = remoteCarForwardOrBackward( (float)CarStepForwardOrBackward , MODE_BLUETEETH , UltraSuccess(BluetoothParseMsg.ModeRocker) );

    encoder_last = ((encoder_left) + (encoder_right)) - 0;
    encoder_cur = encoder_cur * 0.8 + encoder_last * 0.2;
    ctr.bais[1] += encoder_cur;

    ctr.bais[1] = ctr.bais[1] - remote.ForwardOrBackward;

    if(ctr.bais[1] > 10000)     ctr.bais[1] = 10000;
    if(ctr.bais[1] <-10000)     ctr.bais[1] =-10000;
    ctr.velocity = encoder_cur * vel.kp + ctr.bais[1] * vel.ki;

    return ctr.velocity;
}

10.蓝牙数据包接收并解析

代码如下( ParseBluetoothMessage(char pInput ,uint16_t rcLens , BluetoothParse blueParseMsg ) ):

*
 * 函数名:USART3_IRQHandler
 * 描述  :蓝牙串口接收中断处理函数
 * 输入  :
 * 返回  :
 */
void USART3_IRQHandler(void)
{
    if(USART_GetITStatus(BLUETOOTH_USARTx, USART_IT_RXNE) != RESET)
    {
        if(BlueToothBufIndex > USARTxBlueTooth_RX_LEN)
            BlueToothBufIndex = 0;
        USARTxBlueTooth_RX_DATA[BlueToothBufIndex++] = USART_ReceiveData(BLUETOOTH_USARTx);

        BlueToothNumbers ++;
        USART_ClearITPendingBit(BLUETOOTH_USARTx, USART_IT_RXNE);
    }
}

uint8_t ParseBluetoothMessage(char *pInput , BluetoothParse *blueParseMsg)
{
    unsigned char plen,sum,check;
    ErrorStatus err;
    char *pdata = pInput;

    while(( pdata-pInput ) < BlueToothBufIndex )
    {
        if(*pdata == '#')
        {
            plen = (unsigned char)atof(strtok(pdata+1, ","));
            if(plen == 9)
            {

                sum =   (unsigned char)int_abs( ((int)atof(strtok(pdata+3, ",")) +
                        (int)atof(strtok(NULL, ",")) +
                        (int)atof(strtok(NULL, ",")) +
                        (int)atof(strtok(NULL, ","))) - 7 );

                check = (unsigned char)atof(strtok(NULL, ",")) ;
                if(sum == check)
                {
                    blueParseMsg->ModeRocker = (unsigned char)atof(strtok(pdata+3, ","));
                    blueParseMsg->OpenCloseStatus = (unsigned char)atof(strtok(pdata+5, ","));
                    blueParseMsg->Xrocker = (unsigned char)atof(strtok(pdata+7, ","));
                    blueParseMsg->Yrocker = (unsigned char)atof(strtok(pdata+9, ","));
                    err = SUCCESS;
                }
                else
                {
                    err = ERROR;
                }
            }
            else
            {
                err = ERROR;
            }
        }
        else
        {
            err = ERROR;
        }
        pdata++;
    }
    BlueToothBufIndex = 0;

    return err ;
}

三、原理图

【开源STM32自平衡小车】 教你如何自己DIY一辆双轮自平衡小车

【 PS:点击文章顶部 B站视频链接 或者 点击此处 一键三连+关注,留言邮箱即可获取本小车原理图纸文件。】

Original: https://blog.csdn.net/flight_solar/article/details/124802585
Author: Open开源达人
Title: 【开源STM32自平衡小车】 教你如何自己DIY一辆双轮自平衡小车

原创文章受到原创版权保护。转载请注明出处:https://www.johngo689.com/559210/

转载文章受原作者版权保护。转载请注明原作者出处!

(0)

大家都在看

亲爱的 Coder【最近整理,可免费获取】👉 最新必读书单  | 👏 面试题下载  | 🌎 免费的AI知识星球