diff --git a/MixElement/MemoryUsage.cpp b/MixElement/MemoryUsage.cpp new file mode 100644 index 0000000..36a66f8 --- /dev/null +++ b/MixElement/MemoryUsage.cpp @@ -0,0 +1,115 @@ +/* +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(); +} \ No newline at end of file diff --git a/MixElement/MemoryUsage.h b/MixElement/MemoryUsage.h new file mode 100644 index 0000000..74c8042 --- /dev/null +++ b/MixElement/MemoryUsage.h @@ -0,0 +1,190 @@ +/* +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 +#include + +#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 + + 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 diff --git a/MixElement/MixElement.ino b/MixElement/MixElement.ino new file mode 100644 index 0000000..06f9603 --- /dev/null +++ b/MixElement/MixElement.ino @@ -0,0 +1,614 @@ +#include +#include +#include + +#include "routeMixElement.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 670 +//（后退转）从触碰白线到开始转向延时 +#define BEFORE_BACKTURN_DELAY 500 + +//开始转到开始检测是否摆正的延时 +#define BEFORE_CHECK_STRAIGHT_DELAY 700 +//*************************************************************************************** + + +//****************************************全局变量**************************************** +//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; +//*************************************************************************************** + + +//****************************************自定函数**************************************** +//设置接口低电平作为额外地 +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 + +#ifdef MIXELE_DEBUG + +#include "MemoryUsage.h" + +#define NeoSerialDebug NeoSerial3 +#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 + NeoSerialDebug.begin(DEBUG_BAUT_RATE); + NeoSerialDebug.println(F("#MIXELE: ======================setup()=====================")); + SRamDisplay(); + #endif + + SetGNDPins(); + + move.Stop(); + + radio.SetHandleMessageFunction(HandleMessage); + radio.BeginTransmit(); + + //从 EEPROM 读取数据 + ReadEEPROM(); + + //在开始运行前依次检测各灰度传感器下方黑白是否正常 + CheckGrayStatus(); + + //前往 x, 0 + //沿线直行，到后端传感器接触下一条线离开函数 + LineForward(BACK_END); + + //已越过 x, 0 正式进入循环 +} + + +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 +} + + +//设置接口低电平作为额外地 +void SetGNDPins(void) +{ + uint8_t pin_num = sizeof(gnd_pins) / sizeof(uint8_t); + + for (uint8_t i = 0; i < pin_num; i++) + { + 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); +} \ No newline at end of file diff --git a/MixElement/moveMixElement.cpp b/MixElement/moveMixElement.cpp new file mode 100644 index 0000000..f9ab468 --- /dev/null +++ b/MixElement/moveMixElement.cpp @@ -0,0 +1,264 @@ +#include + +#include "moveMixElement.h" + +#ifdef MOVE_DEBUG +#include +#endif + + +//静态变量 +float Move::global_speed_rate = DEFAULT_GLOBAL_SPEED_RATE; //全局速度比率 +uint8_t Move::current_direction = STOP; //当前运动状态 +float Move::current_speed_rate = 0.0; //当前运动速度比率 +float Move::current_turn_speed_rate; //当前转向时一侧减速的比率 + + +Move::Move() {} + + +Move::Move(float global_speed_rate) +{ + Move::global_speed_rate = global_speed_rate; +} + + +//设置全局速度比率 +void Move::SetGlobalSpeedRate(float global_speed_rate) +{ + Move::global_speed_rate = global_speed_rate; +} + + +//获取当前运动方向 +uint8_t Move::GetCurrentMove(void) +{ + return current_direction; +} + + +//获取当前运动速度比率 +float Move::GetCurrentSpeedRate(void) +{ + return current_speed_rate; +} + + +//获取当前转向时一侧减速的比率 +float Move::GetCurrentTurnSpeedRate(void) +{ + return current_turn_speed_rate; +} + + +//控制某个轮子转动 +void Move::Wheel(uint8_t wheel, uint8_t rotation, float speed_rate) +{ + uint8_t pin_in1, pin_in2, pin_enx; + + switch (wheel) + { + case LEFT_A_WHEEL: + { + pin_in1 = LEFT_L298N_IN1; + pin_in2 = LEFT_L298N_IN2; + pin_enx = LEFT_L298N_ENA; + } break; + + case LEFT_B_WHEEL: + { + pin_in1 = LEFT_L298N_IN3; + pin_in2 = LEFT_L298N_IN4; + pin_enx = LEFT_L298N_ENB; + } break; + + case RIGHT_A_WHEEL: + { + pin_in1 = RIGHT_L298N_IN1; + pin_in2 = RIGHT_L298N_IN2; + pin_enx = RIGHT_L298N_ENA; + } break; + + case RIGHT_B_WHEEL: + { + pin_in1 = RIGHT_L298N_IN3; + pin_in2 = RIGHT_L298N_IN4; + pin_enx = RIGHT_L298N_ENB; + } + } + + if (rotation == STOP_ROTATION) //停止转动 + { + digitalWrite(pin_in1, LOW); + digitalWrite(pin_in2, LOW); + return; //结束函数 + } + + analogWrite(pin_enx, abs(speed_rate) * global_speed_rate * 255.0); //设置 PWM 波，即转速 + + if ((rotation == FORWARD_ROTATION && speed_rate >= 0) || (rotation == BACKWARD_ROTATION && speed_rate < 0)) //向前转动 + { + digitalWrite(pin_in1, LOW); + digitalWrite(pin_in2, HIGH); + } + else //向后转动 + { + digitalWrite(pin_in1, HIGH); + digitalWrite(pin_in2, LOW); + } +} + + +//前进 +void Move::Forward(float speed_rate) +{ + Wheel(LEFT_A_WHEEL, FORWARD_ROTATION, speed_rate); + Wheel(LEFT_B_WHEEL, FORWARD_ROTATION, speed_rate); + 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 +} + + +//后退 +void Move::Backward(float speed_rate) +{ + Wheel(LEFT_A_WHEEL, BACKWARD_ROTATION, speed_rate); + Wheel(LEFT_B_WHEEL, BACKWARD_ROTATION, speed_rate); + Wheel(RIGHT_A_WHEEL, BACKWARD_ROTATION, speed_rate); + Wheel(RIGHT_B_WHEEL, BACKWARD_ROTATION, 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 +} + + +//向前左转 +void Move::ForLeftward(float speed_rate, float turn_speed_rate) +{ + Wheel(LEFT_A_WHEEL, FORWARD_ROTATION, turn_speed_rate * speed_rate); + Wheel(LEFT_B_WHEEL, FORWARD_ROTATION, turn_speed_rate * speed_rate); + Wheel(RIGHT_A_WHEEL, FORWARD_ROTATION, speed_rate); + Wheel(RIGHT_B_WHEEL, FORWARD_ROTATION, 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 +} + + +//向前右转 +void Move::ForRightward(float speed_rate, float turn_speed_rate) +{ + Wheel(LEFT_A_WHEEL, FORWARD_ROTATION, speed_rate); + Wheel(LEFT_B_WHEEL, FORWARD_ROTATION, speed_rate); + Wheel(RIGHT_A_WHEEL, FORWARD_ROTATION, turn_speed_rate * speed_rate); + Wheel(RIGHT_B_WHEEL, FORWARD_ROTATION, turn_speed_rate * 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 +} + + +//向后左转 +void Move::BackLeftward(float speed_rate, float turn_speed_rate) +{ + Wheel(LEFT_A_WHEEL, BACKWARD_ROTATION, turn_speed_rate * speed_rate); + Wheel(LEFT_B_WHEEL, BACKWARD_ROTATION, turn_speed_rate * speed_rate); + Wheel(RIGHT_A_WHEEL, BACKWARD_ROTATION, speed_rate); + Wheel(RIGHT_B_WHEEL, BACKWARD_ROTATION, 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 +} + + +//向后右转 +void Move::BackRightward(float speed_rate, float turn_speed_rate) +{ + Wheel(LEFT_A_WHEEL, BACKWARD_ROTATION, speed_rate); + Wheel(LEFT_B_WHEEL, BACKWARD_ROTATION, speed_rate); + Wheel(RIGHT_A_WHEEL, BACKWARD_ROTATION, turn_speed_rate * speed_rate); + Wheel(RIGHT_B_WHEEL, BACKWARD_ROTATION, turn_speed_rate * 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 +} + + +//制动 +void Move::Stop(void) +{ + Wheel(LEFT_A_WHEEL, STOP_ROTATION); + Wheel(LEFT_B_WHEEL, STOP_ROTATION); + Wheel(RIGHT_A_WHEEL, STOP_ROTATION); + Wheel(RIGHT_B_WHEEL, STOP_ROTATION); + + current_direction = STOP; + current_speed_rate = 0; + current_turn_speed_rate = 1.0; + + #ifdef MOVE_DEBUG + //调试输出制动状态 + NeoSerialDebug.println(F("#MOVE: Move Stop")); + #endif +} + + +//向某方向运动 +void Move::MoveDirection(uint8_t direction, float speed_rate, float turn_speed_rate) +{ + switch (direction) + { + case FORWARD: Forward(speed_rate); break; + case BACKWARD: Backward(speed_rate); break; + case FORLEFTWARD: ForLeftward(speed_rate, turn_speed_rate); break; + case FORRIGHTWARD: ForRightward(speed_rate, turn_speed_rate); break; + case BACKLEFTWARD: BackLeftward(speed_rate, turn_speed_rate); break; + case BACKRIGHTWARD: BackRightward(speed_rate, turn_speed_rate); break; + case STOP: Stop(); + } +} \ No newline at end of file diff --git a/MixElement/moveMixElement.h b/MixElement/moveMixElement.h new file mode 100644 index 0000000..1bb51f2 --- /dev/null +++ b/MixElement/moveMixElement.h @@ -0,0 +1,87 @@ +#ifndef MOVEMIXELEMENT_H +#define MOVEMIXELEMENT_H + + +//注释以关闭调试功能 +#define MOVE_DEBUG + +#ifdef MOVE_DEBUG +#define NeoSerialDebug NeoSerial +#endif + +//轮胎定义 +#define LEFT_A_WHEEL 0 +#define LEFT_B_WHEEL 1 +#define RIGHT_A_WHEEL 2 +#define RIGHT_B_WHEEL 3 + +//轮胎旋转方向定义，包括停止 +#define FORWARD_ROTATION 0 +#define BACKWARD_ROTATION 1 +#define STOP_ROTATION 6 + +//运动方向、状态定义，与轮胎旋转方向定义兼容 +#define FORWARD 0 +#define BACKWARD 1 +#define FORLEFTWARD 2 +#define FORRIGHTWARD 3 +#define BACKLEFTWARD 4 +#define BACKRIGHTWARD 5 +#define STOP 6 + +//默认全局速度比率 +#define DEFAULT_GLOBAL_SPEED_RATE 1.0 + +//默认转向时一侧减速的比率 +#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 + +//右侧 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 + + +class Move +{ +public: + Move(); + Move(float global_speed_rate); + + static void SetGlobalSpeedRate(float global_speed_rate); //设置全局速度比率 + + static uint8_t GetCurrentMove(void); //获取当前运动方向 + static float GetCurrentSpeedRate(void); //获取当前运动速度比率 + static float GetCurrentTurnSpeedRate(void); //获取当前转向时一侧减速的比率 + + static void Wheel(uint8_t wheel, uint8_t rotation, float speed_rate = 1.0); //控制某个轮子转动 + + static void Forward(float speed_rate = 1.0); //前进 + static void Backward(float speed_rate = 1.0); //后退 + static void ForLeftward(float speed_rate = 1.0, float turn_speed_rate = DEFAULT_TRUN_SPEED_RATE); //向前左转 + static void ForRightward(float speed_rate = 1.0, float turn_speed_rate = DEFAULT_TRUN_SPEED_RATE); //向前右转 + static void BackLeftward(float speed_rate = 1.0, float turn_speed_rate = DEFAULT_TRUN_SPEED_RATE); //向后左转 + static void BackRightward(float speed_rate = 1.0, float turn_speed_rate = DEFAULT_TRUN_SPEED_RATE); //向后右转 + static void Stop(void); //制动 + + static void MoveDirection(uint8_t direction, float speed_rate = 1.0, float turn_speed_rate = DEFAULT_TRUN_SPEED_RATE); //向某方向运动 + +private: + static float global_speed_rate; //全局速度比率 + static uint8_t current_direction; //当前运动状态 + static float current_speed_rate; //当前运动速度比率 + static float current_turn_speed_rate; //当前转向时一侧减速的比率 +}; + + +#endif \ No newline at end of file diff --git a/MixElement/radioMixElement.cpp b/MixElement/radioMixElement.cpp new file mode 100644 index 0000000..cd56906 --- /dev/null +++ b/MixElement/radioMixElement.cpp @@ -0,0 +1,226 @@ +#include +#include + +#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; +} \ No newline at end of file diff --git a/MixElement/radioMixElement.h b/MixElement/radioMixElement.h new file mode 100644 index 0000000..afc0c98 --- /dev/null +++ b/MixElement/radioMixElement.h @@ -0,0 +1,69 @@ +#ifndef RADIOMIXELEMENT_H +#define RADIOMIXELEMENT_H + + +#include + +//注释以关闭调试功能 +#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 \ No newline at end of file diff --git a/MixElement/routeMixElement.cpp b/MixElement/routeMixElement.cpp new file mode 100644 index 0000000..d046456 --- /dev/null +++ b/MixElement/routeMixElement.cpp @@ -0,0 +1,329 @@ +#include + +#include "routeMixElement.h" + + +#ifdef ROUTE_DEBUG +#include +#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); +} \ No newline at end of file diff --git a/MixElement/routeMixElement.h b/MixElement/routeMixElement.h new file mode 100644 index 0000000..4ac9646 --- /dev/null +++ b/MixElement/routeMixElement.h @@ -0,0 +1,75 @@ +#ifndef ROUTEMIXELEMENT_H +#define ROUTEMIXELEMENT_H + + +#include + +//注释以关闭调试功能 +#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 \ No newline at end of file diff --git a/MixElement/sensorMixElement.cpp b/MixElement/sensorMixElement.cpp new file mode 100644 index 0000000..563b02d --- /dev/null +++ b/MixElement/sensorMixElement.cpp @@ -0,0 +1,224 @@ +#include +#include + +#include "sensorMixElement.h" + +#ifdef SENSOR_DEBUG +#include +#endif + + +//静态变量 +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_2_VCC, OUTPUT); + digitalWrite(BUTTON_2_VCC, HIGH); +} + + +//设置灰度传感器临界值 +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; + } +} + + +//使灰度传感器闪烁 +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; + int gray_val; + int gray_gate; + + 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.println(gray_gate); + #endif + + if (gray_val > gray_gate) + return true; + else return false; +} + + +//灰度传感器灰度值偏离比例，即 (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) +{ + uint8_t button_out_pin; + int button_val; + + if (button_num == BUTTON_1) + button_out_pin = BUTTON_1_OUT; + else button_out_pin = BUTTON_2_OUT; + + button_val = digitalRead(button_out_pin); + + #ifdef SENSOR_DEBUG + //调试输出按钮状态 + if (button_num == BUTTON_1) + NeoSerialDebug.print(F("#SENSOR: BUTTON_1: ")); + else NeoSerialDebug.print(F("#SENSOR: BUTTON_2: ")); + + if (button_val == LOW) + NeoSerialDebug.println(F("pushed")); + else NeoSerialDebug.println(F("released")); + #endif + + if (button_val == LOW) + return true; + else return false; +} \ No newline at end of file diff --git a/MixElement/sensorMixElement.h b/MixElement/sensorMixElement.h new file mode 100644 index 0000000..fe5ae3f --- /dev/null +++ b/MixElement/sensorMixElement.h @@ -0,0 +1,96 @@ +#ifndef SENSORMIXELEMENT_H +#define SENSORMIXELEMENT_H + + +//注释以关闭调试功能 +#define SENSOR_DEBUG + +#ifdef SENSOR_DEBUG +#define NeoSerialDebug NeoSerial3 +#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 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 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 + +//碰撞传感器 VCC 接口定义 +#define BUTTON_1_VCC 45 +#define BUTTON_2_VCC 46 + +//碰撞传感器标识定义 +#define BUTTON_1 0 +#define BUTTON_2 1 + + +class Sensor +{ +public: + Sensor(); + + //设置灰度传感器临界值 + 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; +}; + + +#endif \ No newline at end of file