// @Author : Hcm
#include <LobotServoController.h> // 舵机板通信
#include <OneButton.h> // 按钮库
#include <String.h> // 处理字符串
// 时间都默认加了50
#define KEY_START 7 // 按钮对应的串口
#define FIRST_STOP_ACTION 1 // 初始化立正动作对应位置
#define FIRST_STOP_ACTION_TIME 550 // 需要的时间，下面都类似
#define GO_STRAIGHT 2 // 直行
#define GO_STRAIGHT_TIME 2750
#define GO_LITTLE_STRAIGHT 3 // 小直行
#define GO_LITTLE_STRAIGHT_TIME 3250
#define GO_MICRO_STRAIGHT 4 // 小小直行 8-9cm
#define GO_MICRO_STRAIGHT_TIME 2650
#define GO_MMICRO_STRAIGHT 20 // 小小小直行 6cm
#define GO_MMICRO_STRAIGHT_TIME 2850
#define GO_RIGHT 5 // 右转
#define GP_RIGHT_TIME 2950
#define GO_LITTLE_RIGHT 6 // 小右转
#define GO_LITTLE_RIGHT_TIME 2950
#define GO_LEFT 7 // 左转
#define GO_LEFT_TIME 5150
#define GO_LITTLE_LEFT 8 // 小左转
#define GO_LITTLE_LEFT_TIME 3850
#define GO_LEFT_TRANSVERSE 9 // 原地左胯一步
#define GO_LEFT_TRANSVERSE_TIME 4050
#define GO_RIGHT_TRANSVERSE 10 // 原地右胯一步
#define GO_RIGHT_TRANSVERSE_TIME 3550
#define GO_OBSTACLE 11 // 越障
#define GO_OBSTACLE_TIME 11550
#define GO_WANYAO 12 // 弯腰
#define GO_WANYAO_TIME 1050
#define GO_ZHILI 13 // 直立
#define GO_ZHILI_TIME 550
#define GO_XUNXIAN_STRAIGHT_LEFT 14 // 巡线左脚前行
#define GO_XUNXIAN_STRAIGHT_LEFT_TIME 2350
#define GO_XUNXIAN_STRAIGHT_RIGHT 15 // 巡线右脚前行
#define GO_XUNXIAN_STRAIGHT_RIGHT_TIME 2150
#define GO_TURN_HEAD_RIGHT 16 // 右转头
#define GO_TURN_HEAD_RIGHT_TIME 250
#define GO_TURN_HEAD_LEFT 17 // 左转头
#define GO_TURN_HEAD_LEFT_TIME 250
#define GO_TURN_ROUND_RIGHT 18 // 原地右转
#define GO_TURN_ROUND_RIGHT_TIME 4550
#define GO_TURN_ROUND_LEFT 19 // 原地左转
#define GO_TURN_ROUND_LEFT_TIME 3050
#define GO_LEFT_TURN 21 // 转头左转
#define GO_LEFT_TURN_TIME 5150
#define GO_LITTLE_LEFT_TURN 22 // 转头小左转
#define GO_LITTLE_LEFT_TURN_TIME 3850
#define GO_XUNXIAN_STRAIGHT_TURN_LEFT 23 // 摇头巡线左脚前行
#define GO_XUNXIAN_STRAIGHT_TURN_LEFT_TIME 2350
#define GO_XUNXIAN_STRAIGHT_TURN_RIGHT 24 // 摇头巡线右脚前行
#define GO_XUNXIAN_STRAIGHT_TURN_RIGHT_TIME 2150
LobotServoController myse; // 实例化舵机通信
OneButton button_2(KEY_START, false); // 实例化按钮 true:按下为低电平 false : 按下为高电平
// 变量定义
int first_code = 0; // 第一个二维码
int second_code = 0; // 第二个二维码
int default_first_code = 5; // 第一个默认代码
int default_second_code = 6; // 第二个默认代码
int code_num = 0; // 记录扫了几个二维码
int find_code_num = 0; // 记录找到了几个二维码
bool ready_to_start = false; // 开始比赛
int game_station = 1; // 比赛模式
int first_turn_count = 0; // 第一个弯道走的步数统计
bool wait_code = false; // 是否在等待二维码
int left_or_right = 1; // 1代表走左脚， 2代表走右脚
unsigned long time_long = 2000; // 扫码等待时间
int turn_head_count = 0; // 记录在扫码等待时转了几次头
bool already_cross = false; // 记录是否已经越障
typedef struct
{
int len;
String data;
} mes_v831;
// 函数定义
void clickonce2start();
void clicklong2change();
String detectString_openmv();
String detectString_v831();
int recv_order();
mes_v831 recv_mes_v831();
void send_to_ticao(int a, int b);
void find_line();
void cross_barr();
void ready_to_scan();
bool count_timer(unsigned long start_time, unsigned long time_long); // 定时器，传入一个开始时间，一个预定的最长停止时间，若未超时，返回False， 超时返回True
void go_by_order(int action, int times); // 通过传参数来使其运动
void first_stop(); // 初始化动作
void go_straight(); // 直行
void go_little_straight(); // 小直行
void go_micro_straight(); // 小小直行
void go_mmicro_straight(); // 小小小直行
void go_right(); // 右转
void go_little_right(); // 小右转
void go_left(); // 左转
void go_little_left(); // 小左转
void go_left_transverse(); // 原地左胯一步
void go_right_transverse(); // 原地右胯一步
void go_obstacle(); // 越障
void go_wanyao(); // 弯腰
void go_zhili(); // 直立
void go_xunxian_straight(); // 巡线直行 内置左右脚
void go_turn_head_right(); // 右转头
void go_turn_head_left(); // 左转头
void go_turn_round_right(); // 原地右转
void go_turn_round_left(); // 原地左转
void go_left_turn(); // 转头左转
void go_little_left_turn(); // 转头小左转
void go_xunxian_straight_turn(); // 转头巡线直行
void setup() {
Serial.begin(9600); // 舵机口通信
Serial1.begin(57600); // 蓝牙通信
Serial2.begin(19200); // openmv通信
Serial3.begin(19200); // v831通信
button_2.reset(); //清除一下按钮状态机的状态
button_2.attachClick(clickonce2start); // 单击
button_2.attachLongPressStart(clicklong2change); // 长按
Serial.println("Ready!");
}
void loop() {
delay(1000);
first_stop();
// ready_to_start = true;
while (!(ready_to_start)) {
button_2.tick();
}
ready_to_start = false;
Serial.println("Start game!"); // 调试用
// send_to_ticao(2,3);
if (game_station == 1) {
while (1) {
if (wait_code) {
unsigned long begin_time = millis();
turn_head_count = 0; // 状态清零
while (1) {
if (Serial3.available()) {
break;
}
if (!count_timer(begin_time, time_long)) // 增加此定时器，提高鲁棒性和容错率
{
if (turn_head_count == 2) {
Serial3.print("n"); //告诉摄像头别等这个二维码了，继续推进状态机
go_zhili();
break;
} else if (turn_head_count == 1) {
turn_head_count++;
begin_time = millis();
go_turn_head_left();
} else if (turn_head_count == 0) {
turn_head_count++;
begin_time = millis();
go_turn_head_right();
}
}
}
wait_code = false;
}
if (Serial3.available()) {
mes_v831 mes = recv_mes_v831();
Serial.print(mes.data); // 打印接收到的v831的信息，调试用
if (mes.data[0] == 'c') // 接收二维码
{
// Serial.println("123");
// Serial.print(mes.data.substring(1, mes.len).toInt());
if (code_num == 0) {
code_num++;
first_code = mes.data.substring(1, mes.len).toInt();
} else if (code_num == 1) {
code_num++;
second_code = mes.data.substring(1, mes.len).toInt();
}
delay(3000); // 扫到二维码后等待3秒
go_zhili(); // 等待结束后直立
} else if (mes.data[0] == 'r') // 越障
{
// go_little_left();
// go_turn_head_left();
// go_micro_straight();
// go_micro_straight();
// go_obstacle();
// Serial3.print("b");
// go_little_left();
// cross_barr();
} else if (mes.data[0] == 'a') // 找到二维码的位置了
{
find_code_num++; // 找到二维码就自动＋1
ready_to_scan();
continue;
}
}
find_line();
if (code_num == 2) {
send_to_ticao(first_code, second_code);
}
if (find_code_num == 2) {
if (code_num == 0) {
send_to_ticao(default_first_code, default_second_code);
} else if (code_num == 1) {
if (first_code == default_first_code) {
send_to_ticao(first_code, default_second_code);
} else {
send_to_ticao(first_code, default_first_code);
}
}
}
}
} else {
}
}
void loop3(){
while (!(ready_to_start)) {
button_2.tick();
}
ready_to_start = false;
Serial.print("Success send!");
send_to_ticao(5, 6);
}
// 测试延时函数有没有问题
void loop2() {
unsigned long begin_time = millis();
while (count_timer(begin_time, time_long)) {
}
while (1)
;
}
// 测试加了flush后串口还会不会有莫名奇妙的数据残留
void loop1() {
Serial.println("begin_test!");
mes_v831 mes = recv_mes_v831();
Serial.print("remain_bytes:");
Serial.println(Serial3.available());
}
void clickonce2start() {
ready_to_start = true;
}
void clicklong2change() {
if (game_station == 1) {
game_station = 2;
} else {
game_station = 1;
}
Serial.print("change_mode");
}
String detectString_openmv() // 判断传入的字符串能否被接收
{
// 我们传入的数据形式 {int&}
while (Serial2.read() != '{')
;
//返回"{}"中的字符串
return (Serial2.readStringUntil('}'));
}
int recv_order() // 接收openmv指令
{
int order;
while (1) {
if (Serial2.available()) break;
}
String s = detectString_openmv();
Serial.println(s); // 调试用
Serial2.flush(); // 清除openmv串口现在还有的缓存数据
String numStr = "";
for (int i = 0; i < s.length(); i++) {
if (s[i] == '&') // 终止标志
{
order = numStr.toInt();
} else {
numStr += s[i];
}
}
return order; // 指令
}
String detectString_v831() {
// 我们传入的数据形式 {int&}
while (Serial3.read() != '{')
;
//返回"{}"中的字符串
return (Serial3.readStringUntil('}'));
}
mes_v831 recv_mes_v831() {
mes_v831 mes;
while (1) {
if (Serial3.available()) break;
}
String s = detectString_v831();
// Serial.print("get_code:");
Serial.println(s);
Serial3.flush(); // 清除v831串口的缓存数据，防止误识别
String new_str = "";
mes.len = s.length();
for (int i = 0; i < mes.len; i++) {
if (s[i] == '&') // 终止标志
{
mes.data = new_str;
} else {
new_str += s[i];
}
}
return mes;
}
void send_to_ticao(int a, int b) {
if (a == 1 && b == 2) {
Serial1.print('a');
}
if (a == 1 && b == 3) {
Serial1.print('b');
}
if (a == 1 && b == 4) {
Serial1.print('c');
}
if (a == 1 && b == 5) {
Serial1.print('d');
}
if (a == 1 && b == 6) {
Serial1.print('e');
}
if (a == 1 && b == 7) {
Serial1.print('f');
}
if (a == 1 && b == 8) {
Serial1.print('g');
}
if (a == 2 && b == 1) {
Serial1.print('h');
}
if (a == 2 && b == 3) {
Serial1.print('i');
}
if (a == 2 && b == 4) {
Serial1.print('j');
}
if (a == 2 && b == 5) {
Serial1.print('k');
}
if (a == 2 && b == 6) {
Serial1.print('l');
}
if (a == 2 && b == 7) {
Serial1.print('m');
}
if (a == 2 && b == 8) {
Serial1.print('n');
}
if (a == 3 && b == 1) {
Serial1.print('o');
}
if (a == 3 && b == 2) {
Serial1.print('p');
}
if (a == 3 && b == 4) {
Serial1.print('q');
}
if (a == 3 && b == 5) {
Serial1.print('r');
}
if (a == 3 && b == 6) {
Serial1.print('s');
}
if (a == 3 && b == 7) {
Serial1.print('t');
}
if (a == 3 && b == 8) {
Serial1.print('u');
}
if (a == 4 && b == 1) {
Serial1.print('v');
}
if (a == 4 && b == 2) {
Serial1.print('w');
}
if (a == 4 && b == 3) {
Serial1.print('x');
}
if (a == 4 && b == 5) {
Serial1.print('y');
}
if (a == 4 && b == 6) {
Serial1.print('z');
}
if (a == 4 && b == 7) {
Serial1.print('A');
}
if (a == 4 && b == 8) {
Serial1.print('B');
}
if (a == 5 && b == 1) {
Serial1.print('C');
}
if (a == 5 && b == 2) {
Serial1.print('D');
}
if (a == 5 && b == 3) {
Serial1.print('E');
}
if (a == 5 && b == 4) {
Serial1.print('F');
}
if (a == 5 && b == 6) {
Serial1.print('G');
}
if (a == 5 && b == 7) {
Serial1.print('H');
}
if (a == 5 && b == 8) {
Serial1.print('I');
}
if (a == 6 && b == 1) {
Serial1.print('J');
}
if (a == 6 && b == 2) {
Serial1.print('K');
}
if (a == 6 && b == 3) {
Serial1.print('L');
}
if (a == 6 && b == 4) {
Serial1.print('M');
}
if (a == 6 && b == 5) {
Serial1.print('N');
}
if (a == 6 && b == 7) {
Serial1.print('O');
}
if (a == 6 && b == 8) {
Serial1.print('P');
}
if (a == 7 && b == 1) {
Serial1.print('Q');
}
if (a == 7 && b == 2) {
Serial1.print('R');
}
if (a == 7 && b == 3) {
Serial1.print('S');
}
if (a == 7 && b == 4) {
Serial1.print('T');
}
if (a == 7 && b == 5) {
Serial1.print('U');
}
if (a == 7 && b == 6) {
Serial1.print('V');
}
if (a == 7 && b == 8) {
Serial1.print('W');
}
if (a == 8 && b == 1) {
Serial1.print('X');
}
if (a == 8 && b == 2) {
Serial1.print('Y');
}
if (a == 8 && b == 3) {
Serial1.print('Z');
}
if (a == 8 && b == 4) {
Serial1.print('+');
}
if (a == 8 && b == 5) {
Serial1.print('-');
}
if (a == 8 && b == 6) {
Serial1.print('*');
}
if (a == 8 && b == 7) {
Serial1.print('/');
}
if (b == '0' || a == b) {
Serial1.print(a);
}
}
// 此函数下是摇头前进
void find_line() {
if(already_cross){
Serial2.print("g"); // 向openmv申请下一个动作
}else{
Serial2.print("r"); // 向openmv申请下一个动作
}
int order = recv_order();
Serial.println(order); // 打印接收的指令，调试用
switch (order) {
case 2: /*go_straight();*/ /*go_xunxian_straight();*/ go_xunxian_straight_turn(); break;
case 3: go_little_straight(); break;
case 4: go_micro_straight(); break;
case 5: go_right(); break;
case 6: go_little_right(); break;
case 7: /*go_left();*/ go_left_turn(); break;
case 8: /*go_little_left();*/ go_little_left_turn(); break;
case 9: go_left_transverse(); break;
case 10: go_right_transverse(); break;
case 11: go_obstacle(); already_cross=true; Serial3.print("o"); break;
case 18: go_turn_round_right(); break;
case 19: go_turn_round_left(); break;
case 20: go_mmicro_straight(); break;
case 99: go_obstacle(); already_cross=true; Serial3.print("o"); break;
case 98: go_turn_round_left(); go_right_transverse(); break;
case 97: go_right_transverse(); go_right_transverse(); go_obstacle(); already_cross=true; Serial3.print("o"); break;
}
}
void cross_barr() {
bool is_cross = false;
while (1) {
Serial3.print("c");
mes_v831 mes = recv_mes_v831();
// int order = mes.data.substring(1, mes.len).toInt();
int order = mes.data.toInt();
Serial.println(order);
switch (order) {
case 2: go_straight(); break;
case 3: go_little_straight(); break;
case 4: go_micro_straight(); break;
case 5: go_right(); break;
case 6: go_little_right(); break;
case 7: go_left(); break;
case 8: go_little_left(); break;
case 9: go_left_transverse(); break;
case 10: go_right_transverse(); break;
case 11:
go_obstacle();
is_cross = true;
Serial3.print("b"); // 告诉v831退出越障扫描模式
break;
case 17: go_xunxian_straight(); break;
case 18: go_turn_round_right(); break;
case 19: go_turn_round_left(); break;
case 20:
go_xunxian_straight();
go_micro_straight();
break;
}
if (is_cross) {
break;
}
}
}
void ready_to_scan() {
while (1) {
Serial3.print("f");
mes_v831 mes = recv_mes_v831();
// 下面的方法是废弃的，已转变为接收到啥就咋运动
int middle_y = mes.data.substring(1, mes.len).toInt(); // middly_y 越小越接近底部
Serial.println(middle_y);
// 下面进入判断
if (middle_y == 1000) {
go_wanyao();
break;
} else if (middle_y <= 85) {
go_xunxian_straight();
go_wanyao();
break;
} else if (middle_y <= 125) {
go_little_straight();
go_wanyao();
break;
} else {
go_straight();
delay(600); // 尝试添加延时函数，取消抖动
}
// int order = mes.data.substring(1, mes.len).toInt();
// Serial.println(order);
// switch (order) {
// case 2: go_straight(); break;
// case 3: go_little_straight(); break;
// case 4: go_micro_straight(); break;
// case 5: go_right(); break;
// case 6: go_little_right(); break;
// case 7: go_left(); break;
// case 8: go_little_left(); break;
// case 9: go_left_transverse(); break;
// case 10: go_right_transverse(); break;
// case 11:
// go_obstacle();
// is_cross = true;
// break;
// // 下面几个case是组合动作输出的
// case 30: break;
// case 31: break;
// case 32: break;
// case 33: break;
// }
// if (is_cross) {
// break;
// }
}
}
bool count_timer(unsigned long start_time, unsigned long time_long) {
// Serial.println(millis() - start_time);
if (millis() - start_time <= time_long) {
return true;
} else {
return false;
}
}
void go_by_order(int action, int times) {
myse.runActionGroup(action, 1);
delay(times);
}
void first_stop() {
myse.runActionGroup(FIRST_STOP_ACTION, 1);
delay(FIRST_STOP_ACTION_TIME);
}
void go_straight() {
myse.runActionGroup(GO_STRAIGHT, 1);
delay(GO_STRAIGHT_TIME);
// first_stop();
}
void go_little_straight() {
myse.runActionGroup(GO_LITTLE_STRAIGHT, 1);
delay(GO_LITTLE_STRAIGHT_TIME);
}
void go_micro_straight() {
myse.runActionGroup(GO_MICRO_STRAIGHT, 1);
delay(GO_MICRO_STRAIGHT_TIME);
}
void go_right() {
myse.runActionGroup(GO_RIGHT, 1);
delay(GP_RIGHT_TIME);
}
void go_little_right() {
myse.runActionGroup(GO_LITTLE_RIGHT, 1);
delay(GO_LITTLE_RIGHT_TIME);
}
void go_left() {
myse.runActionGroup(GO_LEFT, 1);
delay(GO_LEFT_TIME);
// first_stop();
}
void go_little_left() {
myse.runActionGroup(GO_LITTLE_LEFT, 1);
delay(GO_LITTLE_LEFT_TIME);
// first_stop();
}
void go_left_transverse() {
myse.runActionGroup(GO_LEFT_TRANSVERSE, 1);
delay(GO_LEFT_TRANSVERSE_TIME);
}
void go_right_transverse() {
myse.runActionGroup(GO_RIGHT_TRANSVERSE, 1);
delay(GO_RIGHT_TRANSVERSE_TIME);
}
void go_obstacle() {
myse.runActionGroup(GO_OBSTACLE, 1);
delay(GO_OBSTACLE_TIME);
}
void go_wanyao() {
myse.runActionGroup(GO_WANYAO, 1);
delay(GO_WANYAO_TIME);
Serial3.print("b");
wait_code = true;
}
void go_zhili() {
myse.runActionGroup(GO_ZHILI, 1);
delay(GO_ZHILI_TIME);
}
void go_xunxian_straight() {
if (left_or_right == 1) {
myse.runActionGroup(GO_XUNXIAN_STRAIGHT_LEFT, 1);
delay(GO_XUNXIAN_STRAIGHT_LEFT_TIME);
left_or_right = 2;
} else {
myse.runActionGroup(GO_XUNXIAN_STRAIGHT_RIGHT, 1);
delay(GO_XUNXIAN_STRAIGHT_RIGHT_TIME);
left_or_right = 1;
}
}
void go_turn_head_right() {
myse.runActionGroup(GO_TURN_HEAD_RIGHT, 1);
delay(GO_TURN_HEAD_RIGHT_TIME);
}
void go_turn_head_left() {
myse.runActionGroup(GO_TURN_HEAD_LEFT, 1);
delay(GO_TURN_HEAD_LEFT_TIME);
}
void go_turn_round_right() {
myse.runActionGroup(GO_TURN_ROUND_RIGHT, 1);
delay(GO_TURN_ROUND_RIGHT_TIME);
}
void go_turn_round_left() {
myse.runActionGroup(GO_TURN_ROUND_LEFT, 1);
delay(GO_TURN_ROUND_LEFT_TIME);
}
void go_left_turn() {
myse.runActionGroup(GO_LEFT_TURN, 1);
delay(GO_LEFT_TURN_TIME);
}
void go_little_left_turn() {
myse.runActionGroup(GO_LITTLE_LEFT_TURN, 1);
delay(GO_LITTLE_LEFT_TURN_TIME);
}
void go_xunxian_straight_turn() {
if (left_or_right == 1) {
myse.runActionGroup(GO_XUNXIAN_STRAIGHT_TURN_LEFT, 1);
delay(GO_XUNXIAN_STRAIGHT_TURN_LEFT_TIME);
left_or_right = 2;
} else {
myse.runActionGroup(GO_XUNXIAN_STRAIGHT_TURN_RIGHT, 1);
delay(GO_XUNXIAN_STRAIGHT_TURN_RIGHT_TIME);
left_or_right = 1;
}
}
void go_mmicro_straight(){
myse.runActionGroup(GO_MMICRO_STRAIGHT, 1);
delay(GO_MMICRO_STRAIGHT_TIME);
}
浙公网安备 33010602011771号