From b1e5478518518b5afa760d661d32ed9e5efdaca5 Mon Sep 17 00:00:00 2001 From: "lxbpxylps@126.com" Date: Sun, 21 Mar 2021 23:32:51 +0800 Subject: [PATCH] =?UTF-8?q?=E6=94=AF=E6=8C=81=E5=A4=9A=E7=A7=8D=E8=BD=AC?= =?UTF-8?q?=E5=90=91=E6=96=B9=E5=BC=8F=E5=B9=B6=E6=94=AF=E6=8C=81=20EEPROM?= =?UTF-8?q?=20=E8=AF=BB=E5=8F=96=E6=95=B0=E6=8D=AE?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- TaiChi/TaiChi.ino | 281 ++++++++++++++++++++++++++++++++++------------ 1 file changed, 207 insertions(+), 74 deletions(-) diff --git a/TaiChi/TaiChi.ino b/TaiChi/TaiChi.ino index c91e4f3..c74e496 100644 --- a/TaiChi/TaiChi.ino +++ b/TaiChi/TaiChi.ino @@ -1,11 +1,17 @@ +#include +#include +#include + #include "moveTaiChi.h" //轮胎运动库 #include "sensorTaiChi.h" //传感器库 #include "servoTaiChi.h" //舵机库 +#include "radioTaiChi.h" //通信库 Move move; //轮胎运动实例 Sensor sensor; //传感器实例 Servo servo; //舵机实例 +Radio radio; //通讯实例 //****************************************运动路径**************************************** @@ -53,6 +59,9 @@ int8_t route[][3] = //****************************************可调参数**************************************** +//EEPROM 写入位置 +#define EEPROM_ADDRESS 0x0 + //是否关闭爪子状态检测功能 #define DISABLE_CLAW_CHECK @@ -103,7 +112,31 @@ int8_t route[][3] = //****************************************全局变量**************************************** //GND Pins -uint8_t gnd_pins[4] = {14, 15, 16, 17}; +uint8_t gnd_pins[8] = {14, 15, 32, 33, 34, 35, 36, 37}; + +//EEPROM 储存的调试数据 +struct StroageInfo +{ + float real_angle_val[8]; + int gray_7_gate; + int delay_time_after_turn; +} stroage_info; + +//下一点绝对朝向角 +float north_left_angle; +float north_right_angle; +float west_left_angle; +float west_right_angle; +float south_left_angle; +float south_right_angle; +float east_left_angle; +float east_right_angle; + +//7 号传感器灰度临界值 +int gray_7_gate; + +//转向开始到结束的时间 +int delay_time_after_turn; //数组位置标记 int passed_flag = 0; @@ -118,6 +151,13 @@ int max_flag; //下一点朝向 uint8_t next_position = FRONT_NEXT; +#define NORTH 0 +#define WEST 1 +#define SOUTH 2 +#define EAST 3 +//下一点绝对朝向 +uint8_t current_real_direction = NORTH; + //爪子是否抓有环 bool is_claw_catch = false; @@ -126,6 +166,12 @@ bool is_claw_ok = true; //底盘是否携带环 bool is_carry = false; + +#define TURN_BY_TIME 0 //基于时间 +#define TURN_BY_GRAY 1 //基于灰度传感器 +#define TURN_BY_EARTH 2 //基于地磁场 +//转向方式 +uint8_t turn_method = TURN_BY_TIME; //*************************************************************************************** @@ -133,6 +179,9 @@ bool is_carry = false; //设置接口低电平作为额外地 void SetGNDPins(void); +//从 EEPROM 读取数据 +void ReadFromEEPROM(void); + //在开始运行前依次检测各灰度传感器下方黑白是否正常，若不正常，异常传感器闪烁，程序不继续进行 void CheckGrayStatus(void); @@ -167,6 +216,9 @@ bool CatchAndCheck(uint8_t type = CATCH_TYPE_CATCH, float speed = 1.0); //打开爪子，并判断爪子是否能正常工作 bool OpenClawAndCheck(void); + +//通讯消息处理函数 +void HandleMessage(char * message); //*************************************************************************************** @@ -177,41 +229,21 @@ bool OpenClawAndCheck(void); #ifdef TAICHI_DEBUG #define DEBUG_BAUT_RATE 115200 -#define DEBUG_PAUSE_INTERRUPTNUM 2 //PIN 21 -#define DEBUG_PAUSE_PIN 21 int loop_time = 0; -//中断函数，用于调试时暂停程序 -void DebugPause(void) -{ - //记录之前的运动状态 - uint8_t current_direction = move.GetCurrentMove(); //当前运动状态 - float current_speed_rate = move.GetCurrentSpeedRate(); //当前运动速度比率 - float current_turn_speed_rate = move.GetCurrentTurnSpeedRate(); //当前转向时一侧减速的比率 - - //停止运动 - move.Stop(); - - //等待低电平结束 - while (digitalRead(DEBUG_PAUSE_PIN) == LOW) {} - - //恢复之前的运动 - move.MoveDirection(current_direction, current_speed_rate, current_turn_speed_rate); -} - //错误消息函数，用于在出现致命错误后结束程序 void DebugCanNotContinue(char* message) { - Serial.print("#TAICHI: CAN NOT CONTINUE WHEN "); Serial.println(message); - Serial.print("#TAICHI: loop_time: "); Serial.println(loop_time); - Serial.print("#TAICHI: pass: ["); Serial.print(route[passed_flag][X]); Serial.print(", "); Serial.print(route[passed_flag][Y]); Serial.print("]"); - Serial.print(" flag: "); Serial.print(passed_flag); - Serial.print(" TYPE: "); Serial.println((int)route[passed_flag][TYPE]); - Serial.print("#TAICHI: next: ["); Serial.print(route[next_flag][X]); Serial.print(", "); Serial.print(route[next_flag][Y]); Serial.print("]"); - Serial.print(" next_position: "); Serial.print((int)next_position); - Serial.print(" TYPE: "); Serial.println((int)route[next_flag][TYPE]); - Serial.print("#TAICHI: is_claw_catch: "); Serial.print((int)is_claw_catch); Serial.print(" is_claw_ok: "); Serial.println((int)is_claw_ok); + NeoSerial.print("#TAICHI: CAN NOT CONTINUE WHEN "); NeoSerial.println(message); + NeoSerial.print("#TAICHI: loop_time: "); NeoSerial.println(loop_time); + NeoSerial.print("#TAICHI: pass: ["); NeoSerial.print(route[passed_flag][X]); NeoSerial.print(", "); NeoSerial.print(route[passed_flag][Y]); NeoSerial.print("]"); + NeoSerial.print(" flag: "); NeoSerial.print(passed_flag); + NeoSerial.print(" TYPE: "); NeoSerial.println((int)route[passed_flag][TYPE]); + NeoSerial.print("#TAICHI: next: ["); NeoSerial.print(route[next_flag][X]); NeoSerial.print(", "); NeoSerial.print(route[next_flag][Y]); NeoSerial.print("]"); + NeoSerial.print(" next_position: "); NeoSerial.print((int)next_position); + NeoSerial.print(" TYPE: "); NeoSerial.println((int)route[next_flag][TYPE]); + NeoSerial.print("#TAICHI: is_claw_catch: "); NeoSerial.print((int)is_claw_catch); NeoSerial.print(" is_claw_ok: "); NeoSerial.println((int)is_claw_ok); while (1) {} } @@ -222,17 +254,26 @@ void DebugCanNotContinue(char* message) void setup() { #ifdef TAICHI_DEBUG - pinMode(DEBUG_PAUSE_PIN, INPUT_PULLUP); - attachInterrupt(DEBUG_PAUSE_INTERRUPTNUM, DebugPause, LOW); - Serial.begin(DEBUG_BAUT_RATE); - Serial.println("#TAICHI: ======================setup()====================="); + NeoSerial.begin(DEBUG_BAUT_RATE); + NeoSerial.println("#TAICHI: ======================setup()====================="); #endif SetGNDPins(); move.Stop(); + + servo.BeginTransmit(); servo.StopAndReset(); + radio.SetHandleMessageFunction(HandleMessage); + radio.BeginTransmit(); + + //从 EEPROM 读取数据 + ReadFromEEPROM(); + + //开启 HMC5883 的 I2C 通讯 + sensor.StartHMC5883(); + //在开始运行前依次检测各灰度传感器下方黑白是否正常 CheckGrayStatus(); @@ -251,15 +292,15 @@ void loop() { #ifdef TAICHI_DEBUG loop_time++; - Serial.println("#TAICHI: ====================New loop()===================="); - Serial.print("#TAICHI: loop_time: "); Serial.println(loop_time); - Serial.print("#TAICHI: pass: ["); Serial.print(route[passed_flag][X]); Serial.print(", "); Serial.print(route[passed_flag][Y]); Serial.print("]"); - Serial.print(" flag: "); Serial.print(passed_flag); - Serial.print(" TYPE: "); Serial.println((int)route[passed_flag][TYPE]); - Serial.print("#TAICHI: next: ["); Serial.print(route[next_flag][X]); Serial.print(", "); Serial.print(route[next_flag][Y]); Serial.print("]"); - Serial.print(" next_position: "); Serial.print((int)next_position); - Serial.print(" TYPE: "); Serial.println((int)route[next_flag][TYPE]); - Serial.print("#TAICHI: is_claw_catch: "); Serial.print((int)is_claw_catch); Serial.print(" is_claw_ok: "); Serial.println((int)is_claw_ok); + NeoSerial.println("#TAICHI: ====================New loop()===================="); + NeoSerial.print("#TAICHI: loop_time: "); NeoSerial.println(loop_time); + NeoSerial.print("#TAICHI: pass: ["); NeoSerial.print(route[passed_flag][X]); NeoSerial.print(", "); NeoSerial.print(route[passed_flag][Y]); NeoSerial.print("]"); + NeoSerial.print(" flag: "); NeoSerial.print(passed_flag); + NeoSerial.print(" TYPE: "); NeoSerial.println((int)route[passed_flag][TYPE]); + NeoSerial.print("#TAICHI: next: ["); NeoSerial.print(route[next_flag][X]); NeoSerial.print(", "); NeoSerial.print(route[next_flag][Y]); NeoSerial.print("]"); + NeoSerial.print(" next_position: "); NeoSerial.print((int)next_position); + NeoSerial.print(" TYPE: "); NeoSerial.println((int)route[next_flag][TYPE]); + NeoSerial.print("#TAICHI: is_claw_catch: "); NeoSerial.print((int)is_claw_catch); NeoSerial.print(" is_claw_ok: "); NeoSerial.println((int)is_claw_ok); #endif int8_t& passed_flag_type = route[passed_flag][TYPE]; @@ -489,15 +530,13 @@ void loop() #ifdef TAICHI_DEBUG DebugCanNotContinue("CHOOSE LOOP"); #endif - - while (1) {} } //更新标记，继续循环 UpdateFlag(); #ifdef TAICHI_DEBUG - Serial.println("#TAICHI: ====================End loop()===================="); + NeoSerial.println("#TAICHI: ====================End loop()===================="); #endif } @@ -505,9 +544,9 @@ void loop() //设置接口低电平作为额外地 void SetGNDPins(void) { - int pin_num = sizeof(gnd_pins) / sizeof(uint8_t); + uint8_t pin_num = sizeof(gnd_pins) / sizeof(uint8_t); - for (int i = 0; i < pin_num; i++) + for (uint8_t i = 0; i < pin_num; i++) { pinMode(gnd_pins[i], OUTPUT); digitalWrite(gnd_pins[i], LOW); @@ -515,10 +554,49 @@ void SetGNDPins(void) } +//从 EEPROM 读取数据 +void ReadFromEEPROM(void) +{ + //从 EEPROM 读取调试数据 + EEPROM.get(EEPROM_ADDRESS, stroage_info); + + //转向角度 + north_left_angle = stroage_info.real_angle_val[0]; + north_right_angle = stroage_info.real_angle_val[1]; + west_left_angle = stroage_info.real_angle_val[2]; + west_right_angle = stroage_info.real_angle_val[3]; + south_left_angle = stroage_info.real_angle_val[4]; + south_right_angle = stroage_info.real_angle_val[5]; + east_left_angle = stroage_info.real_angle_val[6]; + east_right_angle = stroage_info.real_angle_val[7]; + + //转向灰度值 + sensor.SetGrayGate(GRAY_7, stroage_info.gray_7_gate); + + //转向时间 + delay_time_after_turn = stroage_info.delay_time_after_turn; + + #ifdef TAICHI_DEBUG + NeoSerial.println("#TAICHI: Data based on EEPROM : "); + NeoSerial.print("north_left_angle: "); NeoSerial.println(north_left_angle); + NeoSerial.print("north_right_angle: "); NeoSerial.println(north_right_angle); + NeoSerial.print("west_left_angle: "); NeoSerial.println(west_left_angle); + NeoSerial.print("west_right_angle: "); NeoSerial.println(west_right_angle); + NeoSerial.print("south_left_angle: "); NeoSerial.println(south_left_angle); + NeoSerial.print("south_right_angle: "); NeoSerial.println(south_right_angle); + NeoSerial.print("east_left_angle: "); NeoSerial.println(east_left_angle); + NeoSerial.print("east_right_angle: "); NeoSerial.println(east_right_angle); + NeoSerial.print("gray_7_gate: "); NeoSerial.println(stroage_info.gray_7_gate); + NeoSerial.print("delay_time_after_turn: "); NeoSerial.println(delay_time_after_turn); + #endif +} + + //在开始运行前依次检测各灰度传感器下方黑白是否正常，若不正常，异常传感器闪烁，程序不继续进行 void CheckGrayStatus(void) { - //若正常，1 2 5 6 号传感器检测到黑色，3 4 7 号传感器检测到白色 + //若正常，1 2 5 6 号传感器检测到黑色，3 4 号传感器检测到白色 + //若一开始就采用灰度转弯，7 号也应检测到白色 bool is_status_right = false; while (!is_status_right) @@ -535,13 +613,13 @@ void CheckGrayStatus(void) sensor.FlashGraySensor(GRAY_5); else if (sensor.IsWhite(GRAY_6)) sensor.FlashGraySensor(GRAY_6); - else if (!sensor.IsWhite(GRAY_7)) + else if (turn_method == TURN_BY_GRAY && !sensor.IsWhite(GRAY_7)) sensor.FlashGraySensor(GRAY_7); else is_status_right = true; } #ifdef TAICHI_DEBUG - Serial.println("#TAICHI: Gray Sensor Status OK!"); + NeoSerial.println("#TAICHI: Gray Sensor Status OK!"); #endif } @@ -670,9 +748,9 @@ void LineForward(uint8_t end_position, float speed_rate) { #ifdef TAICHI_DEBUG //调试输出沿线直行状态 - Serial.print("#TAICHI: Line Forward"); - Serial.print(" end_position: "); - Serial.println((int)end_position); + NeoSerial.print("#TAICHI: Line Forward"); + NeoSerial.print(" end_position: "); + NeoSerial.println((int)end_position); #endif //记录开始时间 @@ -734,7 +812,7 @@ void LineForward(uint8_t end_position, float speed_rate) #ifdef TAICHI_DEBUG //调试输出沿线直行结束 - Serial.println("#TAICHI: End Line Forward"); + NeoSerial.println("#TAICHI: End Line Forward"); #endif } @@ -744,9 +822,9 @@ void LineBackward(uint8_t end_position, float speed_rate) { #ifdef TAICHI_DEBUG //调试输出沿线后退状态 - Serial.print("#TAICHI: Line Backward"); - Serial.print(" end_position: "); - Serial.println((int)end_position); + NeoSerial.print("#TAICHI: Line Backward"); + NeoSerial.print(" end_position: "); + NeoSerial.println((int)end_position); #endif //记录灰度传感器匹配情况 @@ -796,7 +874,7 @@ void LineBackward(uint8_t end_position, float speed_rate) #ifdef TAICHI_DEBUG //调试输出沿线后退结束 - Serial.println("#TAICHI: End Line Backward"); + NeoSerial.println("#TAICHI: End Line Backward"); #endif } @@ -814,7 +892,7 @@ void TurnDirection(float speed_rate) #ifdef TAICHI_DEBUG //调试输出直行或后退或转向状态 - Serial.println("#TAICHI: JUMP THE RELEASE/GETOUT/GAIN POINT FOR STATUS REASON"); + NeoSerial.println("#TAICHI: JUMP THE RELEASE/GETOUT/GAIN POINT FOR STATUS REASON"); #endif } @@ -822,9 +900,9 @@ void TurnDirection(float speed_rate) #ifdef TAICHI_DEBUG //调试输出直行或后退或转向状态 - Serial.print("#TAICHI: Turn Direction"); - Serial.print(" direction: "); - Serial.println((int)direction); + NeoSerial.print("#TAICHI: Turn Direction"); + NeoSerial.print(" direction: "); + NeoSerial.println((int)direction); #endif if (direction == FORWARD) //继续直行 @@ -847,14 +925,62 @@ void TurnDirection(float speed_rate) //旋转 move.MoveDirection(direction, speed_rate); - //等待一定时间后检测是否摆正 - delay(BEFORE_CHECK_STRAIGHT_DELAY); - while (!sensor.IsWhite(GRAY_7)) {} + //角度相关计算 + float next_real_angle; + uint8_t next_real_direction; + if (direction == FORLEFTWARD || direction == BACKRIGHTWARD) + { + switch (current_real_direction) + { + case NORTH: next_real_direction = WEST; next_real_angle = west_left_angle; break; + case WEST: next_real_direction = SOUTH; next_real_angle = south_left_angle; break; + case SOUTH: next_real_direction = EAST; next_real_angle = east_left_angle; break; + case EAST: next_real_direction = NORTH; next_real_angle = north_left_angle; + } + } + else + { + switch (current_real_direction) + { + case NORTH: next_real_direction = EAST; next_real_angle = east_right_angle; break; + case WEST: next_real_direction = NORTH; next_real_angle = north_right_angle; break; + case SOUTH: next_real_direction = WEST; next_real_angle = west_right_angle; break; + case EAST: next_real_direction = SOUTH; next_real_angle = south_right_angle; + } + } + + //以不同方式判定结束 + switch (turn_method) + { + case TURN_BY_TIME: //基于时间 + { + delay(delay_time_after_turn); + } break; + + case TURN_BY_GRAY: //基于灰度传感器 + { + //等待一定时间后检测是否摆正 + delay(BEFORE_CHECK_STRAIGHT_DELAY); + while (!sensor.IsWhite(GRAY_7)) {} + } break; + + case TURN_BY_EARTH: //基于地磁场 + { + //等待一定时间后检测是否摆正 + delay(BEFORE_CHECK_STRAIGHT_DELAY); + + if (direction == FORLEFTWARD || direction == BACKRIGHTWARD) + while (sensor.GetAngle() < next_real_angle) {} + else while (sensor.GetAngle() > next_real_angle) {} + } + } + + current_real_direction = next_real_direction; } #ifdef TAICHI_DEBUG //调试输出直行或后退或转向结束 - Serial.println("#TAICHI: End Turn Direction"); + NeoSerial.println("#TAICHI: End Turn Direction"); #endif } @@ -889,8 +1015,8 @@ bool CatchAndCheck(uint8_t type, float speed) { #ifdef TAICHI_DEBUG //调试输出失败信息 - Serial.print("#TAICHI: **********FAIL CATCH!**********"); - Serial.print(" catch_times: "); Serial.println((int)catch_times); + NeoSerial.print("#TAICHI: **********FAIL CATCH!**********"); + NeoSerial.print(" catch_times: "); NeoSerial.println((int)catch_times); #endif if (type == CATCH_TYPE_GAIN) @@ -924,14 +1050,14 @@ bool CatchAndCheck(uint8_t type, float speed) #ifndef DISABLE_CLAW_CHECK #ifdef TAICHI_DEBUG //调试输出成功信息 - Serial.println("#TAICHI: SUCCUESS CATCH!"); + NeoSerial.println("#TAICHI: SUCCESS CATCH!"); #endif #endif #ifdef DISABLE_CLAW_CHECK #ifdef TAICHI_DEBUG //调试输出结束信息 - Serial.println("#TAICHI: CATCH WITHOUT CHECK!"); + NeoSerial.println("#TAICHI: CATCH WITHOUT CHECK!"); #endif #endif @@ -946,7 +1072,7 @@ bool OpenClawAndCheck(void) #ifdef DISABLE_CLAW_CHECK #ifdef TAICHI_DEBUG //调试输出信息 - Serial.println("#TAICHI: DISABLE CLAW CHECK!"); + NeoSerial.println("#TAICHI: DISABLE CLAW CHECK!"); #endif is_claw_ok = true; @@ -966,7 +1092,7 @@ bool OpenClawAndCheck(void) { #ifdef TAICHI_DEBUG //调试输出成功信息 - Serial.println("#TAICHI: SUCCUESS OPEN CLAW!"); + NeoSerial.println("#TAICHI: SUCCESS OPEN CLAW!"); #endif is_claw_ok = true; @@ -976,9 +1102,16 @@ bool OpenClawAndCheck(void) #ifdef TAICHI_DEBUG //调试输出失败信息 - Serial.println("#TAICHI: $$$$$$$$$$FAIL CLAW!$$$$$$$$$$"); + NeoSerial.println("#TAICHI: $$$$$$$$$$FAIL CLAW!$$$$$$$$$$"); #endif is_claw_ok = false; return false; +} + + +//通讯消息处理函数 +void HandleMessage(char* message) +{ + // } \ No newline at end of file