MYGL/Car
4
0
Fork 0
forked from StopPointTeam/TaiChi
Code Issues Pull Requests Projects Releases Wiki Activity

由路径库接管路径控制

Browse Source
This commit is contained in:
lxbpxylps@126.com 2021-03-23 12:25:53 +08:00
parent 474c40c3c1
commit 11e34760eb
1 changed files with 52 additions and 113 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

165
TaiChi/TaiChi.ino
View File

@ -2,62 +2,20 @@
#include <EEPROM.h> #include <EEPROM.h>
#include <NeoHWSerial.h> #include <NeoHWSerial.h>
#include "routeTaiChi.h" //路径库
#include "moveTaiChi.h" //轮胎运动库 #include "moveTaiChi.h" //轮胎运动库
#include "sensorTaiChi.h" //传感器库 #include "sensorTaiChi.h" //传感器库
#include "servoTaiChi.h" //舵机库 #include "servoTaiChi.h" //舵机库
#include "radioTaiChi.h" //通信库 #include "radioTaiChi.h" //通信库
Route route; //路径实例
Move move; //轮胎运动实例 Move move; //轮胎运动实例
Sensor sensor; //传感器实例 Sensor sensor; //传感器实例
Servo servo; //舵机实例 Servo servo; //舵机实例
Radio radio; //通讯实例 Radio radio; //通讯实例
//****************************************运动路径****************************************
//坐标点操作定义
#define NORMAL_POINT 0 //普通点
#define CATCH_POINT 1 //抓取点
#define RELEASE_POINT 2 //释放点（使用机械臂）
#define CARRY_POINT 3 //携带点（从底盘）
#define GETOUT_POINT 4 //释放点（从底盘）
#define GAIN_POINT 5 //增益点
//坐标点数组定义
#define X 0
#define Y 1
#define TYPE 2
int8_t route[][3] =
{
{0, 0, NORMAL_POINT},
{0, 1, NORMAL_POINT},
{0, 2, NORMAL_POINT},
{-1, 2, NORMAL_POINT},
{-1, 1, CATCH_POINT},
{0, 1, NORMAL_POINT},
{0, 0, NORMAL_POINT},
{-1, 0, RELEASE_POINT},
{0, 0, NORMAL_POINT},
{0, 1, NORMAL_POINT},
{0, 2, NORMAL_POINT},
{-1, 2, NORMAL_POINT},
{-1, 1, NORMAL_POINT},
{0, 1, NORMAL_POINT},
{0, 0, NORMAL_POINT},
{-1, 0, GAIN_POINT},
{0, 0, NORMAL_POINT},
{0, 1, NORMAL_POINT},
{0, 2, NORMAL_POINT},
{-1, 2, NORMAL_POINT},
{-1, 1, NORMAL_POINT},
{0, 1, NORMAL_POINT},
{0, 0, NORMAL_POINT},
{-1, 0, RELEASE_POINT}
};
//***************************************************************************************
//****************************************可调参数**************************************** //****************************************可调参数****************************************
//EEPROM 写入位置 //EEPROM 写入位置
#define EEPROM_ADDRESS 0x0 #define EEPROM_ADDRESS 0x0
@ -138,13 +96,10 @@ int gray_7_gate;
//转向开始到结束的时间 //转向开始到结束的时间
int delay_time_after_turn; int delay_time_after_turn;
//数组位置标记 //从 route 实例获取的点
int passed_flag = 0; Point passed_point;
int next_flag = 1; Point next_point;
int next_next_flag = 2; Point next_next_point;
//最大数组位置标记
int max_flag;
#define FRONT_NEXT 0 #define FRONT_NEXT 0
#define BACK_NEXT 1 #define BACK_NEXT 1
@ -185,10 +140,8 @@ void ReadFromEEPROM(void);
//在开始运行前依次检测各灰度传感器下方黑白是否正常，若不正常，异常传感器闪烁，程序不继续进行 //在开始运行前依次检测各灰度传感器下方黑白是否正常，若不正常，异常传感器闪烁，程序不继续进行
void CheckGrayStatus(void); void CheckGrayStatus(void);
#define NO_JUMP 0 //从 route 实例获取点
#define JUMP_DEAD_ROAD 1 void GetPointFromRoute(void);
//更新标记
void UpdateFlag(uint8_t jump_choice = NO_JUMP);
//计算方向，同时更改完成转向后相对下一点的朝向 //计算方向，同时更改完成转向后相对下一点的朝向
uint8_t CalcDirection(void); uint8_t CalcDirection(void);
@ -228,6 +181,8 @@ void HandleMessage(char * message);
#ifdef TAICHI_DEBUG #ifdef TAICHI_DEBUG
#include "MemoryUsage.h"
#define DEBUG_BAUT_RATE 115200 #define DEBUG_BAUT_RATE 115200
int loop_time = 0; int loop_time = 0;
@ -237,13 +192,13 @@ void DebugCanNotContinue(char* message)
{ {
NeoSerial.print("#TAICHI: CAN NOT CONTINUE WHEN "); NeoSerial.println(message); NeoSerial.print("#TAICHI: CAN NOT CONTINUE WHEN "); NeoSerial.println(message);
NeoSerial.print("#TAICHI: loop_time: "); NeoSerial.println(loop_time); NeoSerial.print("#TAICHI: loop_time: "); NeoSerial.println(loop_time);
NeoSerial.print("#TAICHI: pass: ["); NeoSerial.print(route[passed_flag][X]); NeoSerial.print(", "); NeoSerial.print(route[passed_flag][Y]); NeoSerial.print("]"); NeoSerial.print("#TAICHI: pass: ["); NeoSerial.print(passed_point.x); NeoSerial.print(", "); NeoSerial.print(passed_point.y); NeoSerial.print("]");
NeoSerial.print(" flag: "); NeoSerial.print(passed_flag); NeoSerial.print(" TYPE: "); NeoSerial.println((int)passed_point.type);
NeoSerial.print(" TYPE: "); NeoSerial.println((int)route[passed_flag][TYPE]); NeoSerial.print("#TAICHI: next: ["); NeoSerial.print(next_point.x); NeoSerial.print(", "); NeoSerial.print(next_point.y); NeoSerial.print("]");
NeoSerial.print("#TAICHI: next: ["); NeoSerial.print(route[next_flag][X]); NeoSerial.print(", "); NeoSerial.print(route[next_flag][Y]); NeoSerial.print("]");
NeoSerial.print(" next_position: "); NeoSerial.print((int)next_position); NeoSerial.print(" next_position: "); NeoSerial.print((int)next_position);
NeoSerial.print(" TYPE: "); NeoSerial.println((int)route[next_flag][TYPE]); NeoSerial.print(" TYPE: "); NeoSerial.println((int)next_point.type);
NeoSerial.print("#TAICHI: is_claw_catch: "); NeoSerial.print((int)is_claw_catch); NeoSerial.print(" is_claw_ok: "); NeoSerial.println((int)is_claw_ok); NeoSerial.print("#TAICHI: is_claw_catch: "); NeoSerial.print((int)is_claw_catch); NeoSerial.print(" is_claw_ok: "); NeoSerial.println((int)is_claw_ok);
SRamDisplay();
while (1) {} while (1) {}
} }
@ -256,6 +211,7 @@ void setup()
#ifdef TAICHI_DEBUG #ifdef TAICHI_DEBUG
NeoSerial.begin(DEBUG_BAUT_RATE); NeoSerial.begin(DEBUG_BAUT_RATE);
NeoSerial.println("#TAICHI: ======================setup()====================="); NeoSerial.println("#TAICHI: ======================setup()=====================");
SRamDisplay();
#endif #endif
SetGNDPins(); SetGNDPins();
@ -277,9 +233,6 @@ void setup()
//在开始运行前依次检测各灰度传感器下方黑白是否正常 //在开始运行前依次检测各灰度传感器下方黑白是否正常
CheckGrayStatus(); CheckGrayStatus();
//计算最大数组下标
max_flag = sizeof(route) / sizeof(route[0]) - 1;
//前往 0, 0 //前往 0, 0
//沿线直行，到后端传感器接触下一条线离开函数 //沿线直行，到后端传感器接触下一条线离开函数
LineForward(BACK_END); LineForward(BACK_END);
@ -290,25 +243,24 @@ void setup()
void loop() void loop()
{ {
//从 route 实例获取点
GetPointFromRoute();
#ifdef TAICHI_DEBUG #ifdef TAICHI_DEBUG
loop_time++; loop_time++;
NeoSerial.println("#TAICHI: ====================New loop()===================="); NeoSerial.println("#TAICHI: ====================New loop()====================");
NeoSerial.print("#TAICHI: loop_time: "); NeoSerial.println(loop_time); NeoSerial.print("#TAICHI: loop_time: "); NeoSerial.println(loop_time);
NeoSerial.print("#TAICHI: pass: ["); NeoSerial.print(route[passed_flag][X]); NeoSerial.print(", "); NeoSerial.print(route[passed_flag][Y]); NeoSerial.print("]"); NeoSerial.print("#TAICHI: pass: ["); NeoSerial.print(passed_point.x); NeoSerial.print(", "); NeoSerial.print(passed_point.y); NeoSerial.print("]");
NeoSerial.print(" flag: "); NeoSerial.print(passed_flag); NeoSerial.print(" TYPE: "); NeoSerial.println((int)passed_point.type);
NeoSerial.print(" TYPE: "); NeoSerial.println((int)route[passed_flag][TYPE]); NeoSerial.print("#TAICHI: next: ["); NeoSerial.print(next_point.x); NeoSerial.print(", "); NeoSerial.print(next_point.y); NeoSerial.print("]");
NeoSerial.print("#TAICHI: next: ["); NeoSerial.print(route[next_flag][X]); NeoSerial.print(", "); NeoSerial.print(route[next_flag][Y]); NeoSerial.print("]");
NeoSerial.print(" next_position: "); NeoSerial.print((int)next_position); NeoSerial.print(" next_position: "); NeoSerial.print((int)next_position);
NeoSerial.print(" TYPE: "); NeoSerial.println((int)route[next_flag][TYPE]); NeoSerial.print(" TYPE: "); NeoSerial.println((int)next_point.type);
NeoSerial.print("#TAICHI: is_claw_catch: "); NeoSerial.print((int)is_claw_catch); NeoSerial.print(" is_claw_ok: "); NeoSerial.println((int)is_claw_ok); NeoSerial.print("#TAICHI: is_claw_catch: "); NeoSerial.print((int)is_claw_catch); NeoSerial.print(" is_claw_ok: "); NeoSerial.println((int)is_claw_ok);
SRamDisplay();
#endif #endif
int8_t& passed_flag_type = route[passed_flag][TYPE];
int8_t& next_flag_type = route[next_flag][TYPE];
int8_t& next_next_flag_type = route[next_next_flag][TYPE];
//情况一：刚完整经过普通点，下一个点为普通点或携带点 //情况一：刚完整经过普通点，下一个点为普通点或携带点
if (passed_flag_type == NORMAL_POINT && (next_flag_type == NORMAL_POINT || next_flag_type == CARRY_POINT)) if (passed_point.type == NORMAL_POINT && (next_point.type == NORMAL_POINT || next_point.type == CARRY_POINT))
{ {
if (next_position == FRONT_NEXT) if (next_position == FRONT_NEXT)
{ {
@ -316,13 +268,13 @@ void loop()
LineForward(FRONT_END); LineForward(FRONT_END);
//若下一点为携带点 //若下一点为携带点
if (next_flag_type == CARRY_POINT) if (next_point.type == CARRY_POINT)
{ {
//底盘携带 //底盘携带
is_carry = true; is_carry = true;
//越过点成为普通点 //越过点成为普通点
next_flag_type = NORMAL_POINT; route.SetNextPointType(NORMAL_POINT);
} }
} }
else else
@ -335,7 +287,7 @@ void loop()
TurnDirection(); TurnDirection();
} }
//情况二：刚完整经过普通点，下一个点为抓取点 //情况二：刚完整经过普通点，下一个点为抓取点
else if (passed_flag_type == NORMAL_POINT && next_flag_type == CATCH_POINT) else if (passed_point.type == NORMAL_POINT && next_point.type == CATCH_POINT)
{ {
if (!is_claw_catch && !is_claw_ok) //爪子上无环且状态不正常 if (!is_claw_catch && !is_claw_ok) //爪子上无环且状态不正常
{ {
@ -372,10 +324,10 @@ void loop()
TurnDirection(); TurnDirection();
//将越过的点视为普通点。因为即使抓取失败，环也会被携带在底盘内 //将越过的点视为普通点。因为即使抓取失败，环也会被携带在底盘内
next_flag_type = NORMAL_POINT; route.SetNextPointType(NORMAL_POINT);
} }
//情况三：刚完整经过普通点，下一个点为释放点（使用机械臂） //情况三：刚完整经过普通点，下一个点为释放点（使用机械臂）
else if (passed_flag_type == NORMAL_POINT && next_flag_type == RELEASE_POINT) else if (passed_point.type == NORMAL_POINT && next_point.type == RELEASE_POINT)
{ {
//沿线直行，在释放柱离开函数 //沿线直行，在释放柱离开函数
LineForward(RELEASE_END); LineForward(RELEASE_END);
@ -411,7 +363,7 @@ void loop()
delay(STOP_DELAY_TIME); delay(STOP_DELAY_TIME);
} }
//情况四：刚完整经过释放点（使用机械臂）或增益抓取点，下一个点为普通点 //情况四：刚完整经过释放点（使用机械臂）或增益抓取点，下一个点为普通点
else if ((passed_flag_type == RELEASE_POINT || passed_flag_type == GAIN_POINT) && next_flag_type == NORMAL_POINT) else if ((passed_point.type == RELEASE_POINT || passed_point.type == GAIN_POINT) && next_point.type == NORMAL_POINT)
{ {
//底盘携带清空 //底盘携带清空
is_carry = false; is_carry = false;
@ -423,7 +375,7 @@ void loop()
TurnDirection(); TurnDirection();
} }
//情况五：刚完整经过普通点，下一个点为释放点（从底盘） //情况五：刚完整经过普通点，下一个点为释放点（从底盘）
else if (passed_flag_type == NORMAL_POINT && next_flag_type == GETOUT_POINT) else if (passed_point.type == NORMAL_POINT && next_point.type == GETOUT_POINT)
{ {
//沿线直行，到前端传感器接触下一条线离开函数 //沿线直行，到前端传感器接触下一条线离开函数
LineForward(FRONT_END); LineForward(FRONT_END);
@ -438,7 +390,7 @@ void loop()
next_position = BACK_NEXT; next_position = BACK_NEXT;
} }
//情况六：刚完整经过释放点（从底盘），下一个点为普通点 //情况六：刚完整经过释放点（从底盘），下一个点为普通点
else if (passed_flag_type == GETOUT_POINT && next_flag_type == NORMAL_POINT) else if (passed_point.type == GETOUT_POINT && next_point.type == NORMAL_POINT)
{ {
//沿线后退，到前端传感器接触线离开函数 //沿线后退，到前端传感器接触线离开函数
LineBackward(FRONT_END); LineBackward(FRONT_END);
@ -453,7 +405,7 @@ void loop()
TurnDirection(); TurnDirection();
} }
//情况七：刚完整经过普通点，下一个点为增益抓取点 //情况七：刚完整经过普通点，下一个点为增益抓取点
else if (passed_flag_type == NORMAL_POINT && next_flag_type == GAIN_POINT) else if (passed_point.type == NORMAL_POINT && next_point.type == GAIN_POINT)
{ {
if (!is_claw_ok) //爪子状态不正常 if (!is_claw_ok) //爪子状态不正常
{ {
@ -532,8 +484,8 @@ void loop()
#endif #endif
} }
//更新标记，继续循环 //更新位置，继续循环
UpdateFlag(); route.UpdatePosition();
#ifdef TAICHI_DEBUG #ifdef TAICHI_DEBUG
NeoSerial.println("#TAICHI: ====================End loop()===================="); NeoSerial.println("#TAICHI: ====================End loop()====================");
@ -624,27 +576,12 @@ void CheckGrayStatus(void)
} }
//更新标记 //从 route 实例获取点
void UpdateFlag(uint8_t jump_choice) void GetPointFromRoute(void)
{ {
if (jump_choice == NO_JUMP) passed_point = route.GetPassedPoint();
{ next_point = route.GetNextPoint();
passed_flag = next_flag; next_next_point = route.GetNextNextPoint();
next_flag = next_next_flag;
if (++next_next_flag > max_flag)
next_next_flag = 0;
}
else
{
int current_passed_flag = passed_flag;
UpdateFlag(NO_JUMP);
UpdateFlag(NO_JUMP);
passed_flag = current_passed_flag;
}
} }
@ -652,12 +589,12 @@ void UpdateFlag(uint8_t jump_choice)
uint8_t CalcDirection(void) uint8_t CalcDirection(void)
{ {
//计算第三点与第一点的相对坐标 rx0, ry0 //计算第三点与第一点的相对坐标 rx0, ry0
int8_t rx0 = route[next_next_flag][X] - route[passed_flag][X]; int8_t rx0 = next_next_point.x - passed_point.x;
int8_t ry0 = route[next_next_flag][Y] - route[passed_flag][Y]; int8_t ry0 = next_next_point.y - passed_point.y;
//计算当前小车朝向的方向向量 vx, vy //计算当前小车朝向的方向向量 vx, vy
int8_t vx = route[next_flag][X] - route[passed_flag][X]; int8_t vx = next_point.x - passed_point.x;
int8_t vy = route[next_flag][Y] - route[passed_flag][Y]; int8_t vy = next_point.y - passed_point.y;
//坐标旋转变换 //坐标旋转变换
int8_t rx, ry; int8_t rx, ry;
@ -883,12 +820,13 @@ void LineBackward(uint8_t end_position, float speed_rate)
void TurnDirection(float speed_rate) void TurnDirection(float speed_rate)
{ {
//若下下点为释放点或增益抓取点，判断是否需要跳过 //若下下点为释放点或增益抓取点，判断是否需要跳过
if ((route[next_next_flag][TYPE] == RELEASE_POINT && (!is_claw_catch || is_carry)) if ((next_next_point.type == RELEASE_POINT && (!is_claw_catch || is_carry))
|| (route[next_next_flag][TYPE] == GETOUT_POINT && !is_carry) || (next_next_point.type == GETOUT_POINT && !is_carry)
|| (route[next_next_flag][TYPE] == GAIN_POINT && is_claw_catch) || (next_next_point.type == GAIN_POINT && is_claw_catch)
) )
{ {
UpdateFlag(JUMP_DEAD_ROAD); route.ChangeRoute(JUMP_DEAD_ROAD);
GetPointFromRoute();
#ifdef TAICHI_DEBUG #ifdef TAICHI_DEBUG
//调试输出直行或后退或转向状态 //调试输出直行或后退或转向状态
@ -1113,5 +1051,6 @@ bool OpenClawAndCheck(void)
//通讯消息处理函数 //通讯消息处理函数
void HandleMessage(char* message) void HandleMessage(char* message)
{ {
// radio.Send("Get the message: ");
radio.Send(message);
} }