键盘控制ROS车运动

键盘控制ROS车运动

上位机

使用pyseria库与stm32单片机进行通信控制

#!/usr/bin/env python
# -*- coding: utf-8 -*import sys, select, termios, tty
import serialmsg = """
---------------------------w     a    x    ds     w : +x    a : +y
s : -x    d : -y 
x : stop其他按键 : stopq/z : 线性速度增加/减少1RPM
e/c : 只增加/减少角速度1RPMCTRL-C 退出
---------------------------
"""moveBindings = {
'w':'w',
's':'s',
'a':'a',
'd':'d',
'x':'x',
'q':'q',
'z':'z',
'e':'e',
'c':'c'}def getKey():tty.setraw(sys.stdin.fileno())select.select([sys.stdin], [], [], 0)key = sys.stdin.read(1)termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)return keyif __name__=="__main__":settings = termios.tcgetattr(sys.stdin)port = '/dev/ttyUSB1'  # 串口号baud = 115200  # 波特率ser = serial.Serial(port, baud, timeout=0.5)flag = ser.isOpen()try:print(msg)print(port, "串口打开成功")if flag:while(1):key = getKey()if key in moveBindings.keys():data = moveBindings[key]ser.write(data.encode("gbk"))# print('串口发送数据:  ', data)else:if (key == '\x03'):breakexcept Exception as e:print(e)print("串口打开异常")

下位机

下位机采用USART1进行通信，轮毂电机的控制参考上篇文章STM32HAL库RS485-ModBus协议控制伺服电机

uint16_t data[2] = {0, 0};
uint16_t data1[2] = {-16, 16};
uint16_t data2[2] = {6, 6};
uint16_t data3[2] = {0, 0};
uint8_t RxData;int main(void)
{/* USER CODE BEGIN 1 */uint16_t speed_flag, turn_flag;uint16_t speed = 10;uint16_t turn = 10;speed_flag = turn_flag = 1;/* USER CODE END 1 *///....初始化省略...../* USER CODE BEGIN 2 */HAL_UART_Receive_IT(&huart1, &RxData,1);//RS485发送模式HAL_GPIO_WritePin(GPIOD, GPIO_PIN_7, GPIO_PIN_SET);//电机初始化指令MBRTUMasterWriteSingleRegister(&MBRTUHandle, 1, 0X200D, 0x0003, 100);HAL_Delay(10);MBRTUMasterWriteSingleRegister(&MBRTUHandle, 1, 0X200E, 0x0008, 100);HAL_Delay(10);MBRTUMasterWriteSingleRegister(&MBRTUHandle, 1, 0X200E, 0x0010, 100);/* USER CODE END 2 *//* Infinite loop *//* USER CODE BEGIN WHILE */while (1){/* USER CODE END WHILE *//* USER CODE BEGIN 3 */HAL_GPIO_WritePin(GPIOD, GPIO_PIN_7, GPIO_PIN_SET);if(RxData == 'w'||RxData == 's'||RxData == 'a'||RxData == 'd'||RxData == 'x'||RxData == 'e'||RxData == 'c'||RxData == 'q'||RxData == 'z'){if(RxData == 'w' || RxData == 's'||RxData == 'q'||RxData == 'z'){if(RxData == 'w') speed_flag = 1;if(RxData == 's') speed_flag = -1;if(RxData == 'q') speed+=10;if(RxData == 'z') speed-=10;data[0] = speed_flag * speed;data[1] = -speed_flag * speed;}if(RxData == 'a'||RxData == 'd'||RxData == 'e'||RxData == 'c'){if (RxData == 'a') turn_flag = 1;if (RxData == 'd') turn_flag = -1;if(RxData == 'e') turn+=10;if(RxData == 'c') turn-=10;data[0] = turn_flag * turn;data[1] = turn_flag * turn;}if(RxData == 'x'){data[0] = 0;data[1] = 0;}//0x10指令便于给多寄存器发送指令，电机同时启动MBRTUMasterWriteMultipleRegisters(&MBRTUHandle, 1, 0X2088, 0X0002, data, 100);}RxData = ' ';}

加入按键外部中断控制，进行测试或急停

void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
{if(GPIO_Pin==WK_UP_EXTI_Pin){HAL_Delay(10);if(HAL_GPIO_ReadPin(WK_UP_EXTI_GPIO_Port, WK_UP_EXTI_Pin) == 1){MBRTUMasterWriteMultipleRegisters(&MBRTUHandle, 1, 0X2088, 0X0002, data3, 100);}}else if(GPIO_Pin==KEY0_EXTI_Pin){HAL_Delay(10);if(HAL_GPIO_ReadPin(KEY0_EXTI_GPIO_Port, KEY0_EXTI_Pin) == 0){
//            MBRTUMasterWriteMultipleRegisters(&MBRTUHandle, 1, 0X2088, 0X0002, data1, 100);HAL_GPIO_WritePin(GPIOD, GPIO_PIN_7, GPIO_PIN_RESET);MBRTUMasterReadHoldingRegisters(&MBRTUHandle, 0x01, 0x20AB, 0x0002, 100);HAL_GPIO_TogglePin(GPIOE, GPIO_PIN_5);}}else if(GPIO_Pin==KEY1_EXTI_Pin){HAL_Delay(10);if(HAL_GPIO_ReadPin(KEY1_EXTI_GPIO_Port, KEY1_EXTI_Pin) == 0){MBRTUMasterWriteMultipleRegisters(&MBRTUHandle, 1, 0X2088, 0X0002, data2, 100);}}
}

运行结果

赋予串口权限

sudo chmod 777 /dev/ttyUSB*
python3 keyboard_control.py 

演示视频
键盘控制节点编写