1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
|
#include "path_ctrl_t.h"
#include <errno.h>
#include <fcntl.h>
#include <stdbool.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <termios.h>
#include <unistd.h>
typedef unsigned char byte;
#define MAX_BUFFER_SIZE 1024
#define DEFAULT_SPEED 0x15
int fd; // 轮子的串口文件描述符
char portname[50] = "/dev/ttyUSB0"; // 串口设备名
char clientIP[20] = "";
struct termios tty;
bool whellInit();
bool wheelSend(byte a, byte a_v, byte b, byte b_v);
void speedControl(byte status, byte speed);
void parseCmd(const lcm_recv_buf_t *rbuf, const char *channel,
const path_ctrl_t *msg, void *userdata);
int main() {
if (!whellInit()) {
goto err;
}
lcm_t *path_lcm = lcm_create(NULL);
if (!path_lcm) {
fprintf(stderr, "Failed to create LCM\n");
goto err;
}
path_ctrl_t cmd;
path_ctrl_t_subscribe(path_lcm, "wheel_ctrl", parseCmd, &cmd);
while (true) {
lcm_handle(path_lcm);
}
err:
close(fd);
return 0;
}
bool whellInit() {
// 打开串口
fd = open(portname, O_RDWR | O_NOCTTY | O_SYNC);
if (fd < 0) {
fprintf(stderr, "Error opening %s: %s\n", portname, strerror(errno));
return false;
}
// 设置串口参数
memset(&tty, 0, sizeof tty);
if (tcgetattr(fd, &tty) != 0) {
fprintf(stderr, "Error from tcgetattr: %s\n", strerror(errno));
return false;
}
cfsetospeed(&tty, B115200); // 设置输出波特率为115200
cfsetispeed(&tty, B115200); // 设置输入波特率为115200
tty.c_cflag = (tty.c_cflag & ~CSIZE) | CS8; // 8-bit chars
tty.c_iflag &= ~IGNBRK; // ignore break signal
tty.c_lflag = 0; // no signaling chars, no echo,
// no canonical processing
tty.c_oflag = 0; // no remapping, no delays
tty.c_cc[VMIN] = 0; // read doesn't block
tty.c_cc[VTIME] = 5; // 0.5 seconds read timeout
tty.c_iflag &= ~(IXON | IXOFF | IXANY); // shut off xon/xoff ctrl
tty.c_cflag |= (CLOCAL | CREAD); // ignore modem controls,
// enable reading
tty.c_cflag &= ~(PARENB | PARODD); // shut off parity
tty.c_cflag |= 0;
tty.c_cflag &= ~CSTOPB;
tty.c_cflag &= ~CRTSCTS;
if (tcsetattr(fd, TCSANOW, &tty) != 0) {
fprintf(stderr, "Error from tcsetattr: %s\n", strerror(errno));
return false;
}
return true;
}
bool wheelSend(byte a, byte a_v, byte b, byte b_v) {
unsigned char data[7] = {0x53, 0x05, 0x00, 0x00, 0x00, 0x00, 0x00};
byte checksum = 0;
data[2] = a;
data[3] = a_v;
data[4] = b;
data[5] = b_v;
for (int i = 0; i < 6; i++) {
checksum ^= data[i];
}
data[6] = checksum;
// 发送数据
if (write(fd, data, 7) != 7) {
fprintf(stderr, "Failed to write to the serial port\n");
return false;
}
printf("Data %02X %02X %02X %02X %02X %02X %02X wheelSend successfully!\n",
data[0], data[1], data[2], data[3], data[4], data[5], data[6]);
return true;
}
void parseCmd(const lcm_recv_buf_t *rbuf, const char *channel,
const path_ctrl_t *msg, void *userdata) {
byte status = msg->cmd, speed = msg->speed;
switch (status) {
case 1:
wheelSend(0x01, speed, 0x01, speed);
break;
case 2:
wheelSend(0x02, DEFAULT_SPEED, 0x01, DEFAULT_SPEED);
break;
case 3:
wheelSend(0x01, DEFAULT_SPEED, 0x02, DEFAULT_SPEED);
break;
case 0:
default:
wheelSend(0x00, 0x00, 0x00, 0x00);
break;
}
}
|
