APM32F411V Tiny Board测评】-6-基于压力播磨传感器和mpu6050的振动马达驱动

只看该作者 · 2024-5-31 00:59

本帖最后由 chenqiguang1998 于 2024-6-3 11:17 编辑

引脚配置，具体配置如下：

MPU6050：I2C1_SDA（PB7），I2C1_SCL（PB6）
压力薄膜传感器：ADC1_IN1（PA1）
振动马达：PWM1_CH1（PB4）
OLED：I2C2_SDA（PB3），I2C2_SCL（PB10）
按键：KEY1（PA0），KEY2（PA1）

GPIO初始化



void MX_GPIO_Init(void) 

{

    GPIO_InitTypeDef GPIO_InitStruct = {0};



    // Enable GPIO Clocks

    __DAL_RCM_GPIOA_CLK_ENABLE();

    __DAL_RCM_GPIOB_CLK_ENABLE();

    __DAL_RCM_GPIOE_CLK_ENABLE();



    // Configure GPIO pins for buttons (PA0, PA1)

    GPIO_InitStruct.Pin = GPIO_PIN_0 | GPIO_PIN_1;

    GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING;

    GPIO_InitStruct.Pull = GPIO_NOPULL;

    DAL_GPIO_Init(GPIOA, &GPIO_InitStruct);



    // Configure GPIO pin for PWM (PB4)

    GPIO_InitStruct.Pin = GPIO_PIN_4;

    GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;

    GPIO_InitStruct.Pull = GPIO_NOPULL;

    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;

    GPIO_InitStruct.Alternate = GPIO_AF1_TMR1;

    DAL_GPIO_Init(GPIOB, &GPIO_InitStruct);



    // EXTI interrupt init

    DAL_NVIC_SetPriority(EXTI0_IRQn, 2, 0);

    DAL_NVIC_EnableIRQ(EXTI0_IRQn);



    DAL_NVIC_SetPriority(EXTI1_IRQn, 2, 0);

    DAL_NVIC_EnableIRQ(EXTI1_IRQn);

}

I2C初始化部分

复制

I2C_HandleTypeDef hi2c1;

I2C_HandleTypeDef hi2c2;



void MX_I2C1_Init(void) {

    hi2c1.Instance = I2C1;

    hi2c1.Init.ClockSpeed = 100000;

    hi2c1.Init.DutyCycle = I2C_DUTYCYCLE_2;

    hi2c1.Init.OwnAddress1 = 0;

    hi2c1.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;

    hi2c1.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE;

    hi2c1.Init.OwnAddress2 = 0;

    hi2c1.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE;

    hi2c1.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE;

    DAL_I2C_Init(&hi2c1);

}



void MX_I2C2_Init(void) {

    hi2c2.Instance = I2C2;

    hi2c2.Init.ClockSpeed = 100000;

    hi2c2.Init.DutyCycle = I2C_DUTYCYCLE_2;

    hi2c2.Init.OwnAddress1 = 0;

    hi2c2.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;

    hi2c2.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE;

    hi2c2.Init.OwnAddress2 = 0;

    hi2c2.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE;

    hi2c2.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE;

    DAL_I2C_Init(&hi2c2);

}

ADC 初始化和功能设置

复制



ADC_HandleTypeDef hadc1;



void MX_ADC1_Init(void) {

    ADC_ChannelConfTypeDef sConfig = {0};



    hadc1.Instance = ADC1;

    hadc1.Init.ScanConvMode = DISABLE;

    hadc1.Init.ContinuousConvMode = DISABLE;

    hadc1.Init.DiscontinuousConvMode = DISABLE;

    hadc1.Init.ExternalTrigConv = ADC_SOFTWARE_START;

    hadc1.Init.DataAlign = ADC_DATAALIGN_RIGHT;

    hadc1.Init.NbrOfConversion = 1;

    DAL_ADC_Init(&hadc1);



    sConfig.Channel = ADC_CHANNEL_1;

    sConfig.Rank = 1U;

    sConfig.SamplingTime = ADC_SAMPLETIME_3CYCLES;

    DAL_ADC_ConfigChannel(&hadc1, &sConfig);

}



u16 Get_Adc(ADC_HandleTypeDef *hadc1)

{    

    while(!__DAL_ADC_GET_FLAG(&hadc1, ADC_FLAG_EOC ));//等待转换结束

    return DAL_ADC_GetValue(&hadc1); //返回最近一次ADC1规则组的转换结果

}



u16 Get_Adc_Average(ADC_HandleTypeDef *hadc1, u8 times) {

    u32 temp_sum = 0;

    u16 adc_value;

    u8 t;

    for(t = 0; t < times; t++) {

        adc_value = Get_Adc(hadc1); 

        temp_sum += adc_value;

        DAL_Delay(5); 

    }

    return (times > 1) ? (u16)(temp_sum / times) : adc_value;

}

PWM 初始化和功能设置

复制



TMR_HandleTypeDef hTMR1;



void MX_TMR1_Init(void) {

    TMR_OC_InitTypeDef sConfigOC = {0};



    hTMR1.Instance = TMR1;

    hTMR1.Init.Prescaler = 0;

    hTMR1.Init.CounterMode = TMR_COUNTERMODE_UP;

    hTMR1.Init.Period = 255;

    hTMR1.Init.ClockDivision = TMR_CLOCKDIVISION_DIV1;

    hTMR1.Init.RepetitionCounter = 0;

    if (DAL_TMR_PWM_Init(&hTMR1) != DAL_OK) {

        DAL_ErrorHandler();

    }



    sConfigOC.OCMode = TMR_OCMODE_PWM1;

    sConfigOC.Pulse = 0;

    sConfigOC.OCPolarity = TMR_OCPOLARITY_HIGH;

    sConfigOC.OCFastMode = TMR_OCFAST_DISABLE;

    if (DAL_TMR_PWM_ConfigChannel(&hTMR1, &sConfigOC, TMR_CHANNEL_1) != DAL_OK) {

        DAL_ErrorHandler();

    }



    DAL_TMR_PWM_MspInit(&hTMR1);

}



// 设置 PWM 占空比和频率的函数

void setPWM(uint32_t dutyCycle, uint32_t frequency) {

    uint32_t period = DAL_RCC_GetHCLKFreq() / frequency;

    __DAL_TMR_SET_AUTORELOAD(&hTMRx, period);

    __DAL_TMR_SET_COMPARE(&hTMRx, TMR_CHANNEL_x, dutyCycle);

    __DAL_TMR_ENABLE(&hTMRx);

}



// 5. UART Initialization





UART_HandleTypeDef huart1;



void MX_USART1_UART_Init(void) {

    huart1.Instance = USART1;

    huart1.Init.BaudRate = 115200;

    huart1.Init.WordLength = UART_WORDLENGTH_8B;

    huart1.Init.StopBits = UART_STOPBITS_1;

    huart1.Init.Parity = UART_PARITY_NONE;

    huart1.Init.Mode = UART_MODE_TX_RX;

    huart1.Init.HwFlowCtl = UART_HWCONTROL_NONE;

    huart1.Init.OverSampling = UART_OVERSAMPLING_16;

    DAL_UART_Init(&huart1);

}

马达控制功能函数

复制

// PWM 参数设置函数

void set_soft_mode_params(void) {

    setPWM(50, 1000);  // 设置占空比为 50%，频率为 1000Hz

}



void set_normal_mode_params(void) {

    setPWM(80, 1500);  // 设置占空比为 80%，频率为 1500Hz

}



void set_strong_mode_params(void) {

    setPWM(100, 2000);  // 设置占空比为 100%，频率为 2000Hz

}



// 渐进模式启动函数

void start_progressive_mode() {

    uint32_t step = 10;  // 渐进模式的步长

    uint32_t currentDutyCycle = 0;  // 渐进模式的当前占空比



    while (currentDutyCycle <= 100) {  // 渐进模式持续到占空比达到 100%

        setPWM(currentDutyCycle, 1000);  // 设置当前占空比和频率

        currentDutyCycle += step;  // 增加占空比

        DAL_Delay(100); 

    }

}

传感器基于模式启动函数

复制

void start_sensor_based_mode() {

process_sensor_data_and_set_motor_strength();

        DAL_Delay(100); 

    }

}



void set_stop_mode_params(void) {

    setPWM(100, 2000);  // 设置占空比为 100%，频率为 2000Hz

}

Button 检测功能

按键有按键一，按键二，分别是pa0 pa1,按下按键1时候菜单选中项向上一个，按下按键2时候，菜单选中项向下一个，两个同时按下一下为选中，并启动相应功能，
短时间按下两次为返回上级菜单，如果没有上级菜单则停止功能，同时按下两秒，则停止并停留在当前选项。菜单分为功能菜单和测试菜单，
功能菜单分为轻柔、普通、强力三种模式来驱动振动马达，测试菜单分为强弱渐进和依靠MPU6050和薄膜压力传感器读数为权重来计算强度；
功能菜单设置振动马达的力度基数，测试菜单来确定变化逻辑

复制

// 全局变量声明

volatile uint32_t button1_press_tick = 0;

volatile uint32_t button2_press_tick = 0;

volatile uint8_t button1_pressed = 0;

volatile uint8_t button2_pressed = 0;

volatile uint8_t button1_double_click = 0;

volatile uint8_t button2_double_click = 0;

volatile uint8_t buttons_held = 0;



// 定义菜单选项

typedef enum {

    MENU_FUNCTION,

    MENU_TEST,

    MENU_FUNCTION_SOFT,

    MENU_FUNCTION_NORMAL,

    MENU_FUNCTION_STRONG,

    MENU_TEST_PROGRESSIVE,

    MENU_TEST_SENSOR_BASED

} Menu;



Menu current_menu = MENU_FUNCTION;

Menu previous_menu = MENU_FUNCTION_SOFT;



// GPIO中断回调函数

void DAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) {

    uint32_t current_tick = DAL_GetTick();



    if (GPIO_Pin == GPIO_PIN_0) {

        if (current_tick - button1_press_tick < 250) {

            button1_double_click = 1;

        }

        button1_press_tick = current_tick;

        button1_pressed = 1;

    } else if (GPIO_Pin == GPIO_PIN_1) {

        if (current_tick - button2_press_tick < 250) {

            button2_double_click = 1;

        }

        button2_press_tick = current_tick;

        button2_pressed = 1;

    }

}



// 检查按钮保持状态

void check_button_hold(void) {

    uint32_t current_tick = DAL_GetTick();



    if ((DAL_GPIO_ReadPin(GPIOA, GPIO_PIN_0) == GPIO_PIN_SET) &&

        (DAL_GPIO_ReadPin(GPIOA, GPIO_PIN_1) == GPIO_PIN_SET)) {

        if (current_tick - button1_press_tick >= 2000) {

            buttons_held = 1;

        }

    } else {

        button1_press_tick = current_tick;

        button2_press_tick = current_tick;

        buttons_held = 0;

    }

}



// 处理按钮按下事件

void handle_button_press(void) {

    if (buttons_held) {

        // 两个按键同时按下并保持两秒，则停止并停留在当前选项

        set_stop_mode_params();// 实现停止功能的逻辑

        buttons_held = 0;

    } else if (button1_double_click || button2_double_click) {

        // 短时间内双击按钮，返回上一级菜单或停止功能

        if (current_menu == MENU_FUNCTION || current_menu == MENU_TEST) {

            // 如果是主菜单，则停止功能

            set_stop_mode_params();// 实现停止功能的逻辑

        } else {

            // 返回上一级菜单

            current_menu = previous_menu;

            display_menu(current_menu);

        }

        button1_double_click = 0;

        button2_double_click = 0;

    } else if (button1_pressed) {

        // 按键1按下，菜单项向上移动

        navigate_menu(-1);

        button1_pressed = 0;

    } else if (button2_pressed) {

        // 按键2按下，菜单项向下移动

        navigate_menu(1);

        button2_pressed = 0;

    } else if (button1_pressed && button2_pressed) {

        // 两个按键同时按下，选中当前菜单项并启动相应功能

        execute_current_menu_function();

        button1_pressed = 0;

        button2_pressed = 0;

    }

}

// 菜单导航函数

void navigate_menu(int direction) {

    previous_menu = current_menu;



    switch (current_menu) {

        case MENU_FUNCTION:

            current_menu = (direction > 0) ? MENU_TEST : MENU_TEST;

            break;

        case MENU_TEST:

            current_menu = (direction > 0) ? MENU_FUNCTION_SOFT : MENU_FUNCTION;

            break;

        case MENU_FUNCTION_SOFT:

            current_menu = (direction > 0) ? MENU_FUNCTION_NORMAL : MENU_FUNCTION_STRONG;

            break;

        case MENU_FUNCTION_NORMAL:

            current_menu = (direction > 0) ? MENU_FUNCTION_STRONG : MENU_FUNCTION_SOFT;

            break;

        case MENU_FUNCTION_STRONG:

            current_menu = (direction > 0) ? MENU_TEST_PROGRESSIVE : MENU_FUNCTION_NORMAL;

            break;

        case MENU_TEST_PROGRESSIVE:

            current_menu = (direction > 0) ? MENU_TEST_SENSOR_BASED : MENU_FUNCTION_STRONG;

            break;

        case MENU_TEST_SENSOR_BASED:

            current_menu = (direction > 0) ? MENU_FUNCTION : MENU_TEST_PROGRESSIVE;

            break;

    }

    display_menu(current_menu);

}



// 执行当前菜单功能

void execute_current_menu_function(void) {

    switch (current_menu) {

        case MENU_FUNCTION_SOFT:

            set_soft_mode_params();

            break;

        case MENU_FUNCTION_NORMAL:

            set_normal_mode_params();

            break;

        case MENU_FUNCTION_STRONG:

            set_strong_mode_params();

            break;

        case MENU_TEST_PROGRESSIVE:

            start_progressive_mode();

            break;

        case MENU_TEST_SENSOR_BASED:

            start_sensor_based_mode();

            break;

        default:

            break;

    }

}

OLED 显示功能

复制

typedef enum {

    MENU_FUNCTION,

    MENU_TEST,

    MENU_FUNCTION_SOFT,

    MENU_FUNCTION_NORMAL,

    MENU_FUNCTION_STRONG,

    MENU_TEST_PROGRESSIVE,

    MENU_TEST_SENSOR_BASED

} Menu;



Menu current_menu = MENU_FUNCTION;

Menu previous_menu = MENU_FUNCTION_SOFT;



void display_menu(Menu menu) {

    ssd1306_Fill(SSD1306_COLOR_BLACK);

    ssd1306_SetCursor(0, 0);

    ssd1306_WriteString("Menu:", SSD1306_FONT_6X8, SSD1306_COLOR_WHITE);

    ssd1306_SetCursor(0, 16);



    switch (menu) {

        case MENU_FUNCTION:

            ssd1306_WriteString("Function Menu", SSD1306_FONT_6X8, SSD1306_COLOR_WHITE);

            break;

        case MENU_TEST:

            ssd1306_WriteString("Test Menu", SSD1306_FONT_6X8, SSD1306_COLOR_WHITE);

            break;

        case MENU_FUNCTION_SOFT:

            ssd1306_WriteString("Soft Function", SSD1306_FONT_6X8, SSD1306_COLOR_WHITE);

            break;

        case MENU_FUNCTION_NORMAL:

            ssd1306_WriteString("Normal Function", SSD1306_FONT_6X8, SSD1306_COLOR_WHITE);

            break;

        case MENU_FUNCTION_STRONG:

            ssd1306_WriteString("Strong Function", SSD1306_FONT_6X8, SSD1306_COLOR_WHITE);

            break;

        case MENU_TEST_PROGRESSIVE:

            ssd1306_WriteString("Progressive Test", SSD1306_FONT_6X8, SSD1306_COLOR_WHITE);

            break;

        case MENU_TEST_SENSOR_BASED:

            ssd1306_WriteString("Sensor-Based Test", SSD1306_FONT_6X8, SSD1306_COLOR_WHITE);

            break;

    }

    ssd1306_UpdateScreen();

}



void handle_buttons(void) {

    if (button1_pressed) {

        button1_pressed = 0;

        if (buttons_held) {

            current_menu = MENU_FUNCTION;

        } else {

            switch (current_menu) {

                case MENU_FUNCTION:

                    current_menu = previous_menu;

                    break;

                case MENU_TEST:

                    current_menu = previous_menu;

                    break;

                default:

                    previous_menu = current_menu;

                    current_menu = MENU_FUNCTION;

                    break;

            }

        }

    }



    if (button2_pressed) {

        button2_pressed = 0;

        switch (current_menu) {

            case MENU_FUNCTION:

                current_menu = MENU_TEST;

                break;

            case MENU_TEST:

                current_menu = MENU_FUNCTION_SOFT;

                break;

            case MENU_FUNCTION_SOFT:

                current_menu = MENU_FUNCTION_NORMAL;

                break;

            case MENU_FUNCTION_NORMAL:

                current_menu = MENU_FUNCTION_STRONG;

                break;

            case MENU_FUNCTION_STRONG:

                current_menu = MENU_TEST_PROGRESSIVE;

                break;

            case MENU_TEST_PROGRESSIVE:

                current_menu = MENU_TEST_SENSOR_BASED;

                break;

            case MENU_TEST_SENSOR_BASED:

                current_menu = MENU_FUNCTION;

                break;

        }

    }

    display_menu(current_menu);

}

mpu6050陀螺仪初始化和数据读取功能

复制

#include <math.h>



<div style="color: #cccccc;background-color: #1f1f1f;font-family: 'Droid Sans Mono', 'monospace', monospace;font-weight: normal;font-size: 14px;line-height: 19px;white-space: pre;"><div><span style="color: #c586c0;">#define</span><span style="color: #569cd6;"> </span><span style="color: #569cd6;">MPU6050_ADDR</span><span style="color: #569cd6;">    </span><span style="color: #b5cea8;">0x68</span></div><div><span style="color: #c586c0;">#define</span><span style="color: #569cd6;"> </span><span style="color: #569cd6;">MPU6050_REG_PWR_MGMT_1</span><span style="color: #569cd6;"> </span><span style="color: #b5cea8;">0x6B</span></div><div><span style="color: #c586c0;">#define</span><span style="color: #569cd6;"> </span><span style="color: #569cd6;">MPU6050_REG_SMPLRT_DIV</span><span style="color: #569cd6;"> </span><span style="color: #b5cea8;">0x19</span></div><div><span style="color: #c586c0;">#define</span><span style="color: #569cd6;"> </span><span style="color: #569cd6;">MPU6050_REG_CONFIG</span><span style="color: #569cd6;"> </span><span style="color: #b5cea8;">0x1A</span></div><div><span style="color: #c586c0;">#define</span><span style="color: #569cd6;"> </span><span style="color: #569cd6;">MPU6050_REG_GYRO_CONFIG</span><span style="color: #569cd6;"> </span><span style="color: #b5cea8;">0x1B</span></div><div><span style="color: #c586c0;">#define</span><span style="color: #569cd6;"> </span><span style="color: #569cd6;">MPU6050_REG_ACCEL_CONFIG</span><span style="color: #569cd6;"> </span><span style="color: #b5cea8;">0x1</span></div><div><span style="color: #c586c0;">#define</span><span style="color: #569cd6;"> </span><span style="color: #569cd6;">MPU6050_REG_ACCEL_XOUT_H</span><span style="color: #569cd6;"> </span><span style="color: #b5cea8;">0x3B</span></div><div><span style="color: #c586c0;">#define</span><span style="color: #569cd6;"> </span><span style="color: #569cd6;">MPU6050_ACCEL_XOUT_L</span><span style="color: #569cd6;"> </span><span style="color: #b5cea8;">0x3</span></div><div><span style="color: #c586c0;">#define</span><span style="color: #569cd6;"> </span><span style="color: #569cd6;">MPU6050_ACCEL_YOUT_H</span><span style="color: #569cd6;"> </span><span style="color: #b5cea8;">0x3D</span></div><div><span style="color: #c586c0;">#define</span><span style="color: #569cd6;"> </span><span style="color: #569cd6;">MPU6050_ACCEL_YOUT_L</span><span style="color: #569cd6;"> </span><span style="color: #b5cea8;">0x3E</span></div><div><span style="color: #c586c0;">#define</span><span style="color: #569cd6;"> </span><span style="color: #569cd6;">MPU6050_ACCEL_ZOUT_H</span><span style="color: #569cd6;"> </span><span style="color: #b5cea8;">0x3F</span></div><div><span style="color: #c586c0;">#define</span><span style="color: #569cd6;"> </span><span style="color: #569cd6;">MPU6050_ACCEL_ZOUT_L</span><span style="color: #569cd6;"> </span><span style="color: #b5cea8;">0x40</span></div><div><span style="color: #c586c0;">#define</span><span style="color: #569cd6;"> </span><span style="color: #569cd6;">MPU6050_TEMP_OUT_H</span><span style="color: #569cd6;"> </span><span style="color: #b5cea8;">0x41</span></div><div><span style="color: #c586c0;">#define</span><span style="color: #569cd6;"> </span><span style="color: #569cd6;">MPU6050_TEMP_OUT_L</span><span style="color: #569cd6;"> </span><span style="color: #b5cea8;">0x42</span></div><div><span style="color: #c586c0;">#define</span><span style="color: #569cd6;"> </span><span style="color: #569cd6;">MPU6050_GYRO_XOUT_H</span><span style="color: #569cd6;"> </span><span style="color: #b5cea8;">0x43</span></div><div><span style="color: #c586c0;">#define</span><span style="color: #569cd6;"> </span><span style="color: #569cd6;">MPU6050_GYRO_XOUT_L</span><span style="color: #569cd6;"> </span><span style="color: #b5cea8;">0x44</span></div><div><span style="color: #c586c0;">#define</span><span style="color: #569cd6;"> </span><span style="color: #569cd6;">MPU6050_GYRO_YOUT_H</span><span style="color: #569cd6;"> </span><span style="color: #b5cea8;">0x45</span></div><div><span style="color: #c586c0;">#define</span><span style="color: #569cd6;"> </span><span style="color: #569cd6;">MPU6050_GYRO_YOUT_L</span><span style="color: #569cd6;"> </span><span style="color: #b5cea8;">0x46</span></div><div><span style="color: #c586c0;">#define</span><span style="color: #569cd6;"> </span><span style="color: #569cd6;">MPU6050_GYRO_ZOUT_H</span><span style="color: #569cd6;"> </span><span style="color: #b5cea8;">0x47</span></div><div><span style="color: #c586c0;">#define</span><span style="color: #569cd6;"> </span><span style="color: #569cd6;">MPU6050_GYRO_ZOUT_L</span><span style="color: #569cd6;"> </span><span style="color: #b5cea8;">0x48</span></div><div><span style="color: #c586c0;">#define</span><span style="color: #569cd6;"> </span><span style="color: #569cd6;">MPU6050_WHO_AM_I</span><span style="color: #569cd6;"> </span><span style="color: #b5cea8;">0x75</span></div></div>



void MPU6050_Init(void) {

    uint8_t check;

    uint8_t Data;



    // 检查设备ID

    DAL_I2C_Mem_Read(&hi2c1, MPU6050_ADDR, 0x75, 1, &check, 1, 1000);

    if (check == 0x68) {

        // 唤醒MPU6050

        Data = 0;

        DAL_I2C_Mem_Write(&hi2c1, MPU6050_ADDR, MPU6050_REG_PWR_MGMT_1, 1, &Data, 1, 1000);



        // 设置采样率

        Data = 7;

        DAL_I2C_Mem_Write(&hi2c1, MPU6050_ADDR, MPU6050_REG_SMPLRT_DIV, 1, &Data, 1, 1000);



        // 设置加速度计和陀螺仪

        Data = 0;

        DAL_I2C_Mem_Write(&hi2c1, MPU6050_ADDR, MPU6050_REG_ACCEL_CONFIG, 1, &Data, 1, 1000);

        DAL_I2C_Mem_Write(&hi2c1, MPU6050_ADDR, MPU6050_REG_GYRO_CONFIG, 1, &Data, 1, 1000);

    }

}



void MPU6050_Read_Accel(float* Ax, float* Ay, float* Az) {

    uint8_t Rec_Data[6];

    int16_t Accel_X_RAW, Accel_Y_RAW, Accel_Z_RAW;



    // 读取加速度计数据

    DAL_I2C_Mem_Read(&hi2c1, MPU6050_ADDR, MPU6050_REG_ACCEL_XOUT_H, 1, Rec_Data, 6, 1000);



    Accel_X_RAW = (int16_t)(Rec_Data[0] << 8 | Rec_Data[1]);

    Accel_Y_RAW = (int16_t)(Rec_Data[2] << 8 | Rec_Data[3]);

    Accel_Z_RAW = (int16_t)(Rec_Data[4] << 8 | Rec_Data[5]);



    *Ax = Accel_X_RAW / 16384.0;

    *Ay = Accel_Y_RAW / 16384.0;

    *Az = Accel_Z_RAW / 16384.0;

}



void MPU6050_Read_Gyro(float* Gx, float* Gy, float* Gz) {

    uint8_t Rec_Data[6];

    int16_t Gyro_X_RAW, Gyro_Y_RAW, Gyro_Z_RAW;



    // 读取陀螺仪数据

    DAL_I2C_Mem_Read(&hi2c1, MPU6050_ADDR, MPU6050_REG_GYRO_XOUT_H, 1, Rec_Data, 6, 1000);



    Gyro_X_RAW = (int16_t)(Rec_Data[0] << 8 | Rec_Data[1]);

    Gyro_Y_RAW = (int16_t)(Rec_Data[2] << 8 | Rec_Data[3]);

    Gyro_Z_RAW = (int16_t)(Rec_Data[4] << 8 | Rec_Data[5]);



    *Gx = Gyro_X_RAW / 131.0;

    *Gy = Gyro_Y_RAW / 131.0;

    *Gz = Gyro_Z_RAW / 131.0;

}



// 计算倾角

void calculate_tilt(float Ax, float Ay, float Az, float* roll, float* pitch) {

    *roll = atan2(Ay, Az) * 180.0 / M_PI;

    *pitch = atan2(-Ax, sqrt(Ay * Ay + Az * Az)) * 180.0 / M_PI;

}

压力薄膜传感器初始化和读数函数

复制

#define PRESS_MIN    20

#define PRESS_MAX    6000

#define VOLTAGE_MIN  150

#define VOLTAGE_MAX  3300



uint8_t state = 0;

uint16_t val = 0;

uint16_t value_AD = 0;



long PRESS_AO = 0;

int VOLTAGE_AO = 0;



long map(long x, long in_min, long in_max, long out_min, long out_max) {

    return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;

}



float readPressureSensor() {

    uint16_t value_AD = 0;

    uint16_t voltage_AO = 0;

   float pressure_AO = 0;



    value_AD = Get_Adc_Average(1, 10);  // 10次平均值

    voltage_AO = map(value_AD, 0, 4095, 0, 3300);



    if (voltage_AO < VOLTAGE_MIN) {

        pressure_AO = 0;

    } else if (voltage_AO > VOLTAGE_MAX) {

        pressure_AO = PRESS_MAX;

    } else {

        pressure_AO = map(voltage_AO, VOLTAGE_MIN, VOLTAGE_MAX, PRESS_MIN, PRESS_MAX);

    }



    return pressure_AO;

}

权重公式部分

通过压力的传感器和陀螺仪来自动选择振动的模式，
在原有的强力、普通、轻柔的基础上进行加强或者减弱，
比如压力传感器感受的压力增大就增加强度，直到到达上限制，比如陀螺仪感受到的角度变
化判断在脸部薄弱位置就减轻强度，陀螺仪的优先级高于压力传感器，如果在薄弱位置，
强度上限会减小，并且适当减小当前强度，如果在下颌、额头等位置就会适当增加当前强度，
通过一个权重公式，通过接收到的mpu6050的数据和压力传感器数据，并来确定当下的强度

//    压力传感器权重：PP
//    角度权重：AA
//    总强度权重：WW
//    振动强度：SS

// 权重公式逻辑

//    设定陀螺仪感应到的面部区域：例如，面部薄弱区域（脸颊）与强度较高区域（下颌、额头）。
//    陀螺仪感应到的角度范围与区域映射：
//       面部薄弱区域：角度范围A1 (比如 -45° < 角度 < 45°)
//       强度较高区域：角度范围A2 (比如角度 < -45° 或者角度 > 45°)
//    根据压力传感器和陀螺仪的权重公式调整振动强度：
//       W=P×KP+A×KAW=P×KP+A×KA
//       如果在面部薄弱区域，则总强度权重 WW 减小。
//       如果在强度较高区域，则总强度权重 WW 增加。

复制



#define KP 0.5   // 压力传感器权重

#define KA 0.5   // 角度权重

#define BASE_INTENSITY 50

#define MAX_INTENSITY 255



float calculate_vibration_intensity(float angle, float pressure) {

    float intensity;



    if (-45 <= angle && angle <= 45) {

        // 面部薄弱区域，减少强度

        intensity = (pressure * KP + angle * KA) * 0.5; // 减少强度权重

    } else {

        // 强度较高区域，增加强度

        intensity = (pressure * KP + angle * KA) * 1.5; // 增加强度权重

    }



    // 确保强度在范围内

    if (intensity > MAX_INTENSITY) {

        intensity = MAX_INTENSITY;

    } else if (intensity < BASE_INTENSITY) {

        intensity = BASE_INTENSITY;

    }

    return intensity;

}



void complementary_filter(float* roll, float* pitch, float gx, float gy, float gz, float dt) {

    static float roll_acc = 0, pitch_acc = 0;



    float roll_gyro = *roll + gx * dt;

    float pitch_gyro = *pitch + gy * dt;



    float Ax, Ay, Az;

    MPU6050_Read_Accel(&Ax, &Ay, &Az);

    calculate_tilt(Ax, Ay, Az, &roll_acc, &pitch_acc);



    *roll = ALPHA * roll_gyro + (1.0 - ALPHA) * roll_acc;

    *pitch = ALPHA * pitch_gyro + (1.0 - ALPHA) * pitch_acc;

}





// 处理接收到的数据并确定强度的函数

void process_sensor_data_and_set_motor_strength(void) {

    // 读取加速度和陀螺仪数据

    float Ax, Ay, Az;

    float Gx, Gy, Gz;

    MPU6050_Read_Accel(&Ax, &Ay, &Az);

    MPU6050_Read_Gyro(&Gx, &Gy, &Gz);



    // 计算倾角

    float roll, pitch;

    calculate_tilt(Ax, Ay, Az, &roll, &pitch);



    // 使用互补滤波器融合数据

    float dt = 0.01;  // 假设每10ms调用一次

    complementary_filter(&roll, &pitch, Gx, Gy, Gz, dt);



    // 读取压力传感器数据

    float tmp_pressure = readPressureSensor(); 



    // 计算振动马达强度

    float intensity = calculate_vibration_intensity(roll,tmp_pressure);



    // 设置振动马达的PWM占空比

    set_motor_strength((uint8_t)intensity);

}

// Main Function

复制

int main(void) {

    DAL_Init();

    SystemClock_Config();

    MX_GPIO_Init();

    MX_I2C1_Init();

    MX_I2C2_Init();

    MX_ADC1_Init();

    MX_TMR1_Init();

    MX_TMR2_Init();

    MX_USART1_UART_Init();

    ssd1306_Init();

    

    // 显示初始模式

    display_mode(current_mode);



    // 启动PWM

    DAL_TMR_PWM_Start(&hTMR1, TMR_CHANNEL_1);



    // 主循环

    while (1) {

        // 处理按键

        handle_button_press();

        

    }

}

定时器配置

复制

TMR_HandleTypeDef htmr2;



void MX_TMR2_Init(void) {

    htmr2.Instance = TMR2;

    htmr2.Init.Prescaler = 8399;

    htmr2.Init.CounterMode = TMR_COUNTERMODE_UP;

    htmr2.Init.Period = 9999;

    htmr2.Init.ClockDivision = TMR_CLOCKDIVISION_DIV1;

    htmr2.Init.AutoReloadPreload = TMR_AUTORELOAD_PRELOAD_DISABLE;

    DAL_TMR_Base_Init(&htmr2);



    DAL_TMR_Base_Start_IT(&htmr2);



    DAL_NVIC_SetPriority(TMR2_IRQn, 0, 0);

    DAL_NVIC_EnableIRQ(TMR2_IRQn);

}

// 定时器中断服务函数

float roll, pitch, gx, gy, gz, dt = 0.01;

void TMR2_IRQHandler(void) {

    if (__DAL_TMR_GET_FLAG(&htmr2, TMR_FLAG_UPDATE) != RESET) {

        if (__DAL_TMR_GET_IT_SOURCE(&htmr2, TMR_IT_UPDATE) != RESET) {

            __DAL_TMR_CLEAR_IT(&htmr2, TMR_IT_UPDATE);

process_sensor_data_and_set_motor_strength();

        }

    }

}

只看该作者 · 2024-6-22 21:10

按键IO检测是依靠中断嘛？