forked from StopPointTeam/TaiChi
修正多出错误
This commit is contained in:
parent
41fa2736f6
commit
987db4d8e9
@ -92,24 +92,24 @@ void SRamDisplay(void)
|
||||
|
||||
available -= data_size + bss_size + heap_size + stack_size;
|
||||
|
||||
NeoSerial.print(F("+----------------+ ")); NeoSerial.print((int)&__data_start); NeoSerial.println(" (__data_start)");
|
||||
NeoSerial.print(F("+ data +")); NeoSerial.println();
|
||||
NeoSerial.print(F("+ variables + size = ")); NeoSerial.println(data_size);
|
||||
NeoSerial.print(F("+----------------+ ")); NeoSerial.print((int)&__data_end); NeoSerial.println(" (__data_end / __bss_start)");
|
||||
NeoSerial.print(F("+ bss +")); NeoSerial.println();
|
||||
NeoSerial.print(F("+ variables + size = ")); NeoSerial.println(bss_size);
|
||||
NeoSerial.print(F("+----------------+ ")); NeoSerial.print((int)&__bss_end); NeoSerial.println(" (__bss_end / __heap_start)");
|
||||
NeoSerial.print(F("+ heap + size = ")); NeoSerial.println(heap_size);
|
||||
NeoSerial.print(F("+----------------+ ")); NeoSerial.print((int)heap_end); NeoSerial.println(" (__brkval if not 0, or __heap_start)");
|
||||
NeoSerial.print(F("+ +")); NeoSerial.println();
|
||||
NeoSerial.print(F("+ +")); NeoSerial.println();
|
||||
NeoSerial.print(F("+ FREE RAM + size = ")); NeoSerial.println(available);
|
||||
NeoSerial.print(F("+ +")); NeoSerial.println();
|
||||
NeoSerial.print(F("+ +")); NeoSerial.println();
|
||||
NeoSerial.print(F("+----------------+ ")); NeoSerial.print((int)SP); NeoSerial.println(" (SP)");
|
||||
NeoSerial.print(F("+ stack + size = ")); NeoSerial.println(stack_size);
|
||||
NeoSerial.print(F("+----------------+ ")); NeoSerial.print((int)RAMEND); NeoSerial.println(" (RAMEND / __stack)");
|
||||
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)");
|
||||
|
||||
NeoSerial.println();
|
||||
NeoSerial.println();
|
||||
NeoSerialDebug.println();
|
||||
NeoSerialDebug.println();
|
||||
}
|
||||
@ -23,6 +23,8 @@ Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
|
||||
#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 .
|
||||
@ -119,27 +121,27 @@ extern uint8_t *__bss_end;
|
||||
// Memory addresses
|
||||
//
|
||||
|
||||
/// Print data start on NeoSerial console.
|
||||
#define MEMORY_PRINT_START { NeoSerial.print(F("Data start:")); NeoSerial.println((int) &__data_start); }
|
||||
/// Print data end / heap start on NeoSerial console.
|
||||
#define MEMORY_PRINT_HEAPSTART { NeoSerial.print(F("Heap start:")); NeoSerial.println((int)&__heap_start); }
|
||||
/// Print heap end / free ram area on NeoSerial console.
|
||||
#define MEMORY_PRINT_HEAPEND { NeoSerial.print(F("Heap end:")); NeoSerial.println(__brkval == 0 ? (int)&__heap_start : (int)__brkval); }
|
||||
/// Print free ram end / stack start on NeoSerial console.
|
||||
#define MEMORY_PRINT_STACKSTART { NeoSerial.print(F("Stack start:")); NeoSerial.println((int) SP); }
|
||||
/// Print end of memory on NeoSerial console.
|
||||
#define MEMORY_PRINT_END { NeoSerial.print(F("Stack end:")); NeoSerial.println((int) RAMEND); }
|
||||
/// 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 NeoSerial console.
|
||||
#define MEMORY_PRINT_HEAPSIZE { NeoSerial.print(F("Heap size:")); NeoSerial.println((int) (__brkval == 0 ? (int)&__heap_start : (int)__brkval) - (int)&__heap_start); }
|
||||
/// Print stack size on NeoSerial console.
|
||||
#define MEMORY_PRINT_STACKSIZE { NeoSerial.print(F("Stack size:")); NeoSerial.println((int) RAMEND - (int)SP); }
|
||||
/// Print free ram size on NeoSerial console.
|
||||
#define MEMORY_PRINT_FREERAM { NeoSerial.print(F("Free ram:")); NeoSerial.println((int) SP - (int) (__brkval == 0 ? (int)&__heap_start : (int)__brkval)); }
|
||||
/// Print total SRAM size on NeoSerial console.
|
||||
#define MEMORY_PRINT_TOTALSIZE { NeoSerial.print(F("SRAM size:")); NeoSerial.println((int) RAMEND - (int) &__data_start); }
|
||||
/// 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 NeoSerial console.
|
||||
/// Displays the 'map' of the current state of the Arduino's SRAM memory on the NeoSerialDebug console.
|
||||
void SRamDisplay(void);
|
||||
|
||||
//
|
||||
@ -153,7 +155,7 @@ void SRamDisplay(void);
|
||||
#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; NeoSerial.print(text); NeoSerial.println(mu_stack_size); }
|
||||
#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): "));
|
||||
@ -163,7 +165,7 @@ void SRamDisplay(void);
|
||||
//
|
||||
|
||||
/// Shows the current free SRAM memory with customized text.
|
||||
#define FREERAM_PRINT_TEXT(text) NeoSerial.print(text); NeoSerial.println(mu_freeRam());
|
||||
#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: "));
|
||||
@ -179,7 +181,7 @@ int mu_freeRam(void);
|
||||
uint16_t mu_StackCount(void);
|
||||
|
||||
/// Compute the current maximum and show it now with customized text.
|
||||
#define STACKPAINT_PRINT_TEXT(text) { NeoSerial.print(text); NeoSerial.println(mu_StackCount()); }
|
||||
#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): "));
|
||||
|
||||
@ -26,11 +26,11 @@ Radio radio; //通讯实例
|
||||
//抓取点移动用时
|
||||
#define CATCH_MOVE_DELAY_TIME 500
|
||||
//释放点向前移动用时
|
||||
#define RELEASE_MOVE_FORWARD_DELAY_TIME 1000
|
||||
#define RELEASE_MOVE_FORWARD_DELAY_TIME 1200
|
||||
//释放点暂停用时
|
||||
#define STOP_DELAY_TIME 500
|
||||
//释放点向后向前移动用时
|
||||
#define RELEASE_MOVE_BACKWARD_AND_FORWARD_DELAY_TIME 460
|
||||
#define RELEASE_MOVE_BACKWARD_AND_FORWARD_DELAY_TIME 480
|
||||
//增益点向后向前移动用时
|
||||
#define GAIN_MOVE_BACKWARD_AND_FORWARD_DELAY_TIME 750
|
||||
//增益点稍稍向前移动推环用时
|
||||
@ -70,7 +70,7 @@ Radio radio; //通讯实例
|
||||
|
||||
//****************************************全局变量****************************************
|
||||
//GND Pins
|
||||
const uint8_t gnd_pins[8] PROGMEM = {14, 15, 32, 33, 34, 35, 36, 37};
|
||||
const uint8_t gnd_pins[8] = {12, 13, 32, 33, 34, 35, 36, 37};
|
||||
|
||||
//EEPROM 储存的调试数据
|
||||
struct StroageInfo
|
||||
@ -153,11 +153,14 @@ float CalcFixSpeedRate(float gray_deviation_rate);
|
||||
#define BACK_END 1
|
||||
#define CATCH_END 2
|
||||
#define RELEASE_END 3
|
||||
|
||||
#define ENABLE_FIX 0
|
||||
#define DISABLE_FIX 1
|
||||
//沿线直行，在触发条件后离开函数但不停止
|
||||
void LineForward(uint8_t end_position, float speed_rate = 1.0);
|
||||
void LineForward(uint8_t end_position, uint8_t type = ENABLE_FIX, float speed_rate = 1.0);
|
||||
|
||||
//沿线后退，在触发条件后离开函数但不停止
|
||||
void LineBackward(uint8_t end_position, 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);
|
||||
@ -171,7 +174,7 @@ bool CatchAndCheck(uint8_t type = CATCH_TYPE_CATCH, float speed = 1.0);
|
||||
bool OpenClawAndCheck(void);
|
||||
|
||||
//通讯消息处理函数
|
||||
void HandleMessage(char* message);
|
||||
void HandleMessage(const char* message);
|
||||
//***************************************************************************************
|
||||
|
||||
|
||||
@ -183,14 +186,16 @@ void HandleMessage(char* message);
|
||||
|
||||
#include "MemoryUsage.h"
|
||||
|
||||
#define NeoSerialDebug NeoSerial
|
||||
#define NeoSerialDebug NeoSerial3
|
||||
#define DEBUG_BAUT_RATE 115200
|
||||
|
||||
int loop_time = 0;
|
||||
|
||||
//错误消息函数，用于在出现致命错误后结束程序
|
||||
void DebugCanNotContinue(char* message)
|
||||
void DebugCanNotContinue(const char* message)
|
||||
{
|
||||
move.Stop();
|
||||
|
||||
NeoSerialDebug.print(F("#TAICHI: CAN NOT CONTINUE WHEN ")); NeoSerialDebug.println(message);
|
||||
NeoSerialDebug.print(F("#TAICHI: loop_time: ")); NeoSerialDebug.println(loop_time);
|
||||
NeoSerialDebug.print(F("#TAICHI: pass: [")); NeoSerialDebug.print(passed_point.x); NeoSerialDebug.print(F(", ")); NeoSerialDebug.print(passed_point.y); NeoSerialDebug.print(F("]"));
|
||||
@ -226,7 +231,7 @@ void setup()
|
||||
radio.BeginTransmit();
|
||||
|
||||
//从 EEPROM 读取数据
|
||||
ReadFromEEPROM();
|
||||
ReadEEPROM();
|
||||
|
||||
//开启 HMC5883 的 I2C 通讯
|
||||
sensor.StartHMC5883();
|
||||
@ -370,7 +375,7 @@ void loop()
|
||||
is_carry = false;
|
||||
|
||||
//沿线后退，到后端传感器接触下一条线离开函数
|
||||
LineBackward(BACK_END);
|
||||
LineBackward(BACK_END, DISABLE_FIX);
|
||||
|
||||
//继续后退或转向
|
||||
TurnDirection();
|
||||
@ -394,7 +399,7 @@ void loop()
|
||||
else if (passed_point.type == GETOUT_POINT && next_point.type == NORMAL_POINT)
|
||||
{
|
||||
//沿线后退，到前端传感器接触线离开函数
|
||||
LineBackward(FRONT_END);
|
||||
LineBackward(FRONT_END, DISABLE_FIX);
|
||||
|
||||
//底盘携带清空
|
||||
is_carry = false;
|
||||
@ -508,7 +513,7 @@ void SetGNDPins(void)
|
||||
|
||||
|
||||
//从 EEPROM 读取数据
|
||||
void ReadFromEEPROM(void)
|
||||
void ReadEEPROM(void)
|
||||
{
|
||||
//从 EEPROM 读取调试数据
|
||||
EEPROM.get(EEPROM_ADDRESS, stroage_info);
|
||||
@ -530,17 +535,17 @@ void ReadFromEEPROM(void)
|
||||
delay_time_after_turn = stroage_info.delay_time_after_turn;
|
||||
|
||||
#ifdef TAICHI_DEBUG
|
||||
NeoSerialDebug.println(F("#TAICHI: Data based on EEPROM : "));
|
||||
NeoSerialDebug.print(F("north_left_angle: ")); NeoSerialDebug.println(north_left_angle);
|
||||
NeoSerialDebug.print(F("north_right_angle: ")); NeoSerialDebug.println(north_right_angle);
|
||||
NeoSerialDebug.print(F("west_left_angle: ")); NeoSerialDebug.println(west_left_angle);
|
||||
NeoSerialDebug.print(F("west_right_angle: ")); NeoSerialDebug.println(west_right_angle);
|
||||
NeoSerialDebug.print(F("south_left_angle: ")); NeoSerialDebug.println(south_left_angle);
|
||||
NeoSerialDebug.print(F("south_right_angle: ")); NeoSerialDebug.println(south_right_angle);
|
||||
NeoSerialDebug.print(F("east_left_angle: ")); NeoSerialDebug.println(east_left_angle);
|
||||
NeoSerialDebug.print(F("east_right_angle: ")); NeoSerialDebug.println(east_right_angle);
|
||||
NeoSerialDebug.print(F("gray_7_gate: ")); NeoSerialDebug.println(stroage_info.gray_7_gate);
|
||||
NeoSerialDebug.print(F("delay_time_after_turn: ")); NeoSerialDebug.println(delay_time_after_turn);
|
||||
NeoSerialDebug.println(F("#TAICHI: Data based on EEPROM: "));
|
||||
NeoSerialDebug.print(F("#TAICHI: north_left_angle: ")); NeoSerialDebug.println(north_left_angle);
|
||||
NeoSerialDebug.print(F("#TAICHI: north_right_angle: ")); NeoSerialDebug.println(north_right_angle);
|
||||
NeoSerialDebug.print(F("#TAICHI: west_left_angle: ")); NeoSerialDebug.println(west_left_angle);
|
||||
NeoSerialDebug.print(F("#TAICHI: west_right_angle: ")); NeoSerialDebug.println(west_right_angle);
|
||||
NeoSerialDebug.print(F("#TAICHI: south_left_angle: ")); NeoSerialDebug.println(south_left_angle);
|
||||
NeoSerialDebug.print(F("#TAICHI: south_right_angle: ")); NeoSerialDebug.println(south_right_angle);
|
||||
NeoSerialDebug.print(F("#TAICHI: east_left_angle: ")); NeoSerialDebug.println(east_left_angle);
|
||||
NeoSerialDebug.print(F("#TAICHI: east_right_angle: ")); NeoSerialDebug.println(east_right_angle);
|
||||
NeoSerialDebug.print(F("#TAICHI: gray_7_gate: ")); NeoSerialDebug.println(stroage_info.gray_7_gate);
|
||||
NeoSerialDebug.print(F("#TAICHI: delay_time_after_turn: ")); NeoSerialDebug.println(delay_time_after_turn);
|
||||
#endif
|
||||
}
|
||||
|
||||
@ -686,7 +691,7 @@ float CalcFixSpeedRate(float gray_deviation_rate)
|
||||
|
||||
|
||||
//沿线直行，在触发条件后离开函数但不停止
|
||||
void LineForward(uint8_t end_position, float speed_rate)
|
||||
void LineForward(uint8_t end_position, uint8_t type, float speed_rate)
|
||||
{
|
||||
#ifdef TAICHI_DEBUG
|
||||
//调试输出沿线直行状态
|
||||
@ -695,29 +700,34 @@ void LineForward(uint8_t end_position, float speed_rate)
|
||||
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)
|
||||
while (1)
|
||||
{
|
||||
if (!sensor.IsWhite(GRAY_3) && sensor.IsWhite(GRAY_4)) //左侧越线
|
||||
if (type == ENABLE_FIX)
|
||||
{
|
||||
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 (!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) //前端接触线离开函数
|
||||
@ -760,7 +770,7 @@ void LineForward(uint8_t end_position, float speed_rate)
|
||||
|
||||
|
||||
//沿线后退，在触发条件后离开函数但不停止
|
||||
void LineBackward(uint8_t end_position, float speed_rate)
|
||||
void LineBackward(uint8_t end_position, uint8_t type, float speed_rate)
|
||||
{
|
||||
#ifdef TAICHI_DEBUG
|
||||
//调试输出沿线后退状态
|
||||
@ -773,23 +783,28 @@ void LineBackward(uint8_t end_position, float speed_rate)
|
||||
bool gray_match_a = false;
|
||||
bool gray_match_b = false;
|
||||
|
||||
while(1)
|
||||
move.Backward(speed_rate);
|
||||
|
||||
while (1)
|
||||
{
|
||||
if (!sensor.IsWhite(GRAY_3) && sensor.IsWhite(GRAY_4)) //左侧越线
|
||||
if (type == ENABLE_FIX)
|
||||
{
|
||||
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 (!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) //前端接触线离开函数
|
||||
@ -1054,7 +1069,7 @@ bool OpenClawAndCheck(void)
|
||||
|
||||
|
||||
//通讯消息处理函数
|
||||
void HandleMessage(char* message)
|
||||
void HandleMessage(const char* message)
|
||||
{
|
||||
radio.Send("Get the message: ");
|
||||
radio.Send(message);
|
||||
|
||||
@ -123,8 +123,8 @@ void Move::Forward(float speed_rate)
|
||||
|
||||
#ifdef MOVE_DEBUG
|
||||
//调试输出前进状态
|
||||
NeoSerial.print(F("#MOVE: Move Forward"));
|
||||
NeoSerial.print(F(" speed_rate: ")); NeoSerial.println(speed_rate);
|
||||
NeoSerialDebug.print(F("#MOVE: Move Forward"));
|
||||
NeoSerialDebug.print(F(" speed_rate: ")); NeoSerialDebug.println(speed_rate);
|
||||
#endif
|
||||
}
|
||||
|
||||
@ -143,8 +143,8 @@ void Move::Backward(float speed_rate)
|
||||
|
||||
#ifdef MOVE_DEBUG
|
||||
//调试输出后退状态
|
||||
NeoSerial.print(F("#MOVE: Move Backward"));
|
||||
NeoSerial.print(F(" speed_rate: ")); NeoSerial.println(speed_rate);
|
||||
NeoSerialDebug.print(F("#MOVE: Move Backward"));
|
||||
NeoSerialDebug.print(F(" speed_rate: ")); NeoSerialDebug.println(speed_rate);
|
||||
#endif
|
||||
}
|
||||
|
||||
@ -163,8 +163,8 @@ void Move::ForLeftward(float speed_rate, float turn_speed_rate)
|
||||
|
||||
#ifdef MOVE_DEBUG
|
||||
//调试输出向前左转状态
|
||||
NeoSerial.print(F("#MOVE: Move ForLeftward"));
|
||||
NeoSerial.print(F(" speed_rate: ")); NeoSerial.print(speed_rate); NeoSerial.print(F(" turn_speed_rate: ")); NeoSerial.println(turn_speed_rate);
|
||||
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
|
||||
}
|
||||
|
||||
@ -183,8 +183,8 @@ void Move::ForRightward(float speed_rate, float turn_speed_rate)
|
||||
|
||||
#ifdef MOVE_DEBUG
|
||||
//调试输出向前右转状态
|
||||
NeoSerial.print(F("#MOVE: Move ForRightward"));
|
||||
NeoSerial.print(F(" speed_rate: ")); NeoSerial.print(speed_rate); NeoSerial.print(F(" turn_speed_rate: ")); NeoSerial.println(turn_speed_rate);
|
||||
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
|
||||
}
|
||||
|
||||
@ -203,8 +203,8 @@ void Move::BackLeftward(float speed_rate, float turn_speed_rate)
|
||||
|
||||
#ifdef MOVE_DEBUG
|
||||
//调试输出向后左转状态
|
||||
NeoSerial.print(F("#MOVE: Move BackLeftward"));
|
||||
NeoSerial.print(F(" speed_rate: ")); NeoSerial.print(speed_rate); NeoSerial.print(F(" turn_speed_rate: ")); NeoSerial.println(turn_speed_rate);
|
||||
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
|
||||
}
|
||||
|
||||
@ -223,8 +223,8 @@ void Move::BackRightward(float speed_rate, float turn_speed_rate)
|
||||
|
||||
#ifdef MOVE_DEBUG
|
||||
//调试输出向后右转状态
|
||||
NeoSerial.print(F("#MOVE: Move BackRightward"));
|
||||
NeoSerial.print(F(" speed_rate: ")); NeoSerial.print(speed_rate); NeoSerial.print(F(" turn_speed_rate: ")); NeoSerial.println(turn_speed_rate);
|
||||
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
|
||||
}
|
||||
|
||||
@ -243,7 +243,7 @@ void Move::Stop(void)
|
||||
|
||||
#ifdef MOVE_DEBUG
|
||||
//调试输出制动状态
|
||||
NeoSerial.println(F("#MOVE: Move Stop"));
|
||||
NeoSerialDebug.println(F("#MOVE: Move Stop"));
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
@ -5,6 +5,9 @@
|
||||
//注释以关闭调试功能
|
||||
#define MOVE_DEBUG
|
||||
|
||||
#ifdef MOVE_DEBUG
|
||||
#define NeoSerialDebug NeoSerial
|
||||
#endif
|
||||
|
||||
//轮胎定义
|
||||
#define LEFT_A_WHEEL 0
|
||||
|
||||
@ -23,12 +23,14 @@ void Radio::BeginTransmit(unsigned long baud_rate)
|
||||
|
||||
|
||||
//发送
|
||||
void Radio::Send(char* message, uint8_t send_type, uint8_t send_times)
|
||||
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);
|
||||
@ -46,7 +48,14 @@ void Radio::Send(char* message, uint8_t send_type, uint8_t send_times)
|
||||
}
|
||||
else if (i == BLANK_CHAR_LENGTH)
|
||||
{
|
||||
full_message[i] = START_CHAR;
|
||||
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)
|
||||
{
|
||||
@ -79,11 +88,11 @@ void Radio::Send(char* message, uint8_t send_type, uint8_t send_times)
|
||||
}
|
||||
|
||||
#ifdef RADIO_DEBUG
|
||||
NeoSerial.print(F("#RADIO: SEND: "));
|
||||
NeoSerialDebug.print(F("#RADIO: SEND: "));
|
||||
full_message[FULL_MESSAGE_SIZE - 1] = '\0';
|
||||
NeoSerial.print(full_message);
|
||||
NeoSerial.print(F(" TIMES: "));
|
||||
NeoSerial.println(send_times);
|
||||
NeoSerialDebug.print(full_message);
|
||||
NeoSerialDebug.print(F(" TIMES: "));
|
||||
NeoSerialDebug.println(send_times);
|
||||
#endif
|
||||
|
||||
if (send_type == FORCE_SEND)
|
||||
@ -115,102 +124,103 @@ void Radio::EnableReceiveInterrupt() //恢复接收中断
|
||||
//接收，使用中断触发
|
||||
bool Radio::Receive(uint8_t ch, uint8_t status)
|
||||
{
|
||||
static long begin_time = millis();
|
||||
static bool is_code_record = false;
|
||||
static bool is_start_record = false;
|
||||
static bool is_check_record = false;
|
||||
static bool is_end_record = false;
|
||||
static char message[FULL_MESSAGE_SIZE];
|
||||
static char check_str[CHECK_STR_LENGTH + 1];
|
||||
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;
|
||||
|
||||
//若上一次中断已在 100 ms前，可以认为当前是一次新的传输
|
||||
if (millis() - begin_time > 100)
|
||||
switch (ch)
|
||||
{
|
||||
begin_time = millis(); //更新时间
|
||||
|
||||
case CODE_CHAR:
|
||||
{
|
||||
is_code_record = true;
|
||||
is_start_record = false;
|
||||
is_check_record = false;
|
||||
is_end_record = false;
|
||||
}
|
||||
else
|
||||
|
||||
//重置字符串
|
||||
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:
|
||||
{
|
||||
begin_time = millis(); //更新时间
|
||||
|
||||
switch (ch)
|
||||
if (is_code_record == true)
|
||||
{
|
||||
case START_CHAR:
|
||||
is_code_record = false;
|
||||
is_start_record = true;
|
||||
}
|
||||
} break;
|
||||
|
||||
case CHECK_CHAR:
|
||||
{
|
||||
if (is_start_record == true)
|
||||
{
|
||||
if (is_end_record == false)
|
||||
{
|
||||
is_start_record = true;
|
||||
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';
|
||||
is_start_record = false;
|
||||
is_check_record = true;
|
||||
}
|
||||
} break;
|
||||
|
||||
i = 0;
|
||||
j = 0;
|
||||
}
|
||||
} break;
|
||||
|
||||
case CHECK_CHAR:
|
||||
case END_CHAR:
|
||||
{
|
||||
if (is_check_record == true)
|
||||
{
|
||||
if (is_end_record == false && is_start_record == true)
|
||||
{
|
||||
is_start_record = false;
|
||||
is_check_record = true;
|
||||
}
|
||||
} break;
|
||||
is_check_record = false;
|
||||
|
||||
case END_CHAR:
|
||||
{
|
||||
if (is_end_record == false && is_check_record == true)
|
||||
{
|
||||
is_check_record = false;
|
||||
if (j <= CHECK_STR_LENGTH)
|
||||
check_str[j] = '\0';
|
||||
|
||||
if (j <= CHECK_STR_LENGTH)
|
||||
check_str[j] = '\0';
|
||||
|
||||
if (atoi(check_str) == strlen(message)) //位数校验成功
|
||||
if (atoi(check_str) == strlen(message)) //位数校验成功
|
||||
{
|
||||
if (this_package_code != last_package_code)
|
||||
{
|
||||
is_end_record = true;
|
||||
|
||||
#ifdef RADIO_DEBUG
|
||||
NeoSerial.print(F("#RADIO: RECEIVE: "));
|
||||
NeoSerial.println(message);
|
||||
NeoSerialDebug.print(F("#RADIO: RECEIVE: "));
|
||||
NeoSerialDebug.println(message);
|
||||
#endif
|
||||
|
||||
hm_func(message);
|
||||
}
|
||||
else //位数校验失败
|
||||
{
|
||||
#ifdef RADIO_DEBUG
|
||||
NeoSerial.println(F("#RADIO: RECEIVE CHECK FAIL!"));
|
||||
#endif
|
||||
}
|
||||
}
|
||||
} break;
|
||||
|
||||
default:
|
||||
{
|
||||
if (is_start_record == true)
|
||||
{
|
||||
if (i < FULL_MESSAGE_SIZE)
|
||||
message[i] = ch;
|
||||
i++;
|
||||
last_package_code = this_package_code;
|
||||
}
|
||||
}
|
||||
else if (is_check_record == true)
|
||||
{
|
||||
if (j <= CHECK_STR_LENGTH)
|
||||
check_str[j] = ch;
|
||||
j++;
|
||||
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;
|
||||
}
|
||||
@ -7,6 +7,10 @@
|
||||
//注释以关闭调试功能
|
||||
#define RADIO_DEBUG
|
||||
|
||||
#ifdef RADIO_DEBUG
|
||||
#define NeoSerialDebug NeoSerial3
|
||||
#endif
|
||||
|
||||
//默认与 HC-12 连接串口
|
||||
#define RADIO_SERIAL_NUM NeoSerial2
|
||||
//与 HC-12 串口通信波特率
|
||||
@ -15,13 +19,14 @@
|
||||
//通信包大小
|
||||
#define FULL_MESSAGE_SIZE 50
|
||||
//通信包最大有效信息大小
|
||||
#define MAX_REAL_MESSAGE_SIZE 41
|
||||
#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 '#'
|
||||
@ -36,7 +41,7 @@
|
||||
|
||||
|
||||
//回调函数指针
|
||||
typedef void (*HandleMessageFunction)(char*);
|
||||
typedef void (*HandleMessageFunction)(const char*);
|
||||
|
||||
|
||||
class Radio
|
||||
@ -46,7 +51,7 @@ public:
|
||||
|
||||
static void BeginTransmit(unsigned long baud_rate = RADIO_BAUD_RATE); //打开串口
|
||||
|
||||
static void Send(char* message, uint8_t send_type = NO_FORCE_SEND, uint8_t send_times = DEFAULT_SEND_TIMES); //发送
|
||||
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(); //禁用接收中断
|
||||
|
||||
@ -1,6 +1,13 @@
|
||||
#include <Arduino.h>
|
||||
|
||||
#include "routeTaiChi.h"
|
||||
|
||||
|
||||
#ifdef ROUTE_DEBUG
|
||||
#include <NeoHWSerial.h>
|
||||
#endif
|
||||
|
||||
|
||||
//静态变量
|
||||
Point* Route::head_point = NULL;
|
||||
Point* Route::passed_point = NULL;
|
||||
@ -56,6 +63,10 @@ void Route::SetNextNextPointType(uint8_t type)
|
||||
void Route::UpdatePosition(void)
|
||||
{
|
||||
passed_point = passed_point->next;
|
||||
|
||||
#ifdef ROUTE_DEBUG
|
||||
NeoSerialDebug.println(F("#ROUTE: UPDATE POSITION!"));
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
@ -275,9 +286,9 @@ Point* Route::AddPoint(Point* front_point, int8_t x, int8_t y, uint8_t type, Poi
|
||||
|
||||
//生成基本路径点链
|
||||
void Route::InitBaseRoute(void)
|
||||
{
|
||||
{
|
||||
//基本路径数组
|
||||
const static int8_t base_route [][3] PROGMEM =
|
||||
const static int8_t base_route [][3] =
|
||||
{
|
||||
{0, 0, NORMAL_POINT},
|
||||
{0, 1, NORMAL_POINT},
|
||||
|
||||
@ -7,6 +7,10 @@
|
||||
//注释以关闭调试功能
|
||||
#define ROUTE_DEBUG
|
||||
|
||||
#ifdef ROUTE_DEBUG
|
||||
#define NeoSerialDebug NeoSerial3
|
||||
#endif
|
||||
|
||||
//坐标点操作定义
|
||||
#define NORMAL_POINT 0 //普通点
|
||||
#define CATCH_POINT 1 //抓取点
|
||||
|
||||
@ -86,13 +86,13 @@ void Sensor::FlashGraySensor(uint8_t gray_sensor_num)
|
||||
//调试输出闪烁信息
|
||||
switch (gray_sensor_num)
|
||||
{
|
||||
case GRAY_1: NeoSerial.println(F("#SENSOR: FLASH GRAY_1 NOW!")); break;
|
||||
case GRAY_2: NeoSerial.println(F("#SENSOR: FLASH GRAY_2 NOW!")); break;
|
||||
case GRAY_3: NeoSerial.println(F("#SENSOR: FLASH GRAY_3 NOW!")); break;
|
||||
case GRAY_4: NeoSerial.println(F("#SENSOR: FLASH GRAY_4 NOW!")); break;
|
||||
case GRAY_5: NeoSerial.println(F("#SENSOR: FLASH GRAY_5 NOW!")); break;
|
||||
case GRAY_6: NeoSerial.println(F("#SENSOR: FLASH GRAY_6 NOW!")); break;
|
||||
case GRAY_7: NeoSerial.println(F("#SENSOR: FLASH GRAY_7 NOW!"));
|
||||
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
|
||||
|
||||
@ -125,18 +125,18 @@ bool Sensor::IsWhite(uint8_t gray_sensor_num)
|
||||
//调试输出灰度值
|
||||
switch (gray_sensor_num)
|
||||
{
|
||||
case GRAY_1: NeoSerial.print(F("#SENSOR: GRAY_1 and gate_val: ")); break;
|
||||
case GRAY_2: NeoSerial.print(F("#SENSOR: GRAY_2 and gate_val: ")); break;
|
||||
case GRAY_3: NeoSerial.print(F("#SENSOR: GRAY_3 and gate_val: ")); break;
|
||||
case GRAY_4: NeoSerial.print(F("#SENSOR: GRAY_4 and gate_val: ")); break;
|
||||
case GRAY_5: NeoSerial.print(F("#SENSOR: GRAY_5 and gate_val: ")); break;
|
||||
case GRAY_6: NeoSerial.print(F("#SENSOR: GRAY_6 and gate_val: ")); break;
|
||||
case GRAY_7: NeoSerial.print(F("#SENSOR: GRAY_7 and gate_val: "));
|
||||
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: "));
|
||||
}
|
||||
|
||||
NeoSerial.print(gray_val);
|
||||
NeoSerial.print(F(" "));
|
||||
NeoSerial.println(gray_gate);
|
||||
NeoSerialDebug.print(gray_val);
|
||||
NeoSerialDebug.print(F(" "));
|
||||
NeoSerialDebug.println(gray_gate);
|
||||
#endif
|
||||
|
||||
if (gray_val > gray_gate)
|
||||
@ -170,25 +170,25 @@ float Sensor::GrayDeviationRate(uint8_t gray_sensor_num)
|
||||
//调试输出灰度值
|
||||
switch (gray_sensor_num)
|
||||
{
|
||||
case GRAY_1: NeoSerial.print(F("#SENSOR: GRAY_1 and gate_val: ")); break;
|
||||
case GRAY_2: NeoSerial.print(F("#SENSOR: GRAY_2 and gate_val: ")); break;
|
||||
case GRAY_3: NeoSerial.print(F("#SENSOR: GRAY_3 and gate_val: ")); break;
|
||||
case GRAY_4: NeoSerial.print(F("#SENSOR: GRAY_4 and gate_val: ")); break;
|
||||
case GRAY_5: NeoSerial.print(F("#SENSOR: GRAY_5 and gate_val: ")); break;
|
||||
case GRAY_6: NeoSerial.print(F("#SENSOR: GRAY_6 and gate_val: ")); break;
|
||||
case GRAY_7: NeoSerial.print(F("#SENSOR: GRAY_7 and gate_val: "));
|
||||
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: "));
|
||||
}
|
||||
|
||||
NeoSerial.print(gray_val);
|
||||
NeoSerial.print(F(" "));
|
||||
NeoSerial.print(gray_gate);
|
||||
NeoSerialDebug.print(gray_val);
|
||||
NeoSerialDebug.print(F(" "));
|
||||
NeoSerialDebug.print(gray_gate);
|
||||
#endif
|
||||
|
||||
deviarion_rate = (float)gray_val / gray_gate;
|
||||
|
||||
#ifdef SENSOR_DEBUG
|
||||
NeoSerial.print(F(" deviarion_rate: "));
|
||||
NeoSerial.println(deviarion_rate);
|
||||
NeoSerialDebug.print(F(" deviarion_rate: "));
|
||||
NeoSerialDebug.println(deviarion_rate);
|
||||
#endif
|
||||
|
||||
return deviarion_rate;
|
||||
@ -210,12 +210,12 @@ bool Sensor::IsPushed(uint8_t button_num)
|
||||
#ifdef SENSOR_DEBUG
|
||||
//调试输出按钮状态
|
||||
if (button_num == BUTTON_1)
|
||||
NeoSerial.print(F("#SENSOR: BUTTON_1: "));
|
||||
else NeoSerial.print(F("#SENSOR: BUTTON_2: "));
|
||||
NeoSerialDebug.print(F("#SENSOR: BUTTON_1: "));
|
||||
else NeoSerialDebug.print(F("#SENSOR: BUTTON_2: "));
|
||||
|
||||
if (button_val == LOW)
|
||||
NeoSerial.println(F("pushed"));
|
||||
else NeoSerial.println(F("released"));
|
||||
NeoSerialDebug.println(F("pushed"));
|
||||
else NeoSerialDebug.println(F("released"));
|
||||
#endif
|
||||
|
||||
if (button_val == LOW)
|
||||
@ -235,7 +235,7 @@ void Sensor::StartHMC5883(void)
|
||||
|
||||
#ifdef SENSOR_DEBUG
|
||||
//调试输出
|
||||
NeoSerial.println(F("#SENSOR: Start HMC5883"));
|
||||
NeoSerialDebug.println(F("#SENSOR: Start HMC5883"));
|
||||
#endif
|
||||
}
|
||||
|
||||
@ -270,8 +270,8 @@ float Sensor::GetAngle(void)
|
||||
|
||||
#ifdef SENSOR_DEBUG
|
||||
//调试输出朝向角
|
||||
NeoSerial.print(F("#SENSOR: Angle Value: "));
|
||||
NeoSerial.println(angle);
|
||||
NeoSerialDebug.print(F("#SENSOR: Angle Value: "));
|
||||
NeoSerialDebug.println(angle);
|
||||
#endif
|
||||
|
||||
return angle;
|
||||
|
||||
@ -5,6 +5,9 @@
|
||||
//注释以关闭调试功能
|
||||
#define SENSOR_DEBUG
|
||||
|
||||
#ifdef SENSOR_DEBUG
|
||||
#define NeoSerialDebug NeoSerial3
|
||||
#endif
|
||||
|
||||
//灰度传感器 OUT 接口定义
|
||||
#define GRAY_1_OUT A0
|
||||
|
||||
@ -45,9 +45,9 @@ void Servo::MoveServo(uint8_t servo_id, uint16_t position, uint16_t time)
|
||||
|
||||
#ifdef SERVO_DEBUG
|
||||
//调试输出动作组执行信息
|
||||
NeoSerial.print(F("#SERVO: MoveServo: ")); NeoSerial.print((int)servo_id);
|
||||
NeoSerial.print(F(" position: ")); NeoSerial.print((int)position);
|
||||
NeoSerial.print(F(" time: ")); NeoSerial.println((int)time);
|
||||
NeoSerialDebug.print(F("#SERVO: MoveServo: ")); NeoSerialDebug.print((int)servo_id);
|
||||
NeoSerialDebug.print(F(" position: ")); NeoSerialDebug.print((int)position);
|
||||
NeoSerialDebug.print(F(" time: ")); NeoSerialDebug.println((int)time);
|
||||
#endif
|
||||
}
|
||||
|
||||
@ -70,8 +70,8 @@ void Servo::RunActionGroup(uint8_t action_num, uint16_t times)
|
||||
|
||||
#ifdef SERVO_DEBUG
|
||||
//调试输出动作组执行信息
|
||||
NeoSerial.print(F("#SERVO: RunServoActionGroup: "));
|
||||
NeoSerial.println((int)action_num);
|
||||
NeoSerialDebug.print(F("#SERVO: RunServoActionGroup: "));
|
||||
NeoSerialDebug.println((int)action_num);
|
||||
#endif
|
||||
}
|
||||
|
||||
@ -89,7 +89,7 @@ void Servo::StopActionGroup(void)
|
||||
|
||||
#ifdef SERVO_DEBUG
|
||||
//调试输出动作组停止信息
|
||||
NeoSerial.println(F("#SERVO: StopServoActionGroup"));
|
||||
NeoSerialDebug.println(F("#SERVO: StopServoActionGroup"));
|
||||
#endif
|
||||
}
|
||||
|
||||
@ -112,10 +112,10 @@ void Servo::SetActionGroupSpeed(uint8_t action_num, float speed)
|
||||
|
||||
#ifdef SERVO_DEBUG
|
||||
//调试输出动作组速度设定信息
|
||||
NeoSerial.print(F("#SERVO: SetServoActionGroupSpeed: "));
|
||||
NeoSerial.print((int)action_num);
|
||||
NeoSerial.print(F(" Speed: "));
|
||||
NeoSerial.println(speed);
|
||||
NeoSerialDebug.print(F("#SERVO: SetServoActionGroupSpeed: "));
|
||||
NeoSerialDebug.print((int)action_num);
|
||||
NeoSerialDebug.print(F(" Speed: "));
|
||||
NeoSerialDebug.println(speed);
|
||||
#endif
|
||||
}
|
||||
|
||||
@ -168,7 +168,7 @@ void Servo::Release(float speed)
|
||||
|
||||
|
||||
//增益点放下爪子，指定速度
|
||||
void Servo::GainDown(float speed = SERVO_NORMAL_SPEED)
|
||||
void Servo::GainDown(float speed)
|
||||
{
|
||||
SetActionGroupSpeed(ACTION_GAINDOWN_NUM, speed);
|
||||
RunActionGroup(ACTION_GAINDOWN_NUM, 1);
|
||||
@ -176,7 +176,7 @@ void Servo::GainDown(float speed = SERVO_NORMAL_SPEED)
|
||||
|
||||
|
||||
//增益点抓取，指定速度
|
||||
void Servo::GainCatch(float speed = SERVO_NORMAL_SPEED)
|
||||
void Servo::GainCatch(float speed)
|
||||
{
|
||||
SetActionGroupSpeed(ACTION_GAINCATCH_NUM, speed);
|
||||
RunActionGroup(ACTION_GAINCATCH_NUM, 1);
|
||||
|
||||
@ -7,6 +7,9 @@
|
||||
//注释以关闭调试功能
|
||||
#define SERVO_DEBUG
|
||||
|
||||
#ifdef SERVO_DEBUG
|
||||
#define NeoSerialDebug NeoSerial3
|
||||
#endif
|
||||
|
||||
//与舵机控制板连接串口
|
||||
//使用 Mega 板 18 19 作为串口通信端口
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user