ROBOT基于Slam算法_Ros环境_Stm32f1遥控_Nrf无线传输_Stm32f4下位机_Jetson nano上位机_Robosence激光雷达. 008 车子底层运动搭建(基于遥感控制)

实现功能:通过摇杆基于stm32f103c8t6通过nrf24l01进行无线传输至stm32f407并进一步实时控制小车的运动(并基于oled和lcd进行实时显示小车的速度)(后续可以进一步加入视觉和语音模块,总体基于任务导向式思考完成该项目)

遥控模块(摇杆接线5v(按理讲可以,但是实际配置时其摇杆值就不再是0-4096而是0-3780)or3.3

v和oled接线3.3v)

无线传输模块(nrf24l01接线) 

发送端引脚配置:(stm32f103c8t6)

接收端引脚配置 :(开发板外设直插式)

接下来关键任务就是将其无线传输和oled以及摇杆和lcd结合到一起 

 

 成功将摇杆数据值正确接受并传输到开发板上(具体思路即通过adc接受遥感数据并用oled显示接着将其转成字符数组并进一步通过nrf24l01转到单片机上并在对应的lcd上显示)

接着需要根据接受到的数组通过逆运动学解算 (通过其摇杆两轴数据可获知其运动方向以及对应的速度,进而在套用麦克纳姆轮小车执行逆运动学解算方可知其四个轮子的角速度进而可以控制小车的运动-这里和上位机通过ros往下传数据不同,其是基于两轴实际速度而这里摇杆数据可以视作比例速度(0-1也即至最大速度,这里的两轴速度可视作基于最大转速速和初始pwm波限制下的情况))

接下来需要将pwm波程序融合到开发板的无线通信程序上并以此驱动四个电机根据其摇杆发送的数据转换成对应的比例并进一步通过pwm控制其电机的运行 

#include <stdio.h>  // 用于 sprintf
#include <math.h>
#include "./SYSTEM/sys/sys.h"
#include "./SYSTEM/usart/usart.h"
#include "./SYSTEM/delay/delay.h"
#include "./BSP/LED/led.h"
#include "./BSP/LCD/lcd.h"
#include "./BSP/KEY/key.h"
#include "./BSP/TIM_PWM/gtim.h"
#include "./BSP/NRF24L01/nrf24l01.h"

//float value_x = 0;
//float value_y = 0;
float x = 0;
float y = 0;
//int x = 0;
//int y = 0;
int X = 0;
int Y = 0;
//float w1 = 0;
//float w2 = 0;
//float w3 = 0;
//float w4 = 0;

extern TIM_HandleTypeDef g_tim1_pwm_chy_handle; /* 定时器x句柄 */
extern TIM_HandleTypeDef g_tim2_pwm_chy_handle; /* 定时器x句柄 */
extern TIM_HandleTypeDef g_tim3_pwm_chy_handle; /* 定时器x句柄 */
extern TIM_HandleTypeDef g_tim4_pwm_chy_handle; /* 定时器x句柄 */

//float x1,float y1,
void display_float(float x1,float y1,int X1,int Y1) {
    char buffer1[32];  // 用于存储转换后的字符串
    sprintf(buffer1, "%.2f", x1);  // 将浮点数转换为带两位小数的字符串
    lcd_show_string(30, 185, 200, 16, 16, buffer1,BLUE);  // 在LCD上显示转换后的字符串
    char buffer2[32];  // 用于存储转换后的字符串
    sprintf(buffer2, "%.2f", y1);  // 将浮点数转换为带两位小数的字符串
    lcd_show_string(30, 200, 200, 16, 16, buffer2,BLUE);  // 在LCD上显示转换后的字符串
//    char buffer3[32];  // 用于存储转换后的字符串
//    sprintf(buffer3, "%d", X1);  // 将浮点数转换为带两位小数的字符串
//    lcd_show_string(30, 215, 200, 16, 16, buffer3,BLUE);  // 在LCD上显示转换后的字符串
//    char buffer4[32];  // 用于存储转换后的字符串
//    sprintf(buffer4, "%d", Y1);  // 将浮点数转换为带两位小数的字符串
//    lcd_show_string(30, 230, 200, 16, 16, buffer4,BLUE);  // 在LCD上显示转换后的字符串
    char buffer3[32];  // 用于存储转换后的字符串
    sprintf(buffer3, "%.1f", (float)X1);  // 将浮点数转换为带两位小数的字符串
    lcd_show_string(30, 215, 200, 16, 16, buffer3,BLUE);  // 在LCD上显示转换后的字符串
    char buffer4[32];  // 用于存储转换后的字符串
    sprintf(buffer4, "%.1f", (float)Y1);  // 将浮点数转换为带两位小数的字符串
    lcd_show_string(30, 230, 200, 16, 16, buffer4,BLUE);  // 在LCD上显示转换后的字符串
}

union Transfer
{
    /*定义union,方便进行
     整型和浮点型的转换*/
    float   Float_data;//stm32中float占4个字节32位
    int Byte[4];   //4个uint8_t类型元素,占32位
}transunion,*transptr=&transunion;

/*******************************************************************************
* Function Name  : Float_To_Int
* Description    : 浮点数转化为整形
* Parameters     : 浮点数
* Return         : 32位整型
* Notice         : None
*******************************************************************************/
int Float_To_Int(float data)
{
        int Flash_uInt=0;
    /*将float的数据分别按字节存放在地址内*/
        transptr->Byte[0]=((uint8_t *)&data)[0]; 
        transptr->Byte[1]=((uint8_t *)&data)[1];
        transptr->Byte[2]=((uint8_t *)&data)[2];
        transptr->Byte[3]=((uint8_t *)&data)[3];
        Flash_uInt=*(int*)transptr->Byte;//用uint32_t类型读取该32位地址的数据
        return Flash_uInt;
}

/*******************************************************************************
* Function Name  : Int_To_Float
* Description    : 整形转化为浮点数
* Parameters     : 32位整形
* Return         : 浮点数
* Notice         : None
*******************************************************************************/
float Int_To_Float(uint32_t data)
{
        double Flash_flt=0.0;
        //分解32位整形为4个8位整形
        transptr->Byte[0]=data&0xff;
        transptr->Byte[1]=(data>>8)&0xff;
        transptr->Byte[2]=(data>>16)&0xff;
        transptr->Byte[3]=(data>>24)&0xff;
        Flash_flt=transptr->Float_data;//利用float类型读取出该32位地址的数据
        return Flash_flt;
}

int main(void)
{
//    uint16_t ledrpwmval = 0;
//    uint8_t dir = 1;
    uint8_t key, mode;
    uint16_t t = 0;
    uint8_t tmp_buf[33];
    
    HAL_Init();                         /* 初始化HAL库 */
    sys_stm32_clock_init(336, 8, 2, 7); /* 设置时钟, 168Mhz */
    delay_init(168);                    /* 延时初始化 */
    usart_init(115200);                 /* 串口初始化为115200 */
    led_init();                         /* 初始化LED */
    lcd_init();                         /* 初始化LCD */
    key_init();                         /* 初始化按键 */
    nrf24l01_init();                    /* 初始化NRF24L01 */
    //通过四路pwm波驱动四路电机
    gtim_tim1_pwm_chy_init(500 - 1,168 - 1);    /* 168 000 000 / 168 = 1 000 000 1Mhz的计数频率,2Khz的PWM */
    gtim_tim2_pwm_chy_init(500 - 1,168 - 1);    /* 168 000 000 / 168 = 1 000 000 1Mhz的计数频率,2Khz的PWM */
    gtim_tim3_pwm_chy_init(500 - 1,168 - 1);    /* 168 000 000 / 168 = 1 000 000 1Mhz的计数频率,2Khz的PWM */
    gtim_tim4_pwm_chy_init(500 - 1,168 - 1);    /* 168 000 000 / 168 = 1 000 000 1Mhz的计数频率,2Khz的PWM */
    
    lcd_show_string(30, 50, 200, 16, 16, "STM32", RED);
    lcd_show_string(30, 70, 200, 16, 16, "NRF24L01 TEST", RED);
    lcd_show_string(30, 90, 200, 16, 16, "ATOM@ALIENTEK", RED);
    
    GPIO_InitTypeDef  gpio_init_struct;
    __HAL_RCC_GPIOF_CLK_ENABLE();
    //由于PF8、9、10均和现有硬件资源冲突故8、9换到12、13
    gpio_init_struct.Pin = GPIO_PIN_2;                   /* 引脚 */
    gpio_init_struct.Mode = GPIO_MODE_OUTPUT_PP;            /* 推挽输出 */
    gpio_init_struct.Pull = GPIO_PULLUP;                    /* 上拉 */
    gpio_init_struct.Speed = GPIO_SPEED_FREQ_HIGH;          /* 高速 */
    HAL_GPIO_Init(GPIOF, &gpio_init_struct);       /* 初始化LED0引脚 */

    gpio_init_struct.Pin = GPIO_PIN_3;                   /* 引脚 */
    HAL_GPIO_Init(GPIOF, &gpio_init_struct);       /* 初始化引脚 */

    gpio_init_struct.Pin = GPIO_PIN_4;                   /* 引脚 */
    HAL_GPIO_Init(GPIOF, &gpio_init_struct);       /* 初始化引脚 */
    
    gpio_init_struct.Pin = GPIO_PIN_5;                   /* 引脚 */
    HAL_GPIO_Init(GPIOF, &gpio_init_struct);       /* 初始化引脚 */
    
    gpio_init_struct.Pin = GPIO_PIN_6;                   /* 引脚 */
    HAL_GPIO_Init(GPIOF, &gpio_init_struct);       /* 初始化引脚 */

    gpio_init_struct.Pin = GPIO_PIN_7;                   /* 引脚 */
    HAL_GPIO_Init(GPIOF, &gpio_init_struct);       /* 初始化引脚 */
    
    gpio_init_struct.Pin = GPIO_PIN_8;                   /* 引脚 */
    HAL_GPIO_Init(GPIOB, &gpio_init_struct);       /* 初始化引脚 */
    
    gpio_init_struct.Pin = GPIO_PIN_9;                   /* 引脚 */
    HAL_GPIO_Init(GPIOB, &gpio_init_struct);       /* 初始化引脚 */
    
    HAL_GPIO_WritePin(GPIOF, GPIO_PIN_2, GPIO_PIN_RESET);
    HAL_GPIO_WritePin(GPIOF, GPIO_PIN_3, GPIO_PIN_RESET);
    HAL_GPIO_WritePin(GPIOF, GPIO_PIN_4, GPIO_PIN_RESET);
    HAL_GPIO_WritePin(GPIOF, GPIO_PIN_5, GPIO_PIN_RESET);
    HAL_GPIO_WritePin(GPIOF, GPIO_PIN_6, GPIO_PIN_RESET);
    HAL_GPIO_WritePin(GPIOF, GPIO_PIN_7, GPIO_PIN_RESET);
    HAL_GPIO_WritePin(GPIOB, GPIO_PIN_8, GPIO_PIN_RESET);
    HAL_GPIO_WritePin(GPIOB, GPIO_PIN_9, GPIO_PIN_RESET);
    while (nrf24l01_check()) /* 检查NRF24L01是否在线 */
    {
        lcd_show_string(30, 110, 200, 16, 16, "NRF24L01 Error", RED);
        delay_ms(200);
        lcd_fill(30, 110, 239, 130 + 16, WHITE);
        delay_ms(200);
    }

    lcd_show_string(30, 110, 200, 16, 16, "NRF24L01 OK", RED);

    while (1)           /* 提醒用户选择模式 */
    {
        key = key_scan(0);
        if (key == KEY0_PRES)
        {
            mode = 0;   /* 接收模式 */
            break;
        }
        else if (key == KEY1_PRES)
        {
            mode = 1;   /* 发送模式 */
            break;
        }

        t++;
        if (t == 100)   /* 显示提示信息 */
        {
            lcd_show_string(10, 130, 230, 16, 16, "KEY0:RX_Mode  KEY1:TX_Mode", RED);
        }

        if (t == 200)   /* 关闭提示信息 */
        {
            lcd_fill(10, 130, 230, 150 + 16, WHITE);
            t = 0;
        }

        delay_ms(5);
    }

    lcd_fill(10, 130, 240, 166, WHITE); /* 清空上面的显示 */

    if (mode == 0)          /* RX模式 */
    {
        lcd_show_string(30, 130, 200, 16, 16, "NRF24L01 RX_Mode", BLUE);
        lcd_show_string(30, 150, 200, 16, 16, "Received DATA:", BLUE);
        nrf24l01_rx_mode(); /* 进入RX模式 */

        //程序编写关键代码、无线通信、遥感数据值逆动力学解算、pwm驱动电机
        while (1)
        {
            if (nrf24l01_rx_packet(tmp_buf) == 0) /* 一旦接收到信息,则显示出来. */
            {
                tmp_buf[32] = 0;    /* 加入字符串结束符 */
                lcd_show_string(30, 170, lcddev.width - 1, 32, 16, (char *)tmp_buf, BLUE);
            }
            //else
            //{
                //delay_us(100);
            //}
            x = (float)(tmp_buf[0]-48)*1000+(float)(tmp_buf[1]-48)*100+(float)(tmp_buf[2]-48)*10+(float)(tmp_buf[3]-48);
            y = (float)(tmp_buf[5]-48)*1000+(float)(tmp_buf[6]-48)*100+(float)(tmp_buf[7]-48)*10+(float)(tmp_buf[8]-48);
//            x = (int)(tmp_buf[0]-48)*1000+(int)(tmp_buf[1]-48)*100+(int)(tmp_buf[2]-48)*10+(int)(tmp_buf[3]-48);
//            y = (int)(tmp_buf[5]-48)*1000+(int)(tmp_buf[6]-48)*100+(int)(tmp_buf[7]-48)*10+(int)(tmp_buf[8]-48);
            //            x = value_x;
//            y = value_y;            
            //规范在几何约束最大值但不考虑其长方形值域特点
            x = (x-1930)*400/(1930*2);//[-1/2,+1/2]
            y = (1880-y)*400/(1880*2);//[-1/2,+1/2]
//            X = x;
//            Y = y;
            //关键bug点。。。
            //。。。。。。。。
            //。。。。。。。。
//            X = (int)(round(x));//X+Y对应2、4路
//            Y = (int)(round(y));//Y-X对应1、3路
            X = (int)(x);//X+Y对应2、4路
            Y = (int)(y);//Y-X对应1、3路
            //X = Float_To_Int(x);
            //Y = Float_To_Int(y);
//            X = (int)(round(x*200));//X+Y对应2、4路
//            Y = (int)(round(y*200));//Y-X对应1、3路
            display_float(x,y,X,Y);  // 显示浮点数
            //2、3控制第一轮;3、4控制第二轮;4、5控制第三轮;5、6控制第四轮
//            HAL_GPIO_WritePin(GPIOF, GPIO_PIN_2, GPIO_PIN_SET);//1
//            HAL_GPIO_WritePin(GPIOF, GPIO_PIN_3, GPIO_PIN_RESET);//0
//            __HAL_TIM_SET_COMPARE(&g_tim1_pwm_chy_handle, GTIM_TIM1_PWM_CHY, 150);
//            if (dir)ledrpwmval++;                   /* dir==1 ledrpwmval递增 */
//            else ledrpwmval--;                      /* dir==0 ledrpwmval递减 */

//            if (ledrpwmval > 3000)dir = 0;           /* ledrpwmval到达300后,方向为递减 */
//            if (ledrpwmval == 0)dir = 1;            /* ledrpwmval递减到0后,方向改为递增 */

//            /* 修改比较值控制占空比 */
//            __HAL_TIM_SET_COMPARE(&g_tim1_pwm_chy_handle, GTIM_TIM1_PWM_CHY, ledrpwmval/10);
//            if(X>=0){
//                HAL_GPIO_WritePin(GPIOF, GPIO_PIN_2, GPIO_PIN_SET);//1
//                HAL_GPIO_WritePin(GPIOF, GPIO_PIN_3, GPIO_PIN_RESET);//0
//                __HAL_TIM_SET_COMPARE(&g_tim1_pwm_chy_handle, GTIM_TIM1_PWM_CHY, X);
//            }else{
//                HAL_GPIO_WritePin(GPIOF, GPIO_PIN_2, GPIO_PIN_RESET);//0
//                HAL_GPIO_WritePin(GPIOF, GPIO_PIN_3, GPIO_PIN_SET);//1
//                __HAL_TIM_SET_COMPARE(&g_tim1_pwm_chy_handle, GTIM_TIM1_PWM_CHY, X*(-1));
//            }
            //涉及1、3轮
            if((Y-X)>=0){
                //对应1、3轮正向顺时针转动(对应引脚PF2、PF3、PF6、PF7)
                //1轮
                HAL_GPIO_WritePin(GPIOF, GPIO_PIN_2, GPIO_PIN_SET);//1
                HAL_GPIO_WritePin(GPIOF, GPIO_PIN_3, GPIO_PIN_RESET);//0
                //3轮
                HAL_GPIO_WritePin(GPIOF, GPIO_PIN_6, GPIO_PIN_SET);//1
                HAL_GPIO_WritePin(GPIOF, GPIO_PIN_7, GPIO_PIN_RESET);//0
                //根据遥感数据实时调整其占空比进而调整其转速
                __HAL_TIM_SET_COMPARE(&g_tim1_pwm_chy_handle, GTIM_TIM1_PWM_CHY, Y-X);
                __HAL_TIM_SET_COMPARE(&g_tim3_pwm_chy_handle, GTIM_TIM3_PWM_CHY, Y-X);
            }else{
                //对应1、3轮负向逆时针转动(对应引脚PF2、PF3、PF6、PF7)
                //1轮
                HAL_GPIO_WritePin(GPIOF, GPIO_PIN_2, GPIO_PIN_RESET);//0
                HAL_GPIO_WritePin(GPIOF, GPIO_PIN_3, GPIO_PIN_SET);//1
                //3轮
                HAL_GPIO_WritePin(GPIOF, GPIO_PIN_6, GPIO_PIN_RESET);//0
                HAL_GPIO_WritePin(GPIOF, GPIO_PIN_7, GPIO_PIN_SET);//1
                //根据遥感数据实时调整其占空比进而调整其转速
                __HAL_TIM_SET_COMPARE(&g_tim1_pwm_chy_handle, GTIM_TIM1_PWM_CHY, X-Y);
                __HAL_TIM_SET_COMPARE(&g_tim3_pwm_chy_handle, GTIM_TIM3_PWM_CHY, X-Y);
            }
            
            //涉及2、4轮
            if((X+Y)>=0){
                //对应2、4轮正向顺时针转动(对应引脚PF4、PF5、PF8、PF9)
                //2轮
                HAL_GPIO_WritePin(GPIOF, GPIO_PIN_4, GPIO_PIN_SET);//1
                HAL_GPIO_WritePin(GPIOF, GPIO_PIN_5, GPIO_PIN_RESET);//0
                //4轮
                HAL_GPIO_WritePin(GPIOB, GPIO_PIN_8, GPIO_PIN_SET);//1
                HAL_GPIO_WritePin(GPIOB, GPIO_PIN_9, GPIO_PIN_RESET);//0
                //根据遥感数据实时调整其占空比进而调整其转速
                __HAL_TIM_SET_COMPARE(&g_tim2_pwm_chy_handle, GTIM_TIM2_PWM_CHY, X+Y);
                __HAL_TIM_SET_COMPARE(&g_tim4_pwm_chy_handle, GTIM_TIM4_PWM_CHY, X+Y);
            }else{
                //对应2、4轮负向逆时针针转动(对应引脚PF4、PF5、PF8、PF9)
                //2轮
                HAL_GPIO_WritePin(GPIOF, GPIO_PIN_4, GPIO_PIN_RESET);//0
                HAL_GPIO_WritePin(GPIOF, GPIO_PIN_5, GPIO_PIN_SET);//1
                //4轮
                HAL_GPIO_WritePin(GPIOB, GPIO_PIN_8, GPIO_PIN_RESET);//0
                HAL_GPIO_WritePin(GPIOB, GPIO_PIN_9, GPIO_PIN_SET);//1
                //根据遥感数据实时调整其占空比进而调整其转速
                __HAL_TIM_SET_COMPARE(&g_tim2_pwm_chy_handle, GTIM_TIM2_PWM_CHY, (X+Y)*(-1));
                __HAL_TIM_SET_COMPARE(&g_tim4_pwm_chy_handle, GTIM_TIM4_PWM_CHY, (X+Y)*(-1));
            }
//            /* 修改比较值控制占空比 */
//            __HAL_TIM_SET_COMPARE(&g_tim1_pwm_chy_handle, GTIM_TIM1_PWM_CHY, Y-X);
//            /* 修改比较值控制占空比 */
//            __HAL_TIM_SET_COMPARE(&g_tim2_pwm_chy_handle, GTIM_TIM2_PWM_CHY, X+Y);
//            /* 修改比较值控制占空比 */
//            __HAL_TIM_SET_COMPARE(&g_tim3_pwm_chy_handle, GTIM_TIM3_PWM_CHY, Y-X);
//            /* 修改比较值控制占空比 */
//            __HAL_TIM_SET_COMPARE(&g_tim4_pwm_chy_handle, GTIM_TIM4_PWM_CHY, X+Y);
            //程序正常执行标志
            t++;
            if (t == 1000)         /* 大约1s钟改变一次状态 */
            {
                t = 0;
                LED0_TOGGLE();
            }
        }
    }
    else /* TX模式 */
    {
        lcd_show_string(30, 130, 200, 16, 16, "NRF24L01 TX_Mode", BLUE);
        nrf24l01_tx_mode(); /* 进入TX模式 */
        mode = ' ';         /* 从空格键开始发送 */

        while (1)
        {
            if (nrf24l01_tx_packet(tmp_buf) == 0) /* 发送成功 */
            {
                lcd_show_string(30, 150, 239, 32, 16, "Sended DATA:", BLUE);
                lcd_show_string(0, 170, lcddev.width - 1, 32, 16, (char *)tmp_buf, BLUE);
                key = mode;

                for (t = 0; t < 32; t++)
                {
                    key++;

                    if (key > ('~'))
                        key = ' ';

                    tmp_buf[t] = key;
                }

                mode++;
                if (mode > '~')
                {
                    mode = ' ';
                }

                tmp_buf[32] = 0; /* 加入结束符 */
            }
            else
            {
                lcd_fill(0, 150, lcddev.width, 170 + 16 * 3, WHITE); /* 清空显示 */
                lcd_show_string(30, 150, lcddev.width - 1, 32, 16, "Send Failed ", BLUE);
            }

            LED0_TOGGLE();
            delay_ms(200);
        }
    }
}

有待进一步优化。。。

评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值