单个摇杆加nRF24L01P控制双轮智能小车的方法
一般控制双轮智能小车用两个摇杆(joystick)会比较直观，写程序也简单：只需要读取摇杆的两个前进和后退值然后发给智能小车就可以。但是如果想只用一个摇杆来控制小车前进、后退、左右转变、原地旋转等，就会比较麻烦，需要写一套专门的控制逻辑。
本文使用的摇杆是能买到的普通的arduino摇杆模块。
读取它数据的基本arduino程序如下：
const static uint8_t PIN_X = A1;
const static uint8_t PIN_Y = A2;
const static uint8_t PIN_Z = A3;
void setup(){
Serial.begin(9600);
pinMode(PIN_X, INPUT);
pinMode(PIN_Y, INPUT);
pinMode(PIN_Z, INPUT_PULLUP);
}
void loop() {
long x = analogRead(PIN_X);
long y = analogRead(PIN_Y);
long z = digitalRead(PIN_Z);
Serial.print(x);
Serial.print(" ");
Serial.print(y);
Serial.print(" ");
Serial.print(z);
Serial.print("\n");
delay(10);
}
连线方法略。
向arduino烧写完程序后，通过打开串口监视器，可以观察到这个模块的X、Y两个轴的量程为[0, 1024)范围内的整数。摇杆回正的值一在512附近，不严格等于512。按下摇杆帽SW也就是Z轴后其值为0.本文后续不再考虑Z的值。
将X-Y的值通过减去512，可以换算成 x取[512, 512)，y取[-512,512)的标准笛卡尔坐标。x, y的值可以分为3种情况，大于0，小于0和等于0的情况。考虑到摇杆回正的值经过换算后不一定等于0，所以我们需要设定一个死区，当 abs(x) 或 abs(y) 小于这个值，就直接视为0。
const int deadSection = 12;
void reasd_data() {
int x = analogRead(PIN_X);
int y = analogRead(PIN_Y);
x -= 512;
y -= 512;
// 死区
x = abs(x) < deadSection ? 0 : x;
y = abs(y) < deadSection ? 0 : y;
}
x、y的3种取值情况可以组合成9种情况，这9种又可以分为4大类
- x、y都为0，两轮都停止
- y为0，x不为0，则代表原地旋转，这时两轮速度相同，方向相反
- x为0，y不为0，则代表前进或后退，这时两轮速度相同，方向相同
- x不为0, y也不为0，这时就需要具体求出两轮的速度
以上4类中前3类情况非常简单直观，不作考虑，直接分析第4种。
第4种就是摇杆运动的点不在坐标轴上的情况，分布于四个象限，每个象限情况都类似，因些只以第一象限为例。
设在第一象限摇杆运动到坐标P(x,y)，这时又分为两个情况：
- x<=y，OP与直线y=512交于点A
- x>y， OP与直线x=512相交于某点
这两种情况类似，因此只考虑第1种情况。
对于这种情况我们首先要定出两轮的最大运行速度，类比x轴或y轴，它们的速度区间是[0,512]，如果点在轴上，则最大速度就是相应轴上的坐标值。例如(x, 0)两轮中最大速度是x；(0, y)两轮最大速度为y。
所以定义P点的最大速度为：(OA长度/512)*OP长度。
分别过P点和A点垂直x轴作辅助线，可以看出它们两个是相似三角形，因此 y/512=OP/OA。
如果OA最大值也是被定义成512，那OP就为y。
因此P点的最大速度为y。
更详细的推导如下：
考虑 x <= y 的情况，设 (0, 0) 与 (x, y)的长度为L，其延长线必然交于直线y=512
设原点到交点的长度为M， 则方程为 y/512 = L / M
求得 M = 512 *L / y
将这个线段分成512份（像x,y轴那样分512个阶梯），则每一份为 M/512 = L/y
Vmax分配L对应的速度
所以Vmax = L / (M/512) = y
在求得P点的两轮中一轮的最大速度Vmax后，需要求出另一轮的Vmin速度。
通过分析象限图可以轻松看出两轮的速度分配比例是通过P点在x轴上分割的点来确定的。
对于这种情况 Vmax/Vmin = (512+x)/(512-x)，得出 Vmin = Vmax*(512-x)/(512+x)
因此可以直接写出求两轮中速度Vmax和Vmin的函数
void get_speed(int x, int y, int8_t *Vmax, int8_t *Vmin) {
x = abs(x);
y = abs(y);
// 这里必需用long定义，否则会计算溢出
// 因为arduino的int是16位
long max_v = x > y ? x : y;
long min_v = max_v*(512-x)/(512+x);
// 将速度映射到[0, 127]范围内
*Vmax = map(max_v, 0, 512, 0, 127);
*Vmin = map(min_v, 0, 512, 0, 127);
}
接下来就是决策各种情况下两轮谁取最大值了，这一点比较简单，只要点落在x > 0区域就左侧轮取Vmax，反之则右侧轮取Vmax。
结合以上分析可以写出全部逻辑代码了，其中nRF24L01P的库用的是arduino的RF24，地址：https://tmrh20.github.io/RF24 可以在arduino IDE的库管理器中下载到。
#include
#include "RF24.h"
#include
const static uint8_t PIN_RADIO_CE = 7;
const static uint8_t PIN_RADIO_CSN = 8;
const static uint8_t PIN_X = A1;
const static uint8_t PIN_Y = A2;
const static uint8_t PIN_Z = A3;
const static uint8_t RF_CH = 64;
RF24 radio(PIN_RADIO_CE, PIN_RADIO_CSN);
// 小车nRF24L01P的接收地址
uint8_t address[] = { 0x1, 0x2, 0x3, 0x4, 0x5 };
const int deadSection = 12;
enum {
FORWARD = 1,
STOP = 0,
BACKWARD = -1
};
typedef struct {
int8_t vl;
int8_t vr;
int8_t sw;
} cmd_t;
cmd_t cmd;
void setup(){
Serial.begin(9600);
pinMode(PIN_X, INPUT);
pinMode(PIN_Y, INPUT);
pinMode(PIN_Z, INPUT_PULLUP);
radio.begin();
radio.setPALevel(RF24_PA_MAX);
radio.setDataRate(RF24_2MBPS);
radio.setChannel(RF_CH);
radio.enableDynamicPayloads();
radio.setAddressWidth(5);
radio.openWritingPipe(address);
radio.maskIRQ(1,1,1);
radio.setAutoAck(false);
radio.setCRCLength(RF24_CRC_8);
radio.stopListening();
}
void get_speed(int x, int y, int8_t *Vmax, int8_t *Vmin) {
x = abs(x);
y = abs(y);
// 这里必需用long定义，否则会计算溢出
// 因为arduino的int是16位
long max_v = x > y ? x : y;
long min_v = max_v*(512-x)/(512+x);
// 原地旋转
min_v = y != 0 ? min_v : max_v;
// 同速前进或后退
min_v = x != 0 ? min_v : max_v;
// 将速度映射到[0, 127]范围内
*Vmax = map(max_v, 0, 512, 0, 127);
*Vmin = map(min_v, 0, 512, 0, 127);
}
void read_data(int *xpos, int *ypos) {
*xpos = analogRead(PIN_X);
*ypos = analogRead(PIN_Y);
int z = digitalRead(PIN_Z);
*xpos -= 512;
*ypos -= 512;
cmd.sw = z == 0 ? 1 : 0;
// 死区
int x = abs(*xpos) < deadSection ? 0 : *xpos;
int y = abs(*ypos) < deadSection ? 0 : *ypos;
if(x == 0) {
get_speed(x, y, &cmd.vl, &cmd.vr);
if(y == 0) {
cmd.vl *= STOP;
cmd.vr *= STOP;
}
if(y > 0) {
cmd.vl *= FORWARD;
cmd.vr *= FORWARD;
}
if(y < 0) {
cmd.vl *= BACKWARD;
cmd.vr *= BACKWARD;
}
}
if(x > 0) {
// 左侧轮取较大的速度
get_speed(x, y, &cmd.vl, &cmd.vr);
if(y == 0) {
cmd.vl *= FORWARD;
cmd.vr *= BACKWARD;
}
if(y > 0) {
cmd.vl *= FORWARD;
cmd.vr *= FORWARD;
}
if(y < 0) {
cmd.vl *= BACKWARD;
cmd.vr *= BACKWARD;
}
}
if(x < 0) {
// 右侧轮取较大的速度
get_speed(x, y, &cmd.vr, &cmd.vl);
if(y == 0) {
cmd.vl *= BACKWARD;
cmd.vr *= FORWARD;
}
if(y > 0) {
cmd.vl *= FORWARD;
cmd.vr *= FORWARD;
}
if(y < 0) {
cmd.vl *= BACKWARD;
cmd.vr *= BACKWARD;
}
}
}
void loop() {
int xpos = 0;
int ypos = 0;
read_data(&xpos, &ypos);
char buf[128];
sprintf(buf, "(%4d,%4d): %4d %4d", xpos, ypos, cmd.vl, cmd.vr);
Serial.println(buf);
radio.write(&cmd, sizeof(cmd));
delay(10);
}