最终版本，完结撒花
This commit is contained in:
parent
1dffd82d2e
commit
ebd97ef583
@ -1,115 +0,0 @@
|
||||
/*
|
||||
MemoryUsage.c - MemoryUsage library
|
||||
Copyright (c) 2015 Thierry Paris. All right reserved.
|
||||
|
||||
This library is free software; you can redistribute it and/or
|
||||
modify it under the terms of the GNU Lesser General Public
|
||||
License as published by the Free Software Foundation; either
|
||||
version 2.1 of the License, or (at your option) any later version.
|
||||
|
||||
This library is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
Lesser General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU Lesser General Public
|
||||
License along with this library; if not, write to the Free Software
|
||||
Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
|
||||
*/
|
||||
|
||||
/*! \file MemoryUsage.cpp
|
||||
|
||||
Main library code file.
|
||||
*/
|
||||
|
||||
#include "Arduino.h"
|
||||
#include "MemoryUsage.h"
|
||||
|
||||
/// Thanks to adafruit : https://learn.adafruit.com/memories-of-an-arduino/measuring-free-memory
|
||||
int mu_freeRam()
|
||||
{
|
||||
//extern int __heap_start, *__brkval;
|
||||
int v;
|
||||
return (int)&v - (__brkval == 0 ? (int)&__heap_start : (int)__brkval);
|
||||
}
|
||||
|
||||
/// Copy / adaptation of the library StackPaint available here : https://github.com/WickedDevice/StackPaint
|
||||
|
||||
#define STACK_CANARY 0xc5
|
||||
|
||||
void mu_StackPaint(void) __attribute__((naked)) __attribute__((section(".init1")));
|
||||
|
||||
|
||||
/// Function called before any other function.
|
||||
void mu_StackPaint(void)
|
||||
{
|
||||
#if 1
|
||||
uint8_t *p = &_end;
|
||||
|
||||
while (p <= &__stack)
|
||||
{
|
||||
*p = STACK_CANARY;
|
||||
p++;
|
||||
}
|
||||
#else
|
||||
__asm volatile (
|
||||
" ldi r30,lo8(_end)\n"
|
||||
" ldi r31,hi8(_end)\n"
|
||||
" ldi r24,lo8(0xc5)\n" // STACK_CANARY = 0xc5
|
||||
" ldi r25,hi8(__stack)\n"
|
||||
" rjmp .cmp\n"
|
||||
".loop:\n"
|
||||
" st Z+,r24\n"
|
||||
".cmp:\n"
|
||||
" cpi r30,lo8(__stack)\n"
|
||||
" cpc r31,r25\n"
|
||||
" brlo .loop\n"
|
||||
" breq .loop"::);
|
||||
#endif
|
||||
}
|
||||
|
||||
/// Checks the first undecorated byte.
|
||||
uint16_t mu_StackCount(void)
|
||||
{
|
||||
uint8_t *p = (__brkval == 0 ? (uint8_t *) &__heap_start : __brkval);
|
||||
|
||||
while (*p == STACK_CANARY && (int) p <= SP)
|
||||
p++;
|
||||
|
||||
return (uint16_t)RAMEND - (uint16_t)p;
|
||||
}
|
||||
|
||||
/// Modified function from http://www.avr-developers.com/mm/memoryusage.html
|
||||
void SRamDisplay(void)
|
||||
{
|
||||
int data_size = (int)&__data_end - (int)&__data_start;
|
||||
int bss_size = (int)&__bss_end - (int)&__data_end;
|
||||
int heap_end = (int) (__brkval == 0 ? (uint8_t *) &__heap_start : __brkval);
|
||||
//int heap_end = (int)SP - (int)&__malloc_margin;
|
||||
int heap_size = heap_end - (int)&__bss_end;
|
||||
int stack_size = RAMEND - (int)SP + 1;
|
||||
int available = (RAMEND - (int)&__data_start + 1);
|
||||
|
||||
available -= data_size + bss_size + heap_size + stack_size;
|
||||
|
||||
NeoSerialDebug.print(F("+----------------+ ")); NeoSerialDebug.print((int)&__data_start); NeoSerialDebug.println(" (__data_start)");
|
||||
NeoSerialDebug.print(F("+ data +")); NeoSerialDebug.println();
|
||||
NeoSerialDebug.print(F("+ variables + size = ")); NeoSerialDebug.println(data_size);
|
||||
NeoSerialDebug.print(F("+----------------+ ")); NeoSerialDebug.print((int)&__data_end); NeoSerialDebug.println(" (__data_end / __bss_start)");
|
||||
NeoSerialDebug.print(F("+ bss +")); NeoSerialDebug.println();
|
||||
NeoSerialDebug.print(F("+ variables + size = ")); NeoSerialDebug.println(bss_size);
|
||||
NeoSerialDebug.print(F("+----------------+ ")); NeoSerialDebug.print((int)&__bss_end); NeoSerialDebug.println(" (__bss_end / __heap_start)");
|
||||
NeoSerialDebug.print(F("+ heap + size = ")); NeoSerialDebug.println(heap_size);
|
||||
NeoSerialDebug.print(F("+----------------+ ")); NeoSerialDebug.print((int)heap_end); NeoSerialDebug.println(" (__brkval if not 0, or __heap_start)");
|
||||
NeoSerialDebug.print(F("+ +")); NeoSerialDebug.println();
|
||||
NeoSerialDebug.print(F("+ +")); NeoSerialDebug.println();
|
||||
NeoSerialDebug.print(F("+ FREE RAM + size = ")); NeoSerialDebug.println(available);
|
||||
NeoSerialDebug.print(F("+ +")); NeoSerialDebug.println();
|
||||
NeoSerialDebug.print(F("+ +")); NeoSerialDebug.println();
|
||||
NeoSerialDebug.print(F("+----------------+ ")); NeoSerialDebug.print((int)SP); NeoSerialDebug.println(" (SP)");
|
||||
NeoSerialDebug.print(F("+ stack + size = ")); NeoSerialDebug.println(stack_size);
|
||||
NeoSerialDebug.print(F("+----------------+ ")); NeoSerialDebug.print((int)RAMEND); NeoSerialDebug.println(" (RAMEND / __stack)");
|
||||
|
||||
NeoSerialDebug.println();
|
||||
NeoSerialDebug.println();
|
||||
}
|
||||
@ -1,190 +0,0 @@
|
||||
/*
|
||||
MemoryUsage.h - MemoryUsage library V2.10
|
||||
Copyright (c) 2015 Thierry Paris. All right reserved.
|
||||
|
||||
This library is free software; you can redistribute it and/or
|
||||
modify it under the terms of the GNU Lesser General Public
|
||||
License as published by the Free Software Foundation; either
|
||||
version 2.1 of the License, or (at your option) any later version.
|
||||
|
||||
This library is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
Lesser General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU Lesser General Public
|
||||
License along with this library; if not, write to the Free Software
|
||||
Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
|
||||
*/
|
||||
|
||||
#ifndef __MemoryUsage_h__
|
||||
#define __MemoryUsage_h__
|
||||
|
||||
#include <stdint.h>
|
||||
#include <NeoHWSerial.h>
|
||||
|
||||
#define NeoSerialDebug NeoSerial3
|
||||
|
||||
/*! \mainpage
|
||||
|
||||
A full explanation in french can be read at http://www.locoduino.org/ecrire/?exec=article&action=redirect&type=article&id=149 .
|
||||
|
||||
Roughly, the SRAM memory is divided into four areas: the static data, the heap, the free ram and the stack.
|
||||
|
||||
The static data size is given by the compiler itself after the building. this is filled by all variables and
|
||||
arrays declared in global scope, or with 'static' keyword.
|
||||
|
||||
The heap is filled with all the dynamic allocations done with 'new' keyword or 'malloc' functions.
|
||||
|
||||
The stack start from the end of the SRAM area and grow and shrink downward at each function call, it stores
|
||||
all the local data internal to a function, function arguments (depending of the architecture, arguments can be
|
||||
stored in CPU registers to improve speed...) , and addresses for function returns to caller.
|
||||
|
||||
SRAM memory
|
||||
\verbatim
|
||||
+---------------+------------------+---------------------------------------------+-----------------+
|
||||
| | | | |
|
||||
| | | | |
|
||||
| static | | | |
|
||||
| data | heap | free ram | stack |
|
||||
| | | | |
|
||||
| | | | |
|
||||
| | | | |
|
||||
+---------------+------------------+---------------------------------------------+-----------------+
|
||||
_end or __heap_start __brkval SP RAMEND
|
||||
\endverbatim
|
||||
|
||||
Source : http://www.nongnu.org/avr-libc/user-manual/malloc.html
|
||||
|
||||
MemoryUsage try to help you to find the actual memory status with differents strategies, but dont forget
|
||||
that when you observe something, you change the result of the observation : execution time is consumed
|
||||
by the analysis tools, and memory used will grow because of these tools !
|
||||
|
||||
1. First, there are the MACROs to show memory areas start/end addresses and actual sizes.
|
||||
|
||||
2. Second, there is a display function to show a 'map' of the memory...
|
||||
|
||||
3. Third, a function can give you the current size of the free ram using a stack tag, which is more accurate
|
||||
than the MACRO.
|
||||
|
||||
4. Fourth, an elegant way to try to understand how much size has reached the stack during execution.
|
||||
It will 'decorate' the internal memory, and try to identify after a moment of execution at what place
|
||||
the first byte of the memory is not anymore decorated...
|
||||
The function mu_StackPaint will be called _before the setup() function of your sketch, to 'paint' or
|
||||
'decorate' all the bytes of the SRAM momery with a particular code, called the CANARY... Later, a function
|
||||
mu_StackCount can be called to get the actual maximum size reached by the stack by counter the byte
|
||||
no more painted.
|
||||
This is a copy / adaptation of the library StackPaint available here : https://github.com/WickedDevice/StackPaint
|
||||
|
||||
5. And five at least, and because a stack grow and shrink continuously, the macros STACK_DECLARE / STACK_COMPUTE / STACK_PRINT
|
||||
try to get the greatest size of the stack by 'sampling' the execution.
|
||||
Start your code by
|
||||
|
||||
#include <MemoryUsage.h>
|
||||
|
||||
STACK_DECLARE
|
||||
|
||||
void setup()
|
||||
...
|
||||
|
||||
then add a STACK_COMPUTE in any function that can be called :
|
||||
|
||||
void subFonction()
|
||||
{
|
||||
double v[SIZE];
|
||||
STACK_COMPUTE;
|
||||
|
||||
.... // do things
|
||||
}
|
||||
|
||||
and finish by printing on the console the biggest size of the stack with STACK_PRINT or STACK_PRINT_TEXT.
|
||||
Be careful with this method, this introduce some code in every function of your sketch, so if the timing
|
||||
is important for your applicaion, take care of it !
|
||||
*/
|
||||
|
||||
/*! \file MemoryUsage.h
|
||||
|
||||
Main library header file.
|
||||
*/
|
||||
|
||||
extern uint8_t _end;
|
||||
extern uint8_t __stack;
|
||||
extern uint8_t *__brkval;
|
||||
extern uint8_t *__data_start;
|
||||
extern uint8_t *__data_end;
|
||||
extern uint8_t *__heap_start;
|
||||
extern uint8_t *__heap_end;
|
||||
extern uint8_t *__bss_start;
|
||||
extern uint8_t *__bss_end;
|
||||
|
||||
//
|
||||
// Memory addresses
|
||||
//
|
||||
|
||||
/// Print data start on NeoSerialDebug console.
|
||||
#define MEMORY_PRINT_START { NeoSerialDebug.print(F("Data start:")); NeoSerialDebug.println((int) &__data_start); }
|
||||
/// Print data end / heap start on NeoSerialDebug console.
|
||||
#define MEMORY_PRINT_HEAPSTART { NeoSerialDebug.print(F("Heap start:")); NeoSerialDebug.println((int)&__heap_start); }
|
||||
/// Print heap end / free ram area on NeoSerialDebug console.
|
||||
#define MEMORY_PRINT_HEAPEND { NeoSerialDebug.print(F("Heap end:")); NeoSerialDebug.println(__brkval == 0 ? (int)&__heap_start : (int)__brkval); }
|
||||
/// Print free ram end / stack start on NeoSerialDebug console.
|
||||
#define MEMORY_PRINT_STACKSTART { NeoSerialDebug.print(F("Stack start:")); NeoSerialDebug.println((int) SP); }
|
||||
/// Print end of memory on NeoSerialDebug console.
|
||||
#define MEMORY_PRINT_END { NeoSerialDebug.print(F("Stack end:")); NeoSerialDebug.println((int) RAMEND); }
|
||||
|
||||
/// Print heap size on NeoSerialDebug console.
|
||||
#define MEMORY_PRINT_HEAPSIZE { NeoSerialDebug.print(F("Heap size:")); NeoSerialDebug.println((int) (__brkval == 0 ? (int)&__heap_start : (int)__brkval) - (int)&__heap_start); }
|
||||
/// Print stack size on NeoSerialDebug console.
|
||||
#define MEMORY_PRINT_STACKSIZE { NeoSerialDebug.print(F("Stack size:")); NeoSerialDebug.println((int) RAMEND - (int)SP); }
|
||||
/// Print free ram size on NeoSerialDebug console.
|
||||
#define MEMORY_PRINT_FREERAM { NeoSerialDebug.print(F("Free ram:")); NeoSerialDebug.println((int) SP - (int) (__brkval == 0 ? (int)&__heap_start : (int)__brkval)); }
|
||||
/// Print total SRAM size on NeoSerialDebug console.
|
||||
#define MEMORY_PRINT_TOTALSIZE { NeoSerialDebug.print(F("SRAM size:")); NeoSerialDebug.println((int) RAMEND - (int) &__data_start); }
|
||||
|
||||
/// Displays the 'map' of the current state of the Arduino's SRAM memory on the NeoSerialDebug console.
|
||||
void SRamDisplay(void);
|
||||
|
||||
//
|
||||
// Stack count part. STACK_COMPUTE will get the maximum size of the stack at the moment...
|
||||
//
|
||||
|
||||
/// Must be used only one time, outside any function.
|
||||
#define STACK_DECLARE unsigned int mu_stack_size = (RAMEND - SP);
|
||||
|
||||
/// Must be called to update the current maximum size of the stack, at each function beginning.
|
||||
#define STACK_COMPUTE { mu_stack_size = (RAMEND - SP) > mu_stack_size ? (RAMEND - SP) : mu_stack_size;}
|
||||
|
||||
/// Compute the current maximum and show it now with customized text.
|
||||
#define STACK_PRINT_TEXT(text) { STACK_COMPUTE; NeoSerialDebug.print(text); NeoSerialDebug.println(mu_stack_size); }
|
||||
|
||||
/// Compute the current maximum and show it now with default text.
|
||||
#define STACK_PRINT STACK_PRINT_TEXT(F("Stack Maximum Size (Instrumentation method): "));
|
||||
|
||||
//
|
||||
// Free Ram part.
|
||||
//
|
||||
|
||||
/// Shows the current free SRAM memory with customized text.
|
||||
#define FREERAM_PRINT_TEXT(text) NeoSerialDebug.print(text); NeoSerialDebug.println(mu_freeRam());
|
||||
|
||||
/// Shows the current free SRAM memory with default text.
|
||||
#define FREERAM_PRINT FREERAM_PRINT_TEXT(F("Free Ram Size: "));
|
||||
|
||||
/// Show the free Ram size on console.
|
||||
int mu_freeRam(void);
|
||||
|
||||
//
|
||||
// StackPaint part. This macro gives a view of used stack area at the end of execution...
|
||||
//
|
||||
|
||||
/// Show the stack size on console.
|
||||
uint16_t mu_StackCount(void);
|
||||
|
||||
/// Compute the current maximum and show it now with customized text.
|
||||
#define STACKPAINT_PRINT_TEXT(text) { NeoSerialDebug.print(text); NeoSerialDebug.println(mu_StackCount()); }
|
||||
|
||||
/// Compute the current maximum and show it now with default text.
|
||||
#define STACKPAINT_PRINT STACKPAINT_PRINT_TEXT(F("Stack Maximum Size (Painting method): "));
|
||||
|
||||
|
||||
#endif
|
||||
@ -1,251 +1,157 @@
|
||||
#include <Arduino.h>
|
||||
#include <EEPROM.h>
|
||||
#include <NeoHWSerial.h>
|
||||
|
||||
#include "routeMixElement.h" //路径库
|
||||
#include "moveMixElement.h" //轮胎运动库
|
||||
#include "moveMixElement.h" //运动库
|
||||
#include "sensorMixElement.h" //传感器库
|
||||
#include "radioMixElement.h" //通信库
|
||||
|
||||
|
||||
Route route; //路径实例
|
||||
Move move; //轮胎运动实例
|
||||
Sensor sensor; //传感器实例
|
||||
Radio radio; //通讯实例
|
||||
|
||||
|
||||
//****************************************可调参数****************************************
|
||||
//EEPROM 写入位置
|
||||
#define EEPROM_ADDRESS 0x0
|
||||
//从触碰白线到开始转向延时
|
||||
#define BEFORE_FORTURN_DELAY 175
|
||||
|
||||
//（前进转）从触碰白线到开始转向延时
|
||||
#define BEFORE_FORTURN_DELAY 670
|
||||
//（后退转）从触碰白线到开始转向延时
|
||||
#define BEFORE_BACKTURN_DELAY 500
|
||||
|
||||
//开始转到开始检测是否摆正的延时
|
||||
#define BEFORE_CHECK_STRAIGHT_DELAY 700
|
||||
//开始转到结束转的延时
|
||||
#define AFTER_FORTURN_DELAY 270
|
||||
//***************************************************************************************
|
||||
|
||||
|
||||
//****************************************全局变量****************************************
|
||||
//GND Pins
|
||||
const uint8_t gnd_pins[8] = {12, 13, 32, 33, 34, 35, 36, 37};
|
||||
|
||||
//EEPROM 储存的调试数据
|
||||
struct StroageInfo
|
||||
{
|
||||
int delay_time_after_turn;
|
||||
} stroage_info;
|
||||
|
||||
//转向开始到结束的时间
|
||||
int delay_time_after_turn;
|
||||
|
||||
//从 route 实例获取的点
|
||||
Point passed_point;
|
||||
Point next_point;
|
||||
Point next_next_point;
|
||||
|
||||
#define FRONT_NEXT 0
|
||||
#define BACK_NEXT 1
|
||||
//下一点朝向
|
||||
uint8_t next_position = FRONT_NEXT;
|
||||
|
||||
//底盘是否携带环
|
||||
bool is_carry = false;
|
||||
const uint8_t gnd_pins[1] = {A3};
|
||||
//***************************************************************************************
|
||||
|
||||
|
||||
//****************************************自定函数****************************************
|
||||
//设置接口低电平作为额外地
|
||||
void SetGNDPins(void);
|
||||
|
||||
//从 EEPROM 读取数据
|
||||
void ReadFromEEPROM(void);
|
||||
|
||||
//在开始运行前依次检测各灰度传感器下方黑白是否正常，若不正常，异常传感器闪烁，程序不继续进行
|
||||
void CheckGrayStatus(void);
|
||||
|
||||
//从 route 实例获取点
|
||||
void GetPointFromRoute(void);
|
||||
|
||||
//计算方向，同时更改完成转向后相对下一点的朝向
|
||||
uint8_t CalcDirection(void);
|
||||
|
||||
//由灰度比计算修正减速比
|
||||
float CalcFixSpeedRate(float gray_deviation_rate);
|
||||
|
||||
#define FRONT_END 0
|
||||
#define BACK_END 1
|
||||
|
||||
#define ENABLE_FIX 0
|
||||
#define DISABLE_FIX 1
|
||||
//沿线直行，在触发条件后离开函数但不停止
|
||||
void LineForward(uint8_t end_position, uint8_t type = ENABLE_FIX, float speed_rate = 1.0);
|
||||
|
||||
//沿线后退，在触发条件后离开函数但不停止
|
||||
void LineBackward(uint8_t end_position, uint8_t type = ENABLE_FIX, float speed_rate = 1.0);
|
||||
|
||||
//直行或后退或转向，完成后离开函数但不停止。会自动跳过无需前往的释放点
|
||||
void TurnDirection(float speed_rate = 1.0);
|
||||
|
||||
//通讯消息处理函数
|
||||
void HandleMessage(const char* message);
|
||||
//***************************************************************************************
|
||||
|
||||
|
||||
//****************************************调试相关****************************************
|
||||
//注释以关闭调试功能
|
||||
#define MIXELE_DEBUG
|
||||
#define TAICHI_DEBUG
|
||||
|
||||
#ifdef MIXELE_DEBUG
|
||||
#ifdef TAICHI_DEBUG
|
||||
|
||||
#include "MemoryUsage.h"
|
||||
|
||||
#define NeoSerialDebug NeoSerial3
|
||||
#define NeoSerialDebug NeoSerial
|
||||
#define DEBUG_BAUT_RATE 115200
|
||||
|
||||
int loop_time = 0;
|
||||
|
||||
//错误消息函数，用于在出现致命错误后结束程序
|
||||
void DebugCanNotContinue(const char* message)
|
||||
{
|
||||
move.Stop();
|
||||
|
||||
NeoSerialDebug.print(F("#MIXELE: CAN NOT CONTINUE WHEN ")); NeoSerialDebug.println(message);
|
||||
NeoSerialDebug.print(F("#MIXELE: loop_time: ")); NeoSerialDebug.println(loop_time);
|
||||
NeoSerialDebug.print(F("#MIXELE: pass: [")); NeoSerialDebug.print(passed_point.x); NeoSerialDebug.print(F(", ")); NeoSerialDebug.print(passed_point.y); NeoSerialDebug.print(F("]"));
|
||||
NeoSerialDebug.print(F(" TYPE: ")); NeoSerialDebug.println((int)passed_point.type);
|
||||
NeoSerialDebug.print(F("#MIXELE: next: [")); NeoSerialDebug.print(next_point.x); NeoSerialDebug.print(F(", ")); NeoSerialDebug.print(next_point.y); NeoSerialDebug.print(F("]"));
|
||||
NeoSerialDebug.print(F(" next_position: ")); NeoSerialDebug.print((int)next_position);
|
||||
NeoSerialDebug.print(F(" TYPE: ")); NeoSerialDebug.println((int)next_point.type);
|
||||
SRamDisplay();
|
||||
|
||||
while (1) {}
|
||||
}
|
||||
#endif
|
||||
//***************************************************************************************
|
||||
|
||||
|
||||
void setup()
|
||||
{
|
||||
#ifdef MIXELE_DEBUG
|
||||
uint8_t mode;
|
||||
|
||||
NeoSerialDebug.begin(DEBUG_BAUT_RATE);
|
||||
NeoSerialDebug.println(F("#MIXELE: ======================setup()====================="));
|
||||
SRamDisplay();
|
||||
#endif
|
||||
|
||||
|
||||
SetGNDPins();
|
||||
|
||||
move.Stop();
|
||||
// while (1)
|
||||
// {
|
||||
// sensor.IsWhite(GRAY_1);
|
||||
// }
|
||||
|
||||
radio.SetHandleMessageFunction(HandleMessage);
|
||||
radio.BeginTransmit();
|
||||
while (1)
|
||||
{
|
||||
if (sensor.IsPushed(BUTTON_1))
|
||||
{
|
||||
mode = 0;
|
||||
break;
|
||||
}
|
||||
else if (sensor.IsPushed(BUTTON_2))
|
||||
{
|
||||
mode = 1;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
//从 EEPROM 读取数据
|
||||
ReadEEPROM();
|
||||
if (mode == 0)
|
||||
{
|
||||
uint8_t white_times = 0;
|
||||
|
||||
//在开始运行前依次检测各灰度传感器下方黑白是否正常
|
||||
CheckGrayStatus();
|
||||
move.Forward();
|
||||
|
||||
//前往 x, 0
|
||||
//沿线直行，到后端传感器接触下一条线离开函数
|
||||
LineForward(BACK_END);
|
||||
while (white_times < 3)
|
||||
{
|
||||
if (sensor.IsWhite(GRAY_1))
|
||||
{
|
||||
white_times++;
|
||||
delay(200);
|
||||
}
|
||||
}
|
||||
|
||||
//已越过 x, 0 正式进入循环
|
||||
delay(BEFORE_FORTURN_DELAY);
|
||||
move.ForRightward();
|
||||
delay(AFTER_FORTURN_DELAY);
|
||||
|
||||
move.Stop();
|
||||
|
||||
//white_times = 0;
|
||||
move.Forward();
|
||||
|
||||
long start_time = millis();
|
||||
|
||||
while (1)
|
||||
{
|
||||
if (sensor.IsPushed(BUTTON_3) || millis() - start_time > 10000)
|
||||
{
|
||||
move.Stop();
|
||||
while (1) {}
|
||||
}
|
||||
}
|
||||
|
||||
// while (white_times < 5)
|
||||
// {
|
||||
// if (sensor.IsWhite(GRAY_1))
|
||||
// {
|
||||
// white_times++;
|
||||
// delay(200);
|
||||
// }
|
||||
// }
|
||||
|
||||
// move.Stop();
|
||||
}
|
||||
else
|
||||
{
|
||||
uint8_t white_times = 0;
|
||||
|
||||
move.Forward();
|
||||
|
||||
while (white_times < 3)
|
||||
{
|
||||
if (sensor.IsWhite(GRAY_1))
|
||||
{
|
||||
white_times++;
|
||||
delay(200);
|
||||
}
|
||||
}
|
||||
|
||||
delay(BEFORE_FORTURN_DELAY);
|
||||
|
||||
// move.Stop();
|
||||
// while (1) {}
|
||||
|
||||
move.ForRightward();
|
||||
|
||||
while (1)
|
||||
{
|
||||
move.ForRightward();
|
||||
delay(5000);
|
||||
move.Stop();
|
||||
delay(5000);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void loop()
|
||||
{
|
||||
//从 route 实例获取点
|
||||
GetPointFromRoute();
|
||||
|
||||
#ifdef MIXELE_DEBUG
|
||||
loop_time++;
|
||||
NeoSerialDebug.println(F("#MIXELE: ====================New loop()===================="));
|
||||
NeoSerialDebug.print(F("#MIXELE: loop_time: ")); NeoSerialDebug.println(loop_time);
|
||||
NeoSerialDebug.print(F("#MIXELE: pass: [")); NeoSerialDebug.print(passed_point.x); NeoSerialDebug.print(F(", ")); NeoSerialDebug.print(passed_point.y); NeoSerialDebug.print(F("]"));
|
||||
NeoSerialDebug.print(F(" TYPE: ")); NeoSerialDebug.println((int)passed_point.type);
|
||||
NeoSerialDebug.print(F("#MIXELE: next: [")); NeoSerialDebug.print(next_point.x); NeoSerialDebug.print(F(", ")); NeoSerialDebug.print(next_point.y); NeoSerialDebug.print(F("]"));
|
||||
NeoSerialDebug.print(F(" next_position: ")); NeoSerialDebug.print((int)next_position);
|
||||
NeoSerialDebug.print(F(" TYPE: ")); NeoSerialDebug.println((int)next_point.type);
|
||||
SRamDisplay();
|
||||
#endif
|
||||
|
||||
//情况一：刚完整经过普通点，下一个点为普通点或携带点
|
||||
if (passed_point.type == NORMAL_POINT && (next_point.type == NORMAL_POINT || next_point.type == CARRY_POINT))
|
||||
{
|
||||
if (next_position == FRONT_NEXT)
|
||||
{
|
||||
//沿线直行，到前端传感器接触下一条线离开函数
|
||||
LineForward(FRONT_END);
|
||||
|
||||
//若下一点为携带点
|
||||
if (next_point.type == CARRY_POINT)
|
||||
{
|
||||
//底盘携带
|
||||
is_carry = true;
|
||||
|
||||
//越过点成为普通点
|
||||
route.SetNextPointType(NORMAL_POINT);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
//沿线后退，到后端传感器接触下一条线离开函数
|
||||
LineBackward(BACK_END);
|
||||
}
|
||||
|
||||
//继续直行或后退或转向
|
||||
TurnDirection();
|
||||
}
|
||||
//情况二：刚完整经过普通点，下一个点为释放点（从底盘）
|
||||
else if (passed_point.type == NORMAL_POINT && next_point.type == GETOUT_POINT)
|
||||
{
|
||||
//沿线直行，到前端传感器接触下一条线离开函数
|
||||
LineForward(FRONT_END);
|
||||
|
||||
//沿线直行，到后端传感器接触下一条线离开函数
|
||||
LineForward(BACK_END);
|
||||
|
||||
//停止前进
|
||||
move.Stop();
|
||||
|
||||
//下一点朝向为后
|
||||
next_position = BACK_NEXT;
|
||||
}
|
||||
//情况三：刚完整经过释放点（从底盘），下一个点为普通点
|
||||
else if (passed_point.type == GETOUT_POINT && next_point.type == NORMAL_POINT)
|
||||
{
|
||||
//沿线后退，到前端传感器接触线离开函数
|
||||
LineBackward(FRONT_END, DISABLE_FIX);
|
||||
|
||||
//底盘携带清空
|
||||
is_carry = false;
|
||||
|
||||
//沿线后退，到后端传感器接触线离开函数
|
||||
LineBackward(BACK_END);
|
||||
|
||||
//继续后退或转向
|
||||
TurnDirection();
|
||||
}
|
||||
//出现错误
|
||||
else
|
||||
{
|
||||
move.Stop();
|
||||
|
||||
#ifdef MIXELE_DEBUG
|
||||
DebugCanNotContinue("CHOOSE LOOP");
|
||||
#endif
|
||||
}
|
||||
|
||||
//更新位置，继续循环
|
||||
route.UpdatePosition();
|
||||
|
||||
#ifdef MIXELE_DEBUG
|
||||
NeoSerialDebug.println(F("#MIXELE: ====================End loop()===================="));
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
@ -259,356 +165,4 @@ void SetGNDPins(void)
|
||||
pinMode(gnd_pins[i], OUTPUT);
|
||||
digitalWrite(gnd_pins[i], LOW);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
//从 EEPROM 读取数据
|
||||
void ReadEEPROM(void)
|
||||
{
|
||||
//从 EEPROM 读取调试数据
|
||||
EEPROM.get(EEPROM_ADDRESS, stroage_info);
|
||||
|
||||
//转向时间
|
||||
delay_time_after_turn = stroage_info.delay_time_after_turn;
|
||||
|
||||
#ifdef MIXELE_DEBUG
|
||||
NeoSerialDebug.println(F("#MIXELE: Data based on EEPROM: "));
|
||||
NeoSerialDebug.print(F("#MIXELE: delay_time_after_turn: ")); NeoSerialDebug.println(delay_time_after_turn);
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
//在开始运行前依次检测各灰度传感器下方黑白是否正常，若不正常，异常传感器闪烁，程序不继续进行
|
||||
void CheckGrayStatus(void)
|
||||
{
|
||||
//若正常，1 2 5 6 号传感器检测到黑色，3 4 号传感器检测到白色
|
||||
bool is_status_right = false;
|
||||
|
||||
while (!is_status_right)
|
||||
{
|
||||
if (sensor.IsWhite(GRAY_1))
|
||||
sensor.FlashGraySensor(GRAY_1);
|
||||
else if (sensor.IsWhite(GRAY_2))
|
||||
sensor.FlashGraySensor(GRAY_2);
|
||||
else if (!sensor.IsWhite(GRAY_3))
|
||||
sensor.FlashGraySensor(GRAY_3);
|
||||
else if (!sensor.IsWhite(GRAY_4))
|
||||
sensor.FlashGraySensor(GRAY_4);
|
||||
else if (sensor.IsWhite(GRAY_5))
|
||||
sensor.FlashGraySensor(GRAY_5);
|
||||
else if (sensor.IsWhite(GRAY_6))
|
||||
sensor.FlashGraySensor(GRAY_6);
|
||||
else is_status_right = true;
|
||||
}
|
||||
|
||||
#ifdef MIXELE_DEBUG
|
||||
NeoSerialDebug.println(F("#MIXELE: Gray Sensor Status OK!"));
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
//从 route 实例获取点
|
||||
void GetPointFromRoute(void)
|
||||
{
|
||||
passed_point = route.GetPassedPoint();
|
||||
next_point = route.GetNextPoint();
|
||||
next_next_point = route.GetNextNextPoint();
|
||||
}
|
||||
|
||||
|
||||
//计算方向，同时更改完成转向后相对下一点的朝向
|
||||
uint8_t CalcDirection(void)
|
||||
{
|
||||
//计算第三点与第一点的相对坐标 rx0, ry0
|
||||
int8_t rx0 = next_next_point.x - passed_point.x;
|
||||
int8_t ry0 = next_next_point.y - passed_point.y;
|
||||
|
||||
//计算当前小车朝向的方向向量 vx, vy
|
||||
int8_t vx = next_point.x - passed_point.x;
|
||||
int8_t vy = next_point.y - passed_point.y;
|
||||
|
||||
//坐标旋转变换
|
||||
int8_t rx, ry;
|
||||
if (vx == 0 && vy == 1)
|
||||
{
|
||||
rx = rx0;
|
||||
ry = ry0;
|
||||
}
|
||||
else if (vx == -1 && vy == 0)
|
||||
{
|
||||
rx = ry0;
|
||||
ry = -rx0;
|
||||
}
|
||||
else if (vx == 0 && vy == -1)
|
||||
{
|
||||
rx = -rx0;
|
||||
ry = -ry0;
|
||||
}
|
||||
else if (vx == 1 && vy == 0)
|
||||
{
|
||||
rx = -ry0;
|
||||
ry = rx0;
|
||||
}
|
||||
#ifdef MIXELE_DEBUG
|
||||
else DebugCanNotContinue("CALC DIRECTION <1>"); //调试用
|
||||
#endif
|
||||
|
||||
//判断行进方向
|
||||
if (rx == 0 && ry == 2)
|
||||
{
|
||||
if (next_position == FRONT_NEXT)
|
||||
{
|
||||
return FORWARD;
|
||||
}
|
||||
else
|
||||
{
|
||||
return BACKWARD;
|
||||
}
|
||||
}
|
||||
else if (rx == 0 && ry == 0)
|
||||
{
|
||||
if (next_position == FRONT_NEXT)
|
||||
{
|
||||
next_position = BACK_NEXT;
|
||||
return BACKWARD;
|
||||
}
|
||||
else
|
||||
{
|
||||
next_position = FRONT_NEXT;
|
||||
return FORWARD;
|
||||
}
|
||||
}
|
||||
else if (rx == -1 && ry == 1)
|
||||
{
|
||||
if (next_position == FRONT_NEXT)
|
||||
{
|
||||
return FORLEFTWARD;
|
||||
}
|
||||
else
|
||||
{
|
||||
next_position = FRONT_NEXT;
|
||||
return BACKLEFTWARD;
|
||||
}
|
||||
}
|
||||
else if (rx == 1 && ry == 1)
|
||||
{
|
||||
if (next_position == FRONT_NEXT)
|
||||
{
|
||||
return FORRIGHTWARD;
|
||||
}
|
||||
else
|
||||
{
|
||||
next_position = FRONT_NEXT;
|
||||
return BACKRIGHTWARD;
|
||||
}
|
||||
}
|
||||
#ifdef MIXELE_DEBUG
|
||||
else DebugCanNotContinue("CALC DIRECTION <2>"); //调试用
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
//由灰度比计算修正减速比
|
||||
float CalcFixSpeedRate(float gray_deviation_rate)
|
||||
{
|
||||
return -50.0 * pow((gray_deviation_rate - 1.0), 2.0) + 1.0;
|
||||
}
|
||||
|
||||
|
||||
//沿线直行，在触发条件后离开函数但不停止
|
||||
void LineForward(uint8_t end_position, uint8_t type, float speed_rate)
|
||||
{
|
||||
#ifdef MIXELE_DEBUG
|
||||
//调试输出沿线直行状态
|
||||
NeoSerialDebug.print(F("#MIXELE: Line Forward"));
|
||||
NeoSerialDebug.print(F(" end_position: "));
|
||||
NeoSerialDebug.println((int)end_position);
|
||||
#endif
|
||||
|
||||
move.Forward(speed_rate);
|
||||
|
||||
//记录开始时间
|
||||
unsigned long begin_time = millis();
|
||||
//记录灰度传感器匹配情况
|
||||
bool gray_match_a = false;
|
||||
bool gray_match_b = false;
|
||||
|
||||
while (1)
|
||||
{
|
||||
if (type == ENABLE_FIX)
|
||||
{
|
||||
if (!sensor.IsWhite(GRAY_3) && sensor.IsWhite(GRAY_4)) //左侧越线
|
||||
{
|
||||
move.ForRightward(speed_rate, CalcFixSpeedRate(sensor.GrayDeviationRate(GRAY_3)));
|
||||
}
|
||||
else if (sensor.IsWhite(GRAY_3) && !sensor.IsWhite(GRAY_4)) //右侧越线
|
||||
{
|
||||
move.ForLeftward(speed_rate, CalcFixSpeedRate(sensor.GrayDeviationRate(GRAY_4)));
|
||||
}
|
||||
else if (sensor.IsWhite(GRAY_3) && sensor.IsWhite(GRAY_4)) //在白线上
|
||||
{
|
||||
move.Forward(speed_rate);
|
||||
}
|
||||
else //均不符合
|
||||
{
|
||||
//move.Backward(LINE_FIND_SPEED_RATE);
|
||||
}
|
||||
}
|
||||
|
||||
if (end_position == FRONT_END) //前端接触线离开函数
|
||||
{
|
||||
if (sensor.IsWhite(GRAY_1))
|
||||
gray_match_a = true;
|
||||
|
||||
if (sensor.IsWhite(GRAY_2))
|
||||
gray_match_b = true;
|
||||
}
|
||||
else if (end_position == BACK_END) //后端接触线离开函数
|
||||
{
|
||||
if (sensor.IsWhite(GRAY_5))
|
||||
gray_match_a = true;
|
||||
|
||||
if (sensor.IsWhite(GRAY_6))
|
||||
gray_match_b = true;
|
||||
}
|
||||
|
||||
//对应前端接触线离开函数和后端接触线离开函数的情况
|
||||
if (gray_match_a && gray_match_b)
|
||||
break;
|
||||
}
|
||||
|
||||
#ifdef MIXELE_DEBUG
|
||||
//调试输出沿线直行结束
|
||||
NeoSerialDebug.println(F("#MIXELE: End Line Forward"));
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
//沿线后退，在触发条件后离开函数但不停止
|
||||
void LineBackward(uint8_t end_position, uint8_t type, float speed_rate)
|
||||
{
|
||||
#ifdef MIXELE_DEBUG
|
||||
//调试输出沿线后退状态
|
||||
NeoSerialDebug.print(F("#MIXELE: Line Backward"));
|
||||
NeoSerialDebug.print(F(" end_position: "));
|
||||
NeoSerialDebug.println((int)end_position);
|
||||
#endif
|
||||
|
||||
//记录灰度传感器匹配情况
|
||||
bool gray_match_a = false;
|
||||
bool gray_match_b = false;
|
||||
|
||||
move.Backward(speed_rate);
|
||||
|
||||
while (1)
|
||||
{
|
||||
if (type == ENABLE_FIX)
|
||||
{
|
||||
if (!sensor.IsWhite(GRAY_3) && sensor.IsWhite(GRAY_4)) //左侧越线
|
||||
{
|
||||
move.BackRightward(speed_rate, CalcFixSpeedRate(sensor.GrayDeviationRate(GRAY_3)));
|
||||
}
|
||||
else if (sensor.IsWhite(GRAY_3) && !sensor.IsWhite(GRAY_4)) //右侧越线
|
||||
{
|
||||
move.BackLeftward(speed_rate, CalcFixSpeedRate(sensor.GrayDeviationRate(GRAY_4)));
|
||||
}
|
||||
else if (sensor.IsWhite(GRAY_3) && sensor.IsWhite(GRAY_4)) //在白线上
|
||||
{
|
||||
move.Backward(speed_rate);
|
||||
}
|
||||
else //均不符合
|
||||
{
|
||||
//move.Forward(LINE_FIND_SPEED_RATE);
|
||||
}
|
||||
}
|
||||
|
||||
if (end_position == FRONT_END) //前端接触线离开函数
|
||||
{
|
||||
if (sensor.IsWhite(GRAY_1))
|
||||
gray_match_a = true;
|
||||
|
||||
if (sensor.IsWhite(GRAY_2))
|
||||
gray_match_b = true;
|
||||
}
|
||||
else //后端接触线离开函数
|
||||
{
|
||||
if (sensor.IsWhite(GRAY_5))
|
||||
gray_match_a = true;
|
||||
|
||||
if (sensor.IsWhite(GRAY_6))
|
||||
gray_match_b = true;
|
||||
}
|
||||
|
||||
//对应前端接触线离开函数和后端接触线离开函数的情况
|
||||
if (gray_match_a && gray_match_b)
|
||||
break;
|
||||
}
|
||||
|
||||
#ifdef MIXELE_DEBUG
|
||||
//调试输出沿线后退结束
|
||||
NeoSerialDebug.println(F("#MIXELE: End Line Backward"));
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
//直行或后退或转向，完成后离开函数但不停止。会自动跳过无需前往的释放点
|
||||
void TurnDirection(float speed_rate)
|
||||
{
|
||||
//若下下点为底盘释放点，判断是否需要跳过
|
||||
if (next_next_point.type == GETOUT_POINT && !is_carry)
|
||||
{
|
||||
route.ChangeRoute(JUMP_DEAD_ROAD);
|
||||
GetPointFromRoute();
|
||||
|
||||
#ifdef MIXELE_DEBUG
|
||||
//调试输出直行或后退或转向状态
|
||||
NeoSerialDebug.println(F("#MIXELE: JUMP THE GETOUT POINT FOR STATUS REASON"));
|
||||
#endif
|
||||
}
|
||||
|
||||
uint8_t direction = CalcDirection();
|
||||
|
||||
#ifdef MIXELE_DEBUG
|
||||
//调试输出直行或后退或转向状态
|
||||
NeoSerialDebug.print(F("#MIXELE: Turn Direction"));
|
||||
NeoSerialDebug.print(F(" direction: "));
|
||||
NeoSerialDebug.println((int)direction);
|
||||
#endif
|
||||
|
||||
if (direction == FORWARD) //继续直行
|
||||
{
|
||||
//沿线直行，到后端传感器接触线离开函数
|
||||
LineForward(BACK_END, speed_rate);
|
||||
}
|
||||
else if (direction == BACKWARD) //继续后退
|
||||
{
|
||||
//沿线后退，到前端传感器接触线离开函数
|
||||
LineBackward(FRONT_END, speed_rate);
|
||||
}
|
||||
else //继续转向
|
||||
{
|
||||
//等待小车旋转中心与十字中心重合
|
||||
if (direction == FORLEFTWARD || direction == FORRIGHTWARD)
|
||||
delay(BEFORE_FORTURN_DELAY);
|
||||
else delay(BEFORE_BACKTURN_DELAY);
|
||||
|
||||
//旋转
|
||||
move.MoveDirection(direction, speed_rate);
|
||||
|
||||
//基于时间判定结束
|
||||
delay(delay_time_after_turn);
|
||||
}
|
||||
|
||||
#ifdef MIXELE_DEBUG
|
||||
//调试输出直行或后退或转向结束
|
||||
NeoSerialDebug.println(F("#MIXELE: End Turn Direction"));
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
//通讯消息处理函数
|
||||
void HandleMessage(const char* message)
|
||||
{
|
||||
radio.Send("Get the message: ");
|
||||
radio.Send(message);
|
||||
}
|
||||
@ -2,10 +2,6 @@
|
||||
|
||||
#include "moveMixElement.h"
|
||||
|
||||
#ifdef MOVE_DEBUG
|
||||
#include <NeoHWSerial.h>
|
||||
#endif
|
||||
|
||||
|
||||
//静态变量
|
||||
float Move::global_speed_rate = DEFAULT_GLOBAL_SPEED_RATE; //全局速度比率
|
||||
@ -112,20 +108,14 @@ void Move::Wheel(uint8_t wheel, uint8_t rotation, float speed_rate)
|
||||
//前进
|
||||
void Move::Forward(float speed_rate)
|
||||
{
|
||||
Wheel(LEFT_A_WHEEL, FORWARD_ROTATION, speed_rate);
|
||||
Wheel(LEFT_B_WHEEL, FORWARD_ROTATION, speed_rate);
|
||||
Wheel(LEFT_A_WHEEL, FORWARD_ROTATION, speed_rate * 0.9);
|
||||
Wheel(LEFT_B_WHEEL, FORWARD_ROTATION, speed_rate * 0.9);
|
||||
Wheel(RIGHT_A_WHEEL, FORWARD_ROTATION, speed_rate);
|
||||
Wheel(RIGHT_B_WHEEL, FORWARD_ROTATION, speed_rate);
|
||||
|
||||
current_direction = FORWARD;
|
||||
current_speed_rate = speed_rate;
|
||||
current_turn_speed_rate = 1.0;
|
||||
|
||||
#ifdef MOVE_DEBUG
|
||||
//调试输出前进状态
|
||||
NeoSerialDebug.print(F("#MOVE: Move Forward"));
|
||||
NeoSerialDebug.print(F(" speed_rate: ")); NeoSerialDebug.println(speed_rate);
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
@ -140,12 +130,6 @@ void Move::Backward(float speed_rate)
|
||||
current_direction = BACKWARD;
|
||||
current_speed_rate = speed_rate;
|
||||
current_turn_speed_rate = 1.0;
|
||||
|
||||
#ifdef MOVE_DEBUG
|
||||
//调试输出后退状态
|
||||
NeoSerialDebug.print(F("#MOVE: Move Backward"));
|
||||
NeoSerialDebug.print(F(" speed_rate: ")); NeoSerialDebug.println(speed_rate);
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
@ -160,12 +144,6 @@ void Move::ForLeftward(float speed_rate, float turn_speed_rate)
|
||||
current_direction = FORLEFTWARD;
|
||||
current_speed_rate = speed_rate;
|
||||
current_turn_speed_rate = turn_speed_rate;
|
||||
|
||||
#ifdef MOVE_DEBUG
|
||||
//调试输出向前左转状态
|
||||
NeoSerialDebug.print(F("#MOVE: Move ForLeftward"));
|
||||
NeoSerialDebug.print(F(" speed_rate: ")); NeoSerialDebug.print(speed_rate); NeoSerialDebug.print(F(" turn_speed_rate: ")); NeoSerialDebug.println(turn_speed_rate);
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
@ -180,12 +158,6 @@ void Move::ForRightward(float speed_rate, float turn_speed_rate)
|
||||
current_direction = FORRIGHTWARD;
|
||||
current_speed_rate = speed_rate;
|
||||
current_turn_speed_rate = turn_speed_rate;
|
||||
|
||||
#ifdef MOVE_DEBUG
|
||||
//调试输出向前右转状态
|
||||
NeoSerialDebug.print(F("#MOVE: Move ForRightward"));
|
||||
NeoSerialDebug.print(F(" speed_rate: ")); NeoSerialDebug.print(speed_rate); NeoSerialDebug.print(F(" turn_speed_rate: ")); NeoSerialDebug.println(turn_speed_rate);
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
@ -200,12 +172,6 @@ void Move::BackLeftward(float speed_rate, float turn_speed_rate)
|
||||
current_direction = BACKLEFTWARD;
|
||||
current_speed_rate = speed_rate;
|
||||
current_turn_speed_rate = turn_speed_rate;
|
||||
|
||||
#ifdef MOVE_DEBUG
|
||||
//调试输出向后左转状态
|
||||
NeoSerialDebug.print(F("#MOVE: Move BackLeftward"));
|
||||
NeoSerialDebug.print(F(" speed_rate: ")); NeoSerialDebug.print(speed_rate); NeoSerialDebug.print(F(" turn_speed_rate: ")); NeoSerialDebug.println(turn_speed_rate);
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
@ -220,12 +186,6 @@ void Move::BackRightward(float speed_rate, float turn_speed_rate)
|
||||
current_direction = BACKRIGHTWARD;
|
||||
current_speed_rate = speed_rate;
|
||||
current_turn_speed_rate = turn_speed_rate;
|
||||
|
||||
#ifdef MOVE_DEBUG
|
||||
//调试输出向后右转状态
|
||||
NeoSerialDebug.print(F("#MOVE: Move BackRightward"));
|
||||
NeoSerialDebug.print(F(" speed_rate: ")); NeoSerialDebug.print(speed_rate); NeoSerialDebug.print(F(" turn_speed_rate: ")); NeoSerialDebug.println(turn_speed_rate);
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
@ -240,11 +200,6 @@ void Move::Stop(void)
|
||||
current_direction = STOP;
|
||||
current_speed_rate = 0;
|
||||
current_turn_speed_rate = 1.0;
|
||||
|
||||
#ifdef MOVE_DEBUG
|
||||
//调试输出制动状态
|
||||
NeoSerialDebug.println(F("#MOVE: Move Stop"));
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
|
||||
@ -2,13 +2,6 @@
|
||||
#define MOVEMIXELEMENT_H
|
||||
|
||||
|
||||
//注释以关闭调试功能
|
||||
#define MOVE_DEBUG
|
||||
|
||||
#ifdef MOVE_DEBUG
|
||||
#define NeoSerialDebug NeoSerial
|
||||
#endif
|
||||
|
||||
//轮胎定义
|
||||
#define LEFT_A_WHEEL 0
|
||||
#define LEFT_B_WHEEL 1
|
||||
@ -36,20 +29,20 @@
|
||||
#define DEFAULT_TRUN_SPEED_RATE -1.0
|
||||
|
||||
//左侧 L298N 接口定义
|
||||
#define LEFT_L298N_IN1 24
|
||||
#define LEFT_L298N_IN2 25
|
||||
#define LEFT_L298N_IN3 26
|
||||
#define LEFT_L298N_IN4 27
|
||||
#define LEFT_L298N_ENA 8
|
||||
#define LEFT_L298N_ENB 9
|
||||
#define LEFT_L298N_IN1 7
|
||||
#define LEFT_L298N_IN2 6
|
||||
#define LEFT_L298N_IN3 8
|
||||
#define LEFT_L298N_IN4 9
|
||||
#define LEFT_L298N_ENA 4
|
||||
#define LEFT_L298N_ENB 5
|
||||
|
||||
//右侧 L298N 接口定义
|
||||
#define RIGHT_L298N_IN1 28
|
||||
#define RIGHT_L298N_IN2 29
|
||||
#define RIGHT_L298N_IN3 30
|
||||
#define RIGHT_L298N_IN4 31
|
||||
#define RIGHT_L298N_ENA 10
|
||||
#define RIGHT_L298N_ENB 11
|
||||
#define RIGHT_L298N_IN1 13
|
||||
#define RIGHT_L298N_IN2 12
|
||||
#define RIGHT_L298N_IN3 11
|
||||
#define RIGHT_L298N_IN4 10
|
||||
#define RIGHT_L298N_ENA 2
|
||||
#define RIGHT_L298N_ENB 3
|
||||
|
||||
|
||||
class Move
|
||||
|
||||
@ -1,226 +0,0 @@
|
||||
#include <Arduino.h>
|
||||
#include <NeoHWSerial.h>
|
||||
|
||||
#include "radioMixElement.h"
|
||||
|
||||
|
||||
//静态变量
|
||||
HandleMessageFunction Radio::hm_func;
|
||||
NeoHWSerial* Radio::NeoSerialX = &RADIO_SERIAL_NUM;
|
||||
|
||||
|
||||
Radio::Radio()
|
||||
{
|
||||
EnableReceiveInterrupt();
|
||||
}
|
||||
|
||||
|
||||
//打开串口
|
||||
void Radio::BeginTransmit(unsigned long baud_rate)
|
||||
{
|
||||
NeoSerialX->begin(baud_rate);
|
||||
}
|
||||
|
||||
|
||||
//发送
|
||||
void Radio::Send(const char* message, uint8_t send_type, uint8_t send_times)
|
||||
{
|
||||
//强制发送，禁用接收中断，防止发送被打断
|
||||
if (send_type == FORCE_SEND)
|
||||
DisableReceiveInterrupt();
|
||||
|
||||
static unsigned char send_code = '0';
|
||||
|
||||
//生成完整通信包
|
||||
char full_message[FULL_MESSAGE_SIZE];
|
||||
uint8_t message_length = strlen(message);
|
||||
char check_str[CHECK_STR_LENGTH + 1];
|
||||
|
||||
message_length = message_length < MAX_REAL_MESSAGE_SIZE ? message_length : MAX_REAL_MESSAGE_SIZE;
|
||||
|
||||
itoa(message_length, check_str, 10);
|
||||
|
||||
for (uint8_t i = 0, j = 0, k = 0; i < FULL_MESSAGE_SIZE; i++)
|
||||
{
|
||||
if (i < BLANK_CHAR_LENGTH)
|
||||
{
|
||||
full_message[i] = BLANK_CHAR;
|
||||
}
|
||||
else if (i == BLANK_CHAR_LENGTH)
|
||||
{
|
||||
full_message[i] = CODE_CHAR;
|
||||
|
||||
full_message[++i] = send_code++;
|
||||
|
||||
if (send_code > '9')
|
||||
send_code = '0';
|
||||
|
||||
full_message[++i] = START_CHAR;
|
||||
}
|
||||
else if (j < message_length && j < MAX_REAL_MESSAGE_SIZE)
|
||||
{
|
||||
full_message[i] = message[j];
|
||||
j++;
|
||||
}
|
||||
else if (j == message_length || j == MAX_REAL_MESSAGE_SIZE)
|
||||
{
|
||||
full_message[i] = CHECK_CHAR;
|
||||
j++;
|
||||
}
|
||||
else if (k < CHECK_STR_LENGTH)
|
||||
{
|
||||
full_message[i] = check_str[k];
|
||||
k++;
|
||||
|
||||
if (check_str[k] == '\0')
|
||||
k = CHECK_STR_LENGTH;
|
||||
}
|
||||
else
|
||||
{
|
||||
full_message[i] = END_CHAR;
|
||||
}
|
||||
}
|
||||
|
||||
//发送完整通信包
|
||||
for (uint8_t times = 0; times < send_times; times++)
|
||||
{
|
||||
NeoSerialX->print(full_message);
|
||||
}
|
||||
|
||||
#ifdef RADIO_DEBUG
|
||||
NeoSerialDebug.print(F("#RADIO: SEND: "));
|
||||
full_message[FULL_MESSAGE_SIZE - 1] = '\0';
|
||||
NeoSerialDebug.print(full_message);
|
||||
NeoSerialDebug.print(F(" TIMES: "));
|
||||
NeoSerialDebug.println(send_times);
|
||||
#endif
|
||||
|
||||
if (send_type == FORCE_SEND)
|
||||
EnableReceiveInterrupt();
|
||||
}
|
||||
|
||||
|
||||
//设置接收回调函数
|
||||
void Radio::SetHandleMessageFunction(HandleMessageFunction hm_func)
|
||||
{
|
||||
Radio::hm_func = hm_func;
|
||||
}
|
||||
|
||||
|
||||
//禁用接收中断
|
||||
void Radio::DisableReceiveInterrupt() //禁用接收中断
|
||||
{
|
||||
NeoSerialX->detachInterrupt();
|
||||
}
|
||||
|
||||
|
||||
//恢复接收中断
|
||||
void Radio::EnableReceiveInterrupt() //恢复接收中断
|
||||
{
|
||||
NeoSerialX->attachInterrupt(Receive);
|
||||
}
|
||||
|
||||
|
||||
//接收，使用中断触发
|
||||
bool Radio::Receive(uint8_t ch, uint8_t status)
|
||||
{
|
||||
static bool is_code_record = false;
|
||||
static bool is_start_record = false;
|
||||
static bool is_check_record = false;
|
||||
static char message[FULL_MESSAGE_SIZE] = "";
|
||||
static char check_str[CHECK_STR_LENGTH + 1] = "";
|
||||
static unsigned char this_package_code = 0;
|
||||
static unsigned char last_package_code = 0;
|
||||
static uint8_t i = 0, j = 0;
|
||||
|
||||
switch (ch)
|
||||
{
|
||||
case CODE_CHAR:
|
||||
{
|
||||
is_code_record = true;
|
||||
is_start_record = false;
|
||||
is_check_record = false;
|
||||
|
||||
//重置字符串
|
||||
for (uint8_t k = 0; k < i; k++)
|
||||
message[k] = '\0';
|
||||
for (uint8_t k = 0; k < j; k++)
|
||||
check_str[k] = '\0';
|
||||
|
||||
i = 0;
|
||||
j = 0;
|
||||
} break;
|
||||
|
||||
case START_CHAR:
|
||||
{
|
||||
if (is_code_record == true)
|
||||
{
|
||||
is_code_record = false;
|
||||
is_start_record = true;
|
||||
}
|
||||
} break;
|
||||
|
||||
case CHECK_CHAR:
|
||||
{
|
||||
if (is_start_record == true)
|
||||
{
|
||||
is_start_record = false;
|
||||
is_check_record = true;
|
||||
}
|
||||
} break;
|
||||
|
||||
case END_CHAR:
|
||||
{
|
||||
if (is_check_record == true)
|
||||
{
|
||||
is_check_record = false;
|
||||
|
||||
if (j <= CHECK_STR_LENGTH)
|
||||
check_str[j] = '\0';
|
||||
|
||||
if (atoi(check_str) == strlen(message)) //位数校验成功
|
||||
{
|
||||
if (this_package_code != last_package_code)
|
||||
{
|
||||
#ifdef RADIO_DEBUG
|
||||
NeoSerialDebug.print(F("#RADIO: RECEIVE: "));
|
||||
NeoSerialDebug.println(message);
|
||||
#endif
|
||||
|
||||
hm_func(message);
|
||||
|
||||
last_package_code = this_package_code;
|
||||
}
|
||||
}
|
||||
else //位数校验失败
|
||||
{
|
||||
#ifdef RADIO_DEBUG
|
||||
NeoSerialDebug.println(F("#RADIO: RECEIVE CHECK FAIL!"));
|
||||
#endif
|
||||
}
|
||||
}
|
||||
} break;
|
||||
|
||||
default:
|
||||
{
|
||||
if (is_code_record == true)
|
||||
{
|
||||
this_package_code = ch;
|
||||
}
|
||||
else if (is_start_record == true)
|
||||
{
|
||||
if (i < FULL_MESSAGE_SIZE)
|
||||
message[i] = ch;
|
||||
i++;
|
||||
}
|
||||
else if (is_check_record == true)
|
||||
{
|
||||
if (j <= CHECK_STR_LENGTH)
|
||||
check_str[j] = ch;
|
||||
j++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
@ -1,69 +0,0 @@
|
||||
#ifndef RADIOMIXELEMENT_H
|
||||
#define RADIOMIXELEMENT_H
|
||||
|
||||
|
||||
#include <NeoHWSerial.h>
|
||||
|
||||
//注释以关闭调试功能
|
||||
#define RADIO_DEBUG
|
||||
|
||||
#ifdef RADIO_DEBUG
|
||||
#define NeoSerialDebug NeoSerial3
|
||||
#endif
|
||||
|
||||
//默认与 HC-12 连接串口
|
||||
#define RADIO_SERIAL_NUM NeoSerial2
|
||||
//与 HC-12 串口通信波特率
|
||||
#define RADIO_BAUD_RATE 9600
|
||||
|
||||
//通信包大小
|
||||
#define FULL_MESSAGE_SIZE 50
|
||||
//通信包最大有效信息大小
|
||||
#define MAX_REAL_MESSAGE_SIZE 39
|
||||
//通信包前段空字符填充长度
|
||||
#define BLANK_CHAR_LENGTH 4
|
||||
//通信包校验段字符串长度
|
||||
#define CHECK_STR_LENGTH 2
|
||||
//通信包标志字符
|
||||
#define BLANK_CHAR '~'
|
||||
#define CODE_CHAR '?'
|
||||
#define START_CHAR '!'
|
||||
#define CHECK_CHAR '@'
|
||||
#define END_CHAR '#'
|
||||
#define SUCCUESS_CHAR '$'
|
||||
#define FAIL_CHAR '%'
|
||||
|
||||
//发送相关宏
|
||||
#define FORCE_SEND 0 //强制发送
|
||||
#define NO_FORCE_SEND 1 //非强制发送
|
||||
|
||||
#define DEFAULT_SEND_TIMES 1 //默认重复发送次数
|
||||
|
||||
|
||||
//回调函数指针
|
||||
typedef void (*HandleMessageFunction)(const char*);
|
||||
|
||||
|
||||
class Radio
|
||||
{
|
||||
public:
|
||||
Radio();
|
||||
|
||||
static void BeginTransmit(unsigned long baud_rate = RADIO_BAUD_RATE); //打开串口
|
||||
|
||||
static void Send(const char* message, uint8_t send_type = NO_FORCE_SEND, uint8_t send_times = DEFAULT_SEND_TIMES); //发送
|
||||
|
||||
static void SetHandleMessageFunction(HandleMessageFunction hm_func); //设置接收回调函数
|
||||
static void DisableReceiveInterrupt(); //禁用接收中断
|
||||
static void EnableReceiveInterrupt(); //恢复接收中断
|
||||
|
||||
private:
|
||||
static bool Receive(uint8_t ch, uint8_t status); //接收，使用中断触发
|
||||
|
||||
static HandleMessageFunction hm_func; //接收回调函数
|
||||
|
||||
static NeoHWSerial* NeoSerialX;
|
||||
};
|
||||
|
||||
|
||||
#endif
|
||||
@ -1,329 +0,0 @@
|
||||
#include <Arduino.h>
|
||||
|
||||
#include "routeMixElement.h"
|
||||
|
||||
|
||||
#ifdef ROUTE_DEBUG
|
||||
#include <NeoHWSerial.h>
|
||||
#endif
|
||||
|
||||
|
||||
//静态变量
|
||||
Point* Route::head_point = NULL;
|
||||
Point* Route::passed_point = NULL;
|
||||
|
||||
|
||||
Route::Route()
|
||||
{
|
||||
//生成基本路径点链
|
||||
InitBaseRoute();
|
||||
|
||||
//设置 passed_point = head_point
|
||||
passed_point = head_point;
|
||||
}
|
||||
|
||||
|
||||
Point Route::GetPassedPoint(void)
|
||||
{
|
||||
return *passed_point;
|
||||
}
|
||||
|
||||
|
||||
Point Route::GetNextPoint(void)
|
||||
{
|
||||
return *passed_point->next;
|
||||
}
|
||||
|
||||
|
||||
Point Route::GetNextNextPoint(void)
|
||||
{
|
||||
return *passed_point->next->next;
|
||||
}
|
||||
|
||||
|
||||
void Route::SetPassedPoinType(uint8_t type)
|
||||
{
|
||||
SetPointType(passed_point, type);
|
||||
}
|
||||
|
||||
|
||||
void Route::SetNextPointType(uint8_t type)
|
||||
{
|
||||
SetPointType(passed_point->next, type);
|
||||
}
|
||||
|
||||
|
||||
void Route::SetNextNextPointType(uint8_t type)
|
||||
{
|
||||
SetPointType(passed_point->next->next, type);
|
||||
}
|
||||
|
||||
|
||||
//更新位置
|
||||
void Route::UpdatePosition(void)
|
||||
{
|
||||
passed_point = passed_point->next;
|
||||
|
||||
#ifdef ROUTE_DEBUG
|
||||
NeoSerialDebug.println(F("#ROUTE: UPDATE POSITION!"));
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
//更改路径
|
||||
void Route::ChangeRoute(uint8_t choice)
|
||||
{
|
||||
switch (choice)
|
||||
{
|
||||
case JUMP_DEAD_ROAD:
|
||||
{
|
||||
Connect(passed_point, passed_point->next->next->next);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
//设置点的 x 值
|
||||
void Route::SetPointX(Point* point, int8_t x)
|
||||
{
|
||||
point->x = x;
|
||||
}
|
||||
|
||||
|
||||
//设置点的 y 值
|
||||
void Route::SetPointY(Point* point, int8_t y)
|
||||
{
|
||||
point->y = y;
|
||||
}
|
||||
|
||||
|
||||
//设置点的 type 值
|
||||
void Route::SetPointType(Point* point, uint8_t type)
|
||||
{
|
||||
point->type = type;
|
||||
}
|
||||
|
||||
|
||||
//设置点的 next 值
|
||||
void Route::SetPointNext(Point* point, Point* next)
|
||||
{
|
||||
point->next = next;
|
||||
}
|
||||
|
||||
|
||||
//新建点
|
||||
Point* Route::CreatePoint(int8_t x, int8_t y, uint8_t type)
|
||||
{
|
||||
Point* point = new Point;
|
||||
|
||||
SetPointX(point, x);
|
||||
SetPointY(point, y);
|
||||
SetPointType(point, type);
|
||||
SetPointNext(point, NULL);
|
||||
|
||||
return point;
|
||||
}
|
||||
|
||||
|
||||
//连接 point_a 和 point_b，使 point_a->next == point_b
|
||||
//在主点链存在的情况下，point_a 和 point_b 必须属于主点链，否则可能出现不可预料的结果。函数不对 point_a 和 point_b 是否属于主点链进行检查
|
||||
//在主点链不存在的情况下，point_a 和 point_b 必须属于单向点链，否则可能出现不可预料的结果。函数不对 point_a 和 point_b 是否属于单向点链进行检查
|
||||
//若主点链为单向，point_a 在 point_b 之前，则 point_a 和 point_b 之间的点将被删除
|
||||
//若主点链为环形，head_point 不在 point_a point_b 之间，则 point_a 和 point_b 之间的点将被删除
|
||||
//若主点链为单向，point_a 在 point_b 之后，则 point_b 之前和 point_a 之后的点将被删除，point_b 成为 head_point，形成环形点链
|
||||
//若主点链为环形，head_point 在 point_b point_a 之间，则 point_b point_a 之间的点将被删除，point_b 成为 head_point
|
||||
//若主点链为单向，point_b head_point 均不为空，但 point_a 为空，则从 head_point 开始删除，并将 point_b 作为 head_point
|
||||
//若主点链为环形，point_b head_point 均不为空，但 point_a 为空，则从 head_point 开始删除，并将 point_b 作为 head_point，形成单向点链
|
||||
//若 point_a point_b 均不为空，但 head_point 为空（主点链不存在），则 point_a 成为 head_point，point_a 之前的点和 point_b 之前、之后的点不会被删除，point_a 之后的点会被删除
|
||||
//若 point_b 不为空，point_a head_point 均为空（主点链不存在），则 point_b 成为 head_point，point_b 之后的点不会被删除
|
||||
//若主点链为单向，point_a head_point 均不为空，但 point_b 为空，则从 point_a 的下一点开始删除，point_a 作为末尾点
|
||||
//若主点链为环形，point_a head_point 均不为空，但 point_b 为空，则从 point_a 的下一点开始删除，point_a 作为末尾点，形成单向点链
|
||||
//若 head_point 不为空，point_a point_b 均为空，则删除整个点链
|
||||
//若 point_a point_b head_point 均为空，不进行操作
|
||||
void Route::Connect(Point* point_a, Point* point_b)
|
||||
{
|
||||
if (point_b) //point_b 不为空
|
||||
{
|
||||
if (head_point) //head_point 不为空
|
||||
{
|
||||
Point* temp_point;
|
||||
Point* delete_point;
|
||||
|
||||
if (point_a) //point_a 不为空，则从 point_a 的下一点开始删除，并连接 point_a 和 point_b
|
||||
{
|
||||
temp_point = point_a->next;
|
||||
point_a->next = point_b;
|
||||
}
|
||||
else //point_a 为空，则从 head_point 开始删除，并将 point_b 作为 head_point
|
||||
{
|
||||
temp_point = point_b;
|
||||
|
||||
while (!temp_point)
|
||||
{
|
||||
if (temp_point->next == head_point) //为环形链表，将尾部 next 置空，使其变为单向链表
|
||||
temp_point->next = NULL;
|
||||
|
||||
temp_point = temp_point->next;
|
||||
}
|
||||
|
||||
temp_point = head_point;
|
||||
head_point = point_b;
|
||||
}
|
||||
|
||||
while (temp_point != point_b)
|
||||
{
|
||||
if (!temp_point) //point_a 在 point_b 之后，则从 head_point 开始删除，并将 point_b 作为 head_point
|
||||
{
|
||||
temp_point = head_point;
|
||||
head_point = point_b;
|
||||
|
||||
continue;
|
||||
}
|
||||
else if (temp_point == head_point) //主点链为环，且 head_point 在 point_b point_a 之间，则将 point_b 作为 head_point
|
||||
{
|
||||
head_point = point_b;
|
||||
}
|
||||
|
||||
delete_point = temp_point;
|
||||
temp_point = temp_point->next;
|
||||
delete delete_point;
|
||||
}
|
||||
}
|
||||
else if (point_a) //head_point 为空，point_a 不为空，则 point_a 作为 head_point，删除 point_a 之后的点
|
||||
{
|
||||
Point* temp_point;
|
||||
Point* delete_point;
|
||||
|
||||
temp_point = point_a->next;
|
||||
point_a->next = point_b;
|
||||
head_point = point_a;
|
||||
|
||||
while (temp_point && temp_point != point_a)
|
||||
{
|
||||
delete_point = temp_point;
|
||||
temp_point = temp_point->next;
|
||||
delete delete_point;
|
||||
}
|
||||
}
|
||||
else //head_point 为空，point_a 为空，则 point_b 作为 head_point
|
||||
{
|
||||
head_point = point_b;
|
||||
}
|
||||
}
|
||||
else //point_b 为空
|
||||
{
|
||||
if (head_point) //head_point 不为空
|
||||
{
|
||||
Point* temp_point;
|
||||
Point* delete_point;
|
||||
Point* former_head_point = head_point;
|
||||
|
||||
if (point_a) //point_a 不为空，则从 point_a 的下一点开始删除，point_a 作为末尾点
|
||||
{
|
||||
temp_point = point_a->next;
|
||||
point_a->next = NULL;
|
||||
}
|
||||
else //point_a 为空，则删除整个点链
|
||||
{
|
||||
temp_point = head_point;
|
||||
head_point = NULL;
|
||||
}
|
||||
|
||||
while (temp_point)
|
||||
{
|
||||
delete_point = temp_point;
|
||||
temp_point = temp_point->next;
|
||||
delete delete_point;
|
||||
|
||||
if (temp_point == former_head_point) //为环形链表，将尾部 next 置空，使其变为单向链表
|
||||
temp_point->next = NULL;
|
||||
}
|
||||
}
|
||||
else if (point_a) //head_point 为空，point_a 不为空，则 point_a 作为 head_point，删除point_a 之后的点
|
||||
{
|
||||
Point* temp_point;
|
||||
Point* delete_point;
|
||||
|
||||
temp_point = point_a->next;
|
||||
point_a->next = NULL;
|
||||
head_point = point_a;
|
||||
|
||||
while (temp_point)
|
||||
{
|
||||
delete_point = temp_point;
|
||||
temp_point = temp_point->next;
|
||||
delete delete_point;
|
||||
}
|
||||
}
|
||||
|
||||
//head_point 为空，point_a 为空，无需进行操作
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
//在 front_point 和 next_point 间插入新的点
|
||||
//front_point 和 next_point 必须属于主点链
|
||||
//将首先连接 front_point 和 next_point，具体规则参见 Connect 函数
|
||||
//若 front_point 为空，则新建的 point 为初始点
|
||||
//若 next_point 为空，则新建的 point 为末尾点
|
||||
//若 front_point 和 next_point 均为空，则删除整个点链，新建的 point 为初始点
|
||||
Point* Route::AddPoint(Point* front_point, int8_t x, int8_t y, uint8_t type, Point* next_point)
|
||||
{
|
||||
Point* point = CreatePoint(x, y, type);
|
||||
|
||||
//连接 front_point 和 next_point
|
||||
Connect(front_point, next_point);
|
||||
|
||||
if (front_point)
|
||||
front_point->next = point;
|
||||
else head_point = point; //若 front_point 为空，则新建的 point 为初始点
|
||||
|
||||
point->next = next_point;
|
||||
|
||||
return point;
|
||||
}
|
||||
|
||||
|
||||
//生成基本路径点链
|
||||
void Route::InitBaseRoute(void)
|
||||
{
|
||||
//基本路径数组
|
||||
const static int8_t base_route [][3] =
|
||||
{
|
||||
{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, NORMAL_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, NORMAL_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, NORMAL_POINT}
|
||||
};
|
||||
|
||||
//计算数组元素个数
|
||||
int route_amount = sizeof(base_route) / sizeof(base_route[0]);
|
||||
|
||||
//生成基本路径点链
|
||||
Point* temp_point = NULL;
|
||||
for (int i = 0; i < route_amount; i++)
|
||||
temp_point = AddPoint(temp_point, base_route[i][X], base_route[i][Y], base_route[i][TYPE]);
|
||||
|
||||
//生成环形点链
|
||||
Connect(temp_point, head_point);
|
||||
}
|
||||
@ -1,75 +0,0 @@
|
||||
#ifndef ROUTEMIXELEMENT_H
|
||||
#define ROUTEMIXELEMENT_H
|
||||
|
||||
|
||||
#include <Arduino.h>
|
||||
|
||||
//注释以关闭调试功能
|
||||
#define ROUTE_DEBUG
|
||||
|
||||
#ifdef ROUTE_DEBUG
|
||||
#define NeoSerialDebug NeoSerial3
|
||||
#endif
|
||||
|
||||
//坐标点操作定义
|
||||
#define NORMAL_POINT 0 //普通点
|
||||
#define CARRY_POINT 3 //携带点（从底盘）
|
||||
#define GETOUT_POINT 4 //释放点（从底盘）
|
||||
|
||||
//坐标点数组定义
|
||||
#define X 0
|
||||
#define Y 1
|
||||
#define TYPE 2
|
||||
|
||||
//更改路径选项
|
||||
#define JUMP_DEAD_ROAD 0
|
||||
|
||||
|
||||
//点结构
|
||||
struct Point
|
||||
{
|
||||
int8_t x;
|
||||
int8_t y;
|
||||
uint8_t type;
|
||||
|
||||
Point* next;
|
||||
};
|
||||
|
||||
|
||||
class Route
|
||||
{
|
||||
public:
|
||||
Route();
|
||||
|
||||
static Point GetPassedPoint(void);
|
||||
static Point GetNextPoint(void);
|
||||
static Point GetNextNextPoint(void);
|
||||
|
||||
static void SetPassedPoinType(uint8_t type);
|
||||
static void SetNextPointType(uint8_t type);
|
||||
static void SetNextNextPointType(uint8_t type);
|
||||
|
||||
static void UpdatePosition(void); //更新位置
|
||||
|
||||
static void ChangeRoute(uint8_t choice); //更改路径
|
||||
|
||||
private:
|
||||
static void SetPointX(Point* point, int8_t x); //设置点的 x 值
|
||||
static void SetPointY(Point* point, int8_t y); //设置点的 y 值
|
||||
static void SetPointType(Point* point, uint8_t type); //设置点的 type 值
|
||||
static void SetPointNext(Point* point, Point* next); //设置点的 next 值
|
||||
|
||||
static Point* CreatePoint(int8_t x, int8_t y, uint8_t type); //新建点
|
||||
|
||||
static void Connect(Point* point_a, Point* point_b); //连接 point_a point_b
|
||||
|
||||
static Point* AddPoint(Point* front_point, int8_t x, int8_t y, uint8_t type, Point* next_point = NULL); //在 front_point 和 next_point 间插入新的点
|
||||
|
||||
static void InitBaseRoute(void); //生成基本路径点链
|
||||
|
||||
static Point* head_point; //头部点
|
||||
static Point* passed_point; //经过的点
|
||||
};
|
||||
|
||||
|
||||
#endif
|
||||
@ -1,5 +1,4 @@
|
||||
#include <Arduino.h>
|
||||
#include <Wire.h>
|
||||
|
||||
#include "sensorMixElement.h"
|
||||
|
||||
@ -10,42 +9,13 @@
|
||||
|
||||
//静态变量
|
||||
int Sensor::gray_1_gate = DEFAULT_GRAY_1_GATE;
|
||||
int Sensor::gray_2_gate = DEFAULT_GRAY_2_GATE;
|
||||
int Sensor::gray_3_gate = DEFAULT_GRAY_3_GATE;
|
||||
int Sensor::gray_4_gate = DEFAULT_GRAY_4_GATE;
|
||||
int Sensor::gray_5_gate = DEFAULT_GRAY_5_GATE;
|
||||
int Sensor::gray_6_gate = DEFAULT_GRAY_6_GATE;
|
||||
int Sensor::gray_7_gate = DEFAULT_GRAY_7_GATE;
|
||||
|
||||
|
||||
Sensor::Sensor()
|
||||
{
|
||||
pinMode(BUTTON_1_OUT, INPUT_PULLUP);
|
||||
pinMode(BUTTON_2_OUT, INPUT_PULLUP);
|
||||
|
||||
pinMode(GRAY_1_VCC, OUTPUT);
|
||||
digitalWrite(GRAY_1_VCC, HIGH);
|
||||
|
||||
pinMode(GRAY_2_VCC, OUTPUT);
|
||||
digitalWrite(GRAY_2_VCC, HIGH);
|
||||
|
||||
pinMode(GRAY_3_VCC, OUTPUT);
|
||||
digitalWrite(GRAY_3_VCC, HIGH);
|
||||
|
||||
pinMode(GRAY_4_VCC, OUTPUT);
|
||||
digitalWrite(GRAY_4_VCC, HIGH);
|
||||
|
||||
pinMode(GRAY_5_VCC, OUTPUT);
|
||||
digitalWrite(GRAY_5_VCC, HIGH);
|
||||
|
||||
pinMode(GRAY_6_VCC, OUTPUT);
|
||||
digitalWrite(GRAY_6_VCC, HIGH);
|
||||
|
||||
pinMode(GRAY_7_VCC, OUTPUT);
|
||||
digitalWrite(GRAY_7_VCC, HIGH);
|
||||
|
||||
pinMode(BUTTON_1_VCC, OUTPUT);
|
||||
digitalWrite(BUTTON_1_VCC, HIGH);
|
||||
pinMode(BUTTON_3_OUT, INPUT_PULLUP);
|
||||
|
||||
pinMode(BUTTON_2_VCC, OUTPUT);
|
||||
digitalWrite(BUTTON_2_VCC, HIGH);
|
||||
@ -57,51 +27,11 @@ void Sensor::SetGrayGate(uint8_t gray_sensor_num, int gate)
|
||||
{
|
||||
switch (gray_sensor_num)
|
||||
{
|
||||
case GRAY_1: gray_1_gate = gate; break;
|
||||
case GRAY_2: gray_2_gate = gate; break;
|
||||
case GRAY_3: gray_3_gate = gate; break;
|
||||
case GRAY_4: gray_4_gate = gate; break;
|
||||
case GRAY_5: gray_5_gate = gate; break;
|
||||
case GRAY_6: gray_6_gate = gate; break;
|
||||
case GRAY_7: gray_7_gate = gate;
|
||||
case GRAY_1: gray_1_gate = gate;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
//使灰度传感器闪烁
|
||||
void Sensor::FlashGraySensor(uint8_t gray_sensor_num)
|
||||
{
|
||||
switch (gray_sensor_num)
|
||||
{
|
||||
case GRAY_1: digitalWrite(GRAY_1_VCC, LOW); break;
|
||||
case GRAY_2: digitalWrite(GRAY_2_VCC, LOW); break;
|
||||
case GRAY_3: digitalWrite(GRAY_3_VCC, LOW); break;
|
||||
case GRAY_4: digitalWrite(GRAY_4_VCC, LOW); break;
|
||||
case GRAY_5: digitalWrite(GRAY_5_VCC, LOW); break;
|
||||
case GRAY_6: digitalWrite(GRAY_6_VCC, LOW); break;
|
||||
case GRAY_7: digitalWrite(GRAY_7_VCC, LOW);
|
||||
}
|
||||
|
||||
#ifdef SENSOR_DEBUG
|
||||
//调试输出闪烁信息
|
||||
switch (gray_sensor_num)
|
||||
{
|
||||
case GRAY_1: NeoSerialDebug.println(F("#SENSOR: FLASH GRAY_1 NOW!")); break;
|
||||
case GRAY_2: NeoSerialDebug.println(F("#SENSOR: FLASH GRAY_2 NOW!")); break;
|
||||
case GRAY_3: NeoSerialDebug.println(F("#SENSOR: FLASH GRAY_3 NOW!")); break;
|
||||
case GRAY_4: NeoSerialDebug.println(F("#SENSOR: FLASH GRAY_4 NOW!")); break;
|
||||
case GRAY_5: NeoSerialDebug.println(F("#SENSOR: FLASH GRAY_5 NOW!")); break;
|
||||
case GRAY_6: NeoSerialDebug.println(F("#SENSOR: FLASH GRAY_6 NOW!")); break;
|
||||
case GRAY_7: NeoSerialDebug.println(F("#SENSOR: FLASH GRAY_7 NOW!"));
|
||||
}
|
||||
#endif
|
||||
|
||||
delay(GRAY_FLASH_TIME);
|
||||
Sensor();
|
||||
delay(GRAY_FLASH_TIME);
|
||||
}
|
||||
|
||||
|
||||
bool Sensor::IsWhite(uint8_t gray_sensor_num)
|
||||
{
|
||||
uint8_t gray_out_pin;
|
||||
@ -110,13 +40,7 @@ bool Sensor::IsWhite(uint8_t gray_sensor_num)
|
||||
|
||||
switch (gray_sensor_num)
|
||||
{
|
||||
case GRAY_1: gray_out_pin = GRAY_1_OUT; gray_gate = gray_1_gate; break;
|
||||
case GRAY_2: gray_out_pin = GRAY_2_OUT; gray_gate = gray_2_gate; break;
|
||||
case GRAY_3: gray_out_pin = GRAY_3_OUT; gray_gate = gray_3_gate; break;
|
||||
case GRAY_4: gray_out_pin = GRAY_4_OUT; gray_gate = gray_4_gate; break;
|
||||
case GRAY_5: gray_out_pin = GRAY_5_OUT; gray_gate = gray_5_gate; break;
|
||||
case GRAY_6: gray_out_pin = GRAY_6_OUT; gray_gate = gray_6_gate; break;
|
||||
case GRAY_7: gray_out_pin = GRAY_7_OUT; gray_gate = gray_7_gate;
|
||||
case GRAY_1: gray_out_pin = GRAY_1_OUT; gray_gate = gray_1_gate;
|
||||
}
|
||||
|
||||
gray_val = analogRead(gray_out_pin);
|
||||
@ -125,13 +49,7 @@ bool Sensor::IsWhite(uint8_t gray_sensor_num)
|
||||
//调试输出灰度值
|
||||
switch (gray_sensor_num)
|
||||
{
|
||||
case GRAY_1: NeoSerialDebug.print(F("#SENSOR: GRAY_1 and gate_val: ")); break;
|
||||
case GRAY_2: NeoSerialDebug.print(F("#SENSOR: GRAY_2 and gate_val: ")); break;
|
||||
case GRAY_3: NeoSerialDebug.print(F("#SENSOR: GRAY_3 and gate_val: ")); break;
|
||||
case GRAY_4: NeoSerialDebug.print(F("#SENSOR: GRAY_4 and gate_val: ")); break;
|
||||
case GRAY_5: NeoSerialDebug.print(F("#SENSOR: GRAY_5 and gate_val: ")); break;
|
||||
case GRAY_6: NeoSerialDebug.print(F("#SENSOR: GRAY_6 and gate_val: ")); break;
|
||||
case GRAY_7: NeoSerialDebug.print(F("#SENSOR: GRAY_7 and gate_val: "));
|
||||
case GRAY_1: NeoSerialDebug.print(F("#SENSOR: GRAY_1 and gate_val: "));
|
||||
}
|
||||
|
||||
NeoSerialDebug.print(gray_val);
|
||||
@ -145,56 +63,6 @@ bool Sensor::IsWhite(uint8_t gray_sensor_num)
|
||||
}
|
||||
|
||||
|
||||
//灰度传感器灰度值偏离比例，即 (gray_gate - gray_val) / gray_gate
|
||||
float Sensor::GrayDeviationRate(uint8_t gray_sensor_num)
|
||||
{
|
||||
uint8_t gray_out_pin;
|
||||
int gray_val;
|
||||
int gray_gate;
|
||||
float deviarion_rate;
|
||||
|
||||
switch (gray_sensor_num)
|
||||
{
|
||||
case GRAY_1: gray_out_pin = GRAY_1_OUT; gray_gate = gray_1_gate; break;
|
||||
case GRAY_2: gray_out_pin = GRAY_2_OUT; gray_gate = gray_2_gate; break;
|
||||
case GRAY_3: gray_out_pin = GRAY_3_OUT; gray_gate = gray_3_gate; break;
|
||||
case GRAY_4: gray_out_pin = GRAY_4_OUT; gray_gate = gray_4_gate; break;
|
||||
case GRAY_5: gray_out_pin = GRAY_5_OUT; gray_gate = gray_5_gate; break;
|
||||
case GRAY_6: gray_out_pin = GRAY_6_OUT; gray_gate = gray_6_gate; break;
|
||||
case GRAY_7: gray_out_pin = GRAY_7_OUT; gray_gate = gray_7_gate;
|
||||
}
|
||||
|
||||
gray_val = analogRead(gray_out_pin);
|
||||
|
||||
#ifdef SENSOR_DEBUG
|
||||
//调试输出灰度值
|
||||
switch (gray_sensor_num)
|
||||
{
|
||||
case GRAY_1: NeoSerialDebug.print(F("#SENSOR: GRAY_1 and gate_val: ")); break;
|
||||
case GRAY_2: NeoSerialDebug.print(F("#SENSOR: GRAY_2 and gate_val: ")); break;
|
||||
case GRAY_3: NeoSerialDebug.print(F("#SENSOR: GRAY_3 and gate_val: ")); break;
|
||||
case GRAY_4: NeoSerialDebug.print(F("#SENSOR: GRAY_4 and gate_val: ")); break;
|
||||
case GRAY_5: NeoSerialDebug.print(F("#SENSOR: GRAY_5 and gate_val: ")); break;
|
||||
case GRAY_6: NeoSerialDebug.print(F("#SENSOR: GRAY_6 and gate_val: ")); break;
|
||||
case GRAY_7: NeoSerialDebug.print(F("#SENSOR: GRAY_7 and gate_val: "));
|
||||
}
|
||||
|
||||
NeoSerialDebug.print(gray_val);
|
||||
NeoSerialDebug.print(F(" "));
|
||||
NeoSerialDebug.print(gray_gate);
|
||||
#endif
|
||||
|
||||
deviarion_rate = (float)gray_val / gray_gate;
|
||||
|
||||
#ifdef SENSOR_DEBUG
|
||||
NeoSerialDebug.print(F(" deviarion_rate: "));
|
||||
NeoSerialDebug.println(deviarion_rate);
|
||||
#endif
|
||||
|
||||
return deviarion_rate;
|
||||
}
|
||||
|
||||
|
||||
//碰撞传感器（开关）判断是否闭合
|
||||
bool Sensor::IsPushed(uint8_t button_num)
|
||||
{
|
||||
@ -203,7 +71,9 @@ bool Sensor::IsPushed(uint8_t button_num)
|
||||
|
||||
if (button_num == BUTTON_1)
|
||||
button_out_pin = BUTTON_1_OUT;
|
||||
else button_out_pin = BUTTON_2_OUT;
|
||||
else if (button_num == BUTTON_2)
|
||||
button_out_pin = BUTTON_2_OUT;
|
||||
else button_out_pin = BUTTON_3_OUT;
|
||||
|
||||
button_val = digitalRead(button_out_pin);
|
||||
|
||||
@ -211,14 +81,16 @@ bool Sensor::IsPushed(uint8_t button_num)
|
||||
//调试输出按钮状态
|
||||
if (button_num == BUTTON_1)
|
||||
NeoSerialDebug.print(F("#SENSOR: BUTTON_1: "));
|
||||
else NeoSerialDebug.print(F("#SENSOR: BUTTON_2: "));
|
||||
else if (button_num == BUTTON_2)
|
||||
NeoSerialDebug.print(F("#SENSOR: BUTTON_2: "));
|
||||
else NeoSerialDebug.print(F("#SENSOR: BUTTON_3: "));
|
||||
|
||||
if (button_val == LOW)
|
||||
if (button_val == HIGH)
|
||||
NeoSerialDebug.println(F("pushed"));
|
||||
else NeoSerialDebug.println(F("released"));
|
||||
#endif
|
||||
|
||||
if (button_val == LOW)
|
||||
if (button_val == HIGH)
|
||||
return true;
|
||||
else return false;
|
||||
}
|
||||
@ -6,59 +6,30 @@
|
||||
#define SENSOR_DEBUG
|
||||
|
||||
#ifdef SENSOR_DEBUG
|
||||
#define NeoSerialDebug NeoSerial3
|
||||
#define NeoSerialDebug NeoSerial
|
||||
#endif
|
||||
|
||||
//灰度传感器 OUT 接口定义
|
||||
#define GRAY_1_OUT A0
|
||||
#define GRAY_2_OUT A1
|
||||
#define GRAY_3_OUT A2
|
||||
#define GRAY_4_OUT A3
|
||||
#define GRAY_5_OUT A4
|
||||
#define GRAY_6_OUT A5
|
||||
#define GRAY_7_OUT A6
|
||||
|
||||
//灰度传感器 VCC 接口定义
|
||||
#define GRAY_1_VCC 48
|
||||
#define GRAY_2_VCC 49
|
||||
#define GRAY_3_VCC 50
|
||||
#define GRAY_4_VCC 51
|
||||
#define GRAY_5_VCC 52
|
||||
#define GRAY_6_VCC 53
|
||||
#define GRAY_7_VCC 47
|
||||
#define GRAY_1_OUT A5
|
||||
|
||||
//灰度传感器临界值
|
||||
#define DEFAULT_GRAY_1_GATE 900
|
||||
#define DEFAULT_GRAY_2_GATE 900
|
||||
#define DEFAULT_GRAY_3_GATE 850
|
||||
#define DEFAULT_GRAY_4_GATE 850
|
||||
#define DEFAULT_GRAY_5_GATE 900
|
||||
#define DEFAULT_GRAY_6_GATE 880
|
||||
#define DEFAULT_GRAY_7_GATE 690
|
||||
|
||||
//灰度传感器闪烁时间
|
||||
#define GRAY_FLASH_TIME 200
|
||||
#define DEFAULT_GRAY_1_GATE 500
|
||||
|
||||
//灰度传感器标识定义
|
||||
#define GRAY_1 0
|
||||
#define GRAY_2 1
|
||||
#define GRAY_3 2
|
||||
#define GRAY_4 3
|
||||
#define GRAY_5 4
|
||||
#define GRAY_6 5
|
||||
#define GRAY_7 6
|
||||
|
||||
//碰撞传感器 OUT 接口定义
|
||||
#define BUTTON_1_OUT 2
|
||||
#define BUTTON_2_OUT 3
|
||||
#define BUTTON_1_OUT A0
|
||||
#define BUTTON_2_OUT A1
|
||||
#define BUTTON_3_OUT A4
|
||||
|
||||
//碰撞传感器 VCC 接口定义
|
||||
#define BUTTON_1_VCC 45
|
||||
#define BUTTON_2_VCC 46
|
||||
#define BUTTON_2_VCC A2
|
||||
|
||||
//碰撞传感器标识定义
|
||||
#define BUTTON_1 0
|
||||
#define BUTTON_2 1
|
||||
#define BUTTON_3 2
|
||||
|
||||
|
||||
class Sensor
|
||||
@ -69,27 +40,15 @@ public:
|
||||
//设置灰度传感器临界值
|
||||
static void SetGrayGate(uint8_t gray_sensor_num, int gate);
|
||||
|
||||
//使灰度传感器闪烁
|
||||
static void FlashGraySensor(uint8_t gray_sensor_num);
|
||||
|
||||
//灰度传感器判断下方是否为白色
|
||||
static bool IsWhite(uint8_t gray_sensor_num);
|
||||
|
||||
//灰度传感器灰度值偏离比例，即 (gray_gate - gray_val) / gray_gate
|
||||
static float GrayDeviationRate(uint8_t gray_sensor_num);
|
||||
|
||||
//碰撞传感器（开关）判断是否闭合
|
||||
static bool IsPushed(uint8_t button_num);
|
||||
|
||||
private:
|
||||
//灰度传感器临界值
|
||||
static int gray_1_gate;
|
||||
static int gray_2_gate;
|
||||
static int gray_3_gate;
|
||||
static int gray_4_gate;
|
||||
static int gray_5_gate;
|
||||
static int gray_6_gate;
|
||||
static int gray_7_gate;
|
||||
};
|
||||
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user