# 2024RCJ上海赛 机器人微型迷宫救援项目 初中组（唐开文 李赞 柳泽诚）

**Repository Path**: HKQQSNHDZX/002

## Basic Information

- **Project Name**: 2024RCJ上海赛 机器人微型迷宫救援项目 初中组（唐开文 李赞 柳泽诚）
- **Description**: 上海赛机器人微型迷宫救援项目初中组2
- **Primary Language**: Unknown
- **License**: Not specified
- **Default Branch**: master
- **Homepage**: None
- **GVP Project**: No

## Statistics

- **Stars**: 0
- **Forks**: 0
- **Created**: 2024-03-16
- **Last Updated**: 2024-03-23

## Categories & Tags

**Categories**: Uncategorized

**Tags**: None

## README

# 2024RCJ上海赛机器人微型迷宫救援项目初中组2

## 比赛规则
此次比赛规则在附件

## 全车分布
1识别救援摄像头
2判断位置的前后左右测距和陀螺仪。
3电机马达


## 地图
从规则可知，为了拿到更多的分数应该将每个方块都踩一遍。在判断每个出发点时，我们发现每个出发点都很有特点，有的是前后都有墙壁，有的是就右边有墙壁，我们利用这一点用我们做加装的四个方向的测距来判断S1~S6
![输入图片说明](1234Map.2.0.png)

## 原代码

```
/*----------------------------------------------------------------------------*/
/*                                                                            */
/*    Module:       main.cpp                                                  */
/*    Author:       Kelvin                                                    */
/*    Created:      2024/1/11                                                 */
/*    Description:  IQ2 project                                               */
/*                                                                            */
/*----------------------------------------------------------------------------*/

#pragma region VEXcode Generated Robot Configuration
// Make sure all required headers are included.
#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>
#include <math.h>
#include <string.h>


#include "vex.h"

using namespace vex;

// Brain should be defined by default
brain Brain;


// START IQ MACROS
#define waitUntil(condition)                                                   \
  do {                                                                         \
    wait(5, msec);                                                             \
  } while (!(condition))

#define repeat(iterations)                                                     \
  for (int iterator = 0; iterator < iterations; iterator++)
// END IQ MACROS


// Robot configuration code.
inertial BrainInertial = inertial();
motor LeftMotor = motor(PORT6, false);
motor RightMotor = motor(PORT1, true);
distance FrontDistance = distance(PORT5);
distance LeftDistance = distance(PORT7);
distance RightDistance = distance(PORT12);
touchled TouchLED = touchled(PORT8);
distance BehindDistance = distance(PORT9);
/*vex-vision-config:begin*/
vision::signature Vision11__IMAN = vision::signature (1, -9025, -5431, -7228,-9137, -5399, -7268,3, 0);
vision::signature Vision11__SIG_2 = vision::signature (2, 0, 0, 0,0, 0, 0,3, 0);
vision::signature Vision11__SIG_3 = vision::signature (3, 0, 0, 0,0, 0, 0,3, 0);
vision::signature Vision11__SIG_4 = vision::signature (4, 0, 0, 0,0, 0, 0,3, 0);
vision::signature Vision11__SIG_5 = vision::signature (5, 0, 0, 0,0, 0, 0,3, 0);
vision::signature Vision11__SIG_6 = vision::signature (6, 0, 0, 0,0, 0, 0,3, 0);
vision::signature Vision11__SIG_7 = vision::signature (7, 0, 0, 0,0, 0, 0,3, 0);
vision Vision11 = vision (PORT11, 150, Vision11__IMAN, Vision11__SIG_2, Vision11__SIG_3, Vision11__SIG_4, Vision11__SIG_5, Vision11__SIG_6, Vision11__SIG_7);
/*vex-vision-config:end*/

#pragma endregion VEXcode Generated Robot Configuration

#define gyro1 BrainInertial.rotation(degrees)   // 陀螺仪 转向值
#define gyro2 BrainInertial.heading(degrees)    // 陀螺仪 归位值
#define frontd FrontDistance.objectDistance(mm) // 前测距
#define backd BehindDistance.objectDistance(mm) // 后测距
#define leftd LeftDistance.objectDistance(mm)   // 左测距
#define rightd RightDistance.objectDistance(mm) // 右测距

// Include the IQ Library
#include "vex.h"

// Allows for easier use of the VEX Library
using namespace vex;

/********全局变量定义********/

int beginPlace = 1; // 启动位置
float tt, k;

/********子函数声明********/

// 逻辑子函数
void reset(void);          // 初始化
int JudgingStarting(void); // 判断起点
bool findObject(void);     // 是否检测到绿色
float getGyroValue(void);  // 获取陀螺仪数据(废弃)

// 运动子函数
void run(int leftSpeed, int rightSpeed);                 // 机器人驱动
void runTime(int leftSpeed, int rightSpeed, float time); // 机器人驱动 时间
void runByWallLeft(float time);                          // 沿墙行走(左测距)
void runByWallRight(float time);                         // 沿墙行走(右测距)
void turn(int degree);                                   // 原地旋转
void turnTo(int degree);                                 // 旋转至
void stop(void);                                         // 停止运动
void table(int n);                                       // 运动n格
void gotoWall(void);                                     // 行走到墙壁
void gotoWall2(void);                                    // 行动到离墙壁一格的距离(36cm)
void gotoWall3(void);                                    // 行动到离墙壁2格的距离
void gotoBackWall(void);                                 // 行动到后墙壁
void runBy_gotoWall(char direction, float time);         // 沿墙走到墙壁
void gotoTableWithWall(char direction);                  // 前进到有墙壁的格子(1格)

// 行为子函数
void Starting(void);         // 调用路线
void flashLight(int times);  // 闪灯
void identifyInjured(int m); // 识别伤员
void display(void);          // 显示传感器数值
void light30(void);

// 行进路线子函数
void corner(void); // 拐角处(C字形)
void S1(void);     // 1号出发位
void S2(void);     // 2号出发位
void S3(void);     // 3号出发位
void S4(void);     // 4号出发位
void S5(void);     // 5号出发位
void S6(void);     // 6号出发位

/********主函数********/
int main() {
    reset();
    thread Thread1 = thread(display); // 创建 显示数值 进程
    Starting();
    return 0;
}

/**
 * @name Starting
 * @brief 根据出发位置调用路线
 */
void Starting(void) {
    if (beginPlace == 1)
        S1();
    if (beginPlace == 2)
        S2();
    if (beginPlace == 3)
        S3();
    if (beginPlace == 4)
        S4();
    if (beginPlace == 5)
        S5();
    if (beginPlace == 6)
        S6();
}

void S1(void) {
    turn(-90);
    table(1); // S1-A1
    turn(-90);
    table(1); // A1-S6
    turn(-90);
    table(1); // S6-B2
    gotoWall();
    turn(90);
    table(1); // B2-V3
    identifyInjured(1);
    gotoWall(); // V3-D2
    turn(90);
    runBy_gotoWall('l', 0.9); // D2-D1
    turn(180);
    runBy_gotoWall('r', 2.7); // D1-D2-S5-D4
    turn(180);
    runByWallLeft(0.9); // D4-S5
    turn(90);
    table(1); // S5-C3
    turn(-90);
    gotoBackWall(); // C3-C4
    gotoWall();     // C4-C3
    turn(90);
    runByWallLeft(1.0); // C3-B3
    turn(90);
    gotoWall(); // B3-B4-b5
    turn(90);
    runTime(80, 80, 0.5);
    runByWallRight(1.5); // B5-C5-D5
    gotoWall();
    turn(-90);
    runBy_gotoWall('r', 0.9);
    turn(180);
    runBy_gotoWall('l', 0.9);
    turn(90);
    gotoBackWall();
    runByWallLeft(0.9); // D5-C5
    turn(90);
    runTime(70, 70, 0.5);
    runBy_gotoWall('r', 1.5); // C5-C6-C7-S3
    turn(90);
    runByWallLeft(0.9); // S3-D8
    gotoWall();
    turn(90);
    runByWallLeft(0.9); // D8-D7
    gotoWall();
    turn(180);
    runByWallRight(0.9); // D7-D8
    gotoWall();
    turn(-90);
    runByWallRight(2.7); // D8-S3-B8-A8
    gotoWall();
    turn(180);
    runByWallLeft(0.9); // A8-B8
    turn(90);
    gotoWall(); // B8-B7
    turn(-90);
    gotoWall(); // B7-C7
    turn(90);
    runByWallLeft(1.0); // C7-C6
    gotoWall2();
    turn(90);
    table(1); // C6-V2
    identifyInjured(2);
    gotoWall(); // V2-S2
    turn(90);
    runBy_gotoWall('l', 0.9); // S2-A7
    turn(180);
    runBy_gotoWall('r', 1.8); // A7-A6-A5
    turn(-90);
    table(1); // A5-B5
    turn(90);
    table(1); // B5-B4
    gotoWall2();
    turn(90);
    corner(); // B4-A4-B4
    gotoWall();
    turn(90);
    gotoWall(); // B4-B3
    turn(90);
    gotoTableWithWall('r'); // B3-V1
    turn(-90);
    identifyInjured(2);
    turn(10);
    runByWallRight(0.9); // V1-S1
    return;
}

void S2(void) {
    turn(90);
    runBy_gotoWall('l', 0.9); // S2-A7
    turn(180);
    runBy_gotoWall('r', 1.8); // A7-A6-A5
    turn(-90);
    table(1); // A5-B5
    gotoWall3();
    turn(90);
    table(1); // B5-B4
    gotoWall2();
    turn(90);
    corner(); // B4-A4-B4
    gotoWall();
    turn(90);
    gotoWall(); // B4-B3
    turn(90);
    gotoTableWithWall('r'); // B3-V1
    turn(-90);
    identifyInjured(2);
    turn(10);
    runByWallRight(1.8); // V1-A1
    gotoWall();
    turn(-90);
    table(1); // A1-S6
    turn(-90);
    table(1); // S6-B2
    gotoWall();
    turn(90);
    table(1); // B2-V3
    identifyInjured(1);
    gotoWall(); // V3-D2
    turn(90);
    runBy_gotoWall('l', 0.9); // D2-D1
    turn(180);
    runBy_gotoWall('r', 2.7); // D1-D2-S5-D4
    turn(180);
    runByWallLeft(0.9); // D4-S5
    turn(90);
    table(1); // S5-C3
    turn(-90);
    gotoBackWall(); // C3-C4
    gotoWall();     // C4-C3
    turn(90);
    runByWallLeft(1.0); // C3-B3
    turn(95);
    gotoWall(); // B3-B4-b5
    turn(90);
    runTime(80, 80, 0.5);
    runByWallRight(1.5); // B5-C5-D5
    gotoWall();
    turn(-90);
    runBy_gotoWall('r', 0.9); // D5-S4
    turn(180);
    runBy_gotoWall('l', 0.9); // S4-D5
    turn(90);
    gotoBackWall();
    runByWallLeft(1.1); // D5-C5
    turn(90);
    runTime(70, 70, 0.5);
    runBy_gotoWall('r', 1.5); // C5-C6-C7-S3
    turn(90);
    runByWallLeft(0.9); // S3-D8
    gotoWall();
    turn(90);
    runByWallLeft(0.9); // D8-D7
    gotoWall();
    turn(180);
    runByWallRight(0.9); // D7-D8
    gotoWall();
    turn(-90);
    runByWallRight(2.7); // D8-S3-B8-A8
    gotoWall();
    turn(180);
    runByWallLeft(0.9); // A8-B8
    turn(90);
    gotoWall(); // B8-B7
    turn(-90);
    gotoWall(); // B7-C7
    turn(90);
    runByWallLeft(1.0); // C7-C6
    gotoWall2();
    turn(90);
    table(1); // C6-V2
    identifyInjured(2);
    gotoWall(); // V2-S2
    return;
}

void S3(void) {
    turn(180);
    runByWallLeft(0.9); // S3-D8
    gotoWall();
    turn(90);
    runByWallLeft(0.9); // D8-D7
    gotoWall();
    turn(180);
    runByWallRight(0.9); // D7-D8
    gotoWall();
    turn(-90);
    runByWallRight(2.7); // D8-S3-B8-A8
    gotoWall();
    turn(180);
    runByWallLeft(0.9); // A8-B8
    turn(90);
    gotoWall(); // B8-B7
    turn(-90);
    gotoWall(); // B7-C7
    turn(90);
    runByWallLeft(1.0); // C7-C6
    gotoWall2();
    turn(0 - gyro2);
    table(1); // C6-V2
    identifyInjured(2);
    gotoWall(); // V2-S2
    turn(90);
    runBy_gotoWall('l', 0.9); // S2-A7
    turn(180);
    runBy_gotoWall('r', 1.8); // A7-A6-A5
    turn(-90);
    table(1); // A5-B5
    gotoWall3();
    turn(90);
    table(1); // B5-B4
    gotoWall2();
    turn(90);
    corner(); // B4-A4-B4
    gotoWall();
    turn(90);
    gotoWall(); // B4-B3
    turn(90);
    gotoTableWithWall('r'); // B3-V1
    turn(-90);
    identifyInjured(2);
    turn(10);
    runByWallRight(1.8); // V1-A1
    gotoWall();
    turn(-90);
    table(1); // A1-S6
    turn(-90);
    table(1); // S6-B2
    gotoWall();
    turn(90);
    table(1); // B2-V3
    identifyInjured(1);
    gotoWall(); // V3-D2
    turn(90);
    runBy_gotoWall('l', 0.9); // D2-D1
    turn(180);
    runBy_gotoWall('r', 2.7); // D1-D2-S5-D4
    turn(180);
    runByWallLeft(0.9); // D4-S5
    turn(90);
    table(1); // S5-C3
    turn(-90);
    gotoBackWall(); // C3-C4
    gotoWall();     // C4-C3
    turn(90);
    runByWallLeft(1.0); // C3-B3
    turn(95);
    gotoWall(); // B3-B4-b5
    turn(90);
    runTime(80, 80, 0.5);
    runByWallRight(1.5); // B5-C5-D5
    gotoWall();
    turn(-90);
    runBy_gotoWall('r', 0.9);
    turn(180);
    runBy_gotoWall('l', 0.9);
    turn(90);
    gotoBackWall();
    runByWallLeft(1.1); // D5-C5
    turn(90);
    runTime(70, 70, 0.5);
    runByWallRight(1.5); // C5-C6-C7-S3
    turn(90 - gyro2);
    gotoWall();
    return;
}

void S4(void) {
    turn(-90);
    runBy_gotoWall('l', 0.9); // S4-D5
    turn(90);
    gotoBackWall();
    runByWallLeft(1.1); // D5-C5
    turn(90);
    runTime(70, 70, 0.5);
    runBy_gotoWall('r', 1.5); // C5-C6-C7-S3
    turn(90);
    runByWallLeft(0.9); // S3-D8
    gotoWall();
    turn(90);
    runByWallLeft(0.9); // D8-D7
    gotoWall();
    turn(180);
    runByWallRight(0.9); // D7-D8
    gotoWall();
    turn(-90);
    runByWallRight(2.7); // D8-S3-B8-A8
    gotoWall();
    turn(180);
    runByWallLeft(0.9); // A8-B8
    turn(90);
    gotoWall(); // B8-B7
    turn(-90);
    gotoWall(); // B7-C7
    turn(90);
    runByWallLeft(1.0); // C7-C6
    gotoWall2();
    turn(90);
    table(1); // C6-V2
    identifyInjured(2);
    gotoWall(); // V2-S2
    turn(90);
    runBy_gotoWall('l', 0.9); // S2-A7
    turn(180);
    runBy_gotoWall('r', 1.8); // A7-A6-A5
    turn(-90);
    table(1); // A5-B5
    turn(90);
    table(1); // B5-B4
    gotoWall2();
    turn(90);
    corner(); // B4-A4-B4
    gotoWall();
    turn(90);
    gotoWall(); // B4-B3
    turn(90);
    gotoTableWithWall('r'); // B3-V1
    turn(-90);
    identifyInjured(2);
    turn(10);
    runByWallRight(1.8); // V1-A1
    gotoWall();
    turn(-90);
    table(1); // A1-S6
    turn(-90);
    table(1); // S6-B2
    gotoWall();
    turn(90);
    table(1); // B2-V3
    identifyInjured(1);
    gotoWall(); // V3-D2
    turn(90);
    runBy_gotoWall('l', 0.9); // D2-D1
    turn(180);
    runBy_gotoWall('r', 2.7); // D1-D2-S5-D4
    turn(180);
    runByWallLeft(0.9); // D4-S5
    turn(90);
    table(1); // S5-C3
    turn(-90);
    gotoBackWall(); // C3-C4
    gotoWall();     // C4-C3
    turn(90);
    runByWallLeft(1.0); // C3-B3
    turn(95);
    gotoWall(); // B3-B4-b5
    turn(90);
    runTime(80, 80, 0.5);
    runByWallRight(1.5); // B5-C5-D5
    gotoWall();
    turn(-90);
    runBy_gotoWall('r', 0.9); // D5-S4
    return;
}

void S5(void) {
    turn(90);
    runBy_gotoWall('r', 0.9); // S5-D4
    turn(180);
    runByWallLeft(0.9); // D4-S5
    turn(90);
    table(1); // S5-C3
    turn(-90);
    gotoBackWall(); // C3-C4
    gotoWall();     // C4-C3
    turn(90);
    runByWallLeft(1.0); // C3-B3
    turn(90);
    gotoWall(); // B3-B4-b5
    turn(90);
    runTime(80, 80, 0.5);
    runByWallRight(1.5); // B5-C5-D5
    gotoWall();
    turn(-90);
    runBy_gotoWall('r', 0.9); // D5-S4
    turn(-90);
    runBy_gotoWall('l', 0.9); // S4-D5
    turn(90);
    gotoBackWall();
    runByWallLeft(1.1); // D5-C5
    turn(90);
    runTime(70, 70, 0.5);
    runBy_gotoWall('r', 1.5); // C5-C6-C7-S3
    turn(90);
    runByWallLeft(0.9); // S3-D8
    gotoWall();
    turn(90);
    runByWallLeft(0.9); // D8-D7
    gotoWall();
    turn(180);
    runByWallRight(0.9); // D7-D8
    gotoWall();
    turn(-90);
    runByWallRight(2.7); // D8-S3-B8-A8
    gotoWall();
    turn(180);
    runByWallLeft(0.9); // A8-B8
    turn(90);
    gotoWall(); // B8-B7
    turn(-90);
    gotoWall(); // B7-C7
    turn(90);
    runByWallLeft(1.0); // C7-C6
    gotoWall2();
    turn(90);
    table(1); // C6-V2
    identifyInjured(2);
    gotoWall(); // V2-S2
    turn(90);
    runBy_gotoWall('l', 0.9); // S2-A7
    turn(180);
    runBy_gotoWall('r', 1.8); // A7-A6-A5
    turn(-90);
    table(1); // A5-B5
    turn(90);
    table(1); // B5-B4
    gotoWall2();
    wait(50, msec);
    gotoWall2();
    turn(90);
    corner(); // B4-A4-B4
    gotoWall();
    turn(90);
    gotoWall(); // B4-B3
    turn(90);
    gotoTableWithWall('r'); // B3-V1
    turn(-90);
    identifyInjured(2);
    turn(10);
    runByWallRight(1.8); // V1-A1
    gotoWall();
    turn(-90);
    table(1); // A1-S6
    turn(-90);
    table(1); // S6-B2
    gotoWall();
    turn(90);
    table(1); // B2-V3
    identifyInjured(1);
    gotoWall(); // V3-D2
    turn(90);
    runBy_gotoWall('l', 0.9); // D2-D1
    turn(180);
    runByWallRight(1.8);
    return;
}

void S6(void) {
    turn(90);
    table(1); // S6-B2
    gotoWall();
    turn(90);
    table(1); // B2-V3
    identifyInjured(1);
    gotoWall(); // V3-D2
    turn(90);
    runBy_gotoWall('l', 0.9); // D2-D1
    turn(180);
    runBy_gotoWall('r', 2.7); // D1-D2-S5-D4
    turn(180);
    runByWallLeft(0.9); // D4-S5
    turn(90);
    table(1); // S5-C3
    turn(-90);
    gotoBackWall(); // C3-C4
    gotoWall();     // C4-C3
    turn(90);
    runByWallLeft(1.0); // C3-B3
    turn(95);
    gotoWall(); // B3-B4-b5
    turn(90);
    runTime(80, 80, 0.5);
    runByWallRight(1.5); // B5-C5-D5
    gotoWall();
    turn(-90);
    runBy_gotoWall('r', 0.9); // D5-S4
    turn(-90);
    runBy_gotoWall('l', 0.9); // S4-D5
    turn(90);
    gotoBackWall();
    runByWallLeft(1.1); // D5-C5
    turn(90);
    runTime(70, 70, 0.5);
    runBy_gotoWall('r', 1.5); // C5-C6-C7-S3
    turn(90);
    runByWallLeft(0.9); // S3-D8
    gotoWall();
    turn(90);
    runByWallLeft(0.9); // D8-D7
    gotoWall();
    turn(180);
    runByWallRight(0.9); // D7-D8
    gotoWall();
    turn(-90);
    runByWallRight(2.7); // D8-S3-B8-A8
    gotoWall();
    turn(180);
    runByWallLeft(0.9); // A8-B8
    turn(90);
    gotoWall(); // B8-B7
    turn(-90);
    gotoWall(); // B7-C7
    turn(90);
    runByWallLeft(1.0); // C7-C6
    gotoWall2();
    turn(90);
    table(1); // C6-V2
    identifyInjured(2);
    gotoWall(); // V2-S2
    turn(90);
    runBy_gotoWall('l', 0.9); // S2-A7
    turn(180);
    runBy_gotoWall('r', 1.8); // A7-A6-A5
    turn(-90);
    table(1); // A5-B5
    turn(90);
    table(1); // B5-B4
    gotoWall2();
    turn(90);
    corner(); // B4-A4-B4
    gotoWall();
    turn(90);
    gotoWall(); // B4-B3
    turn(90);
    gotoTableWithWall('r'); // B3-V1
    turn(-90);
    identifyInjured(2);
    turn(10);
    runByWallRight(1.8); // V1-A1
    gotoWall();
    turn(-90);
    table(1); // A1-S6
    return;
}

/**
 * @name run
 * @brief 机器人驱动
 * @param leftSpeed 左轮速度
 * @param rightSpeed 右轮速度
 */
void run(int leftSpeed, int rightSpeed) {
    // 设置运行速度
    LeftMotor.setVelocity(leftSpeed, percent);
    RightMotor.setVelocity(rightSpeed, percent);

    // 设置运行方向为 前进
    LeftMotor.spin(forward);
    RightMotor.spin(forward);
    return;
}

/**
 * @name runTime
 * @brief 机器人驱动 时间
 * @param leftSpeed 左轮速度
 * @param rightSpeed 右轮速度
 * @param time 运动时间
 */
void runTime(int leftSpeed, int rightSpeed, float time) {
    run(leftSpeed, rightSpeed);
    wait(time, seconds);
    stop();
    return;
}

/**
 * @name reset
 * @brief 初始化
 */
void reset(void) {
    tt = Brain.Timer.value();

    Brain.Screen.print("Resetting...");

    thread lig30 = thread(light30);

    // 设置马达停止模式为 锁住
    LeftMotor.setStopping(hold);
    RightMotor.setStopping(hold);

    // 校准陀螺仪
    BrainInertial.calibrate();
    BrainInertial.setHeading(0, degrees);
    BrainInertial.setRotation(0, degrees);

    // flashLight(30);

    wait(30, seconds);

    k = gyro1 - 0;
    k /= 10;

    // 设定起点位置
    beginPlace = JudgingStarting();

    // 清空控制台
    printf("\033[2J\n");

    // 清空机器人屏幕
    Brain.Screen.clearScreen();
    Brain.Screen.setCursor(1, 1);
    Brain.Screen.print("place:S%d", beginPlace); // 输出启动位置
    Brain.Screen.newLine();
}

/**
 * @name stop
 * @brief 停止运动
 */
void stop(void) {
    run(0, 0); // 设置两轮速度为0
    return;
}

/**
 * @name JudgingStarting
 * @brief 判断起点
 * @return 起点位置
 */
int JudgingStarting(void) {

    const int threshold = 100; // 判断阈值

    bool frontw = frontd < threshold; // 前方
    bool backw = backd < threshold;   // 后方
    bool leftw = leftd < threshold;   // 左方
    bool rightw = rightd < threshold; // 右方

    if (frontw && backw && !leftw && !rightw) // front back
        return 1;

    if (frontw && !backw && !leftw && !rightw) // front
        return 2;

    if (!frontw && !backw && !leftw && rightw) // right
        return 3;

    if (frontw && backw && !leftw && rightw) // front back right
        return 4;

    if (!frontw && backw && !leftw && !rightw) // back
        return 5;

    if (!frontw && !backw && leftw && !rightw) // left
        return 6;

    return 1;
}

/**
 * @name corner
 * @brief C字形角落
 */
void corner(void) {
    gotoWall();
    turn(180);
    table(1);
    return;
}

/**
 * @name table
 * @brief 前进n格
 * @param n(int) 前进的格数
 */
void table(int n) {
    const int tableDegree = 480; // 走一个格子所需要的角度

    // 设置两轮速度为100%
    LeftMotor.setVelocity(100, percent);
    RightMotor.setVelocity(100, percent);

    // 驱动机器人
    LeftMotor.spinFor(forward, tableDegree * n, degrees, false);
    RightMotor.spinFor(forward, tableDegree * n, degrees, true);

    stop(); // 停止运动
    return;
}

/**
 * @name identifyInjured
 * @brief 识别伤员
 * @param m(int) 识别位置所在的方向 左:1 右:2
 */
void identifyInjured(int m) {
    if (m == 1) { // 如果识别位置在左边 旋转180度
        turn(180);
        runTime(-40, -40, 0.4);
        wait(0.5, seconds);
    }

    bool isIMAN = false;              // 识别伤员的结果
    for (int i = 1; i <= 1000; i++) { // 在1000ms内不断识别，只要有一次识别到，isIMAN值为true
        if (findObject()) {
            isIMAN = true;
            break;
        }
        wait(1, msec);
    }

    if (isIMAN) // 如果识别到伤员，闪灯5下
        flashLight(5);

    if (m == 1) { // 如果识别位置在左边 旋转180度到初始位置
        runTime(40, 40, 0.4);
        wait(0.5, seconds);
        turn(180);
    }
    return;
}

/**
 * @name gotoWall
 * @brief 前进到离墙壁90mm的距离
 */
void gotoWall(void) {
    float speed = 100;             // 速度
    const int target = 50;         // 目标距离
    float error = target - frontd; // 差值
    float integral = 0;            // 累计误差
    float prevError = error;       // 上一次的误差

    const float Kp = 0.80; // 比例系数
    const float Ki = 0.30; // 积分系数
    const float Kd = 1.00; // 微分系数

    float p, i, d;

    while (abs(error) >= 2) {    // 容许的误差范围 ±2
        error = target - frontd; // 更新差值
        integral += error;

        if (abs(error) > 10 ||
            integral >= 50) {
            integral = 0;
        }

        p = Kp * error;
        i = Ki * integral;
        d = Kd * (error - prevError);

        speed = p + i + d; // 根据差值输出速度

        run(-speed, -speed);

        prevError = error; // 更新差值

        wait(10, msec);
    }

    stop(); // 停止运动

    return;
}

/**
 * @name gotoBackWall
 * @brief 前进到离墙壁90mm的距离
 */
void gotoBackWall(void) {
    float speed = 100;             // 速度
    const int target = 50;         // 目标距离
    float error = target - frontd; // 差值
    float integral = 0;            // 累计误差
    float prevError = error;       // 上一次的误差

    const float Kp = 0.70; // 比例系数
    const float Ki = 0.30; // 积分系数
    const float Kd = 1.00; // 微分系数

    float p, i, d;

    while (abs(error) >= 2) {   // 容许的误差范围 ±2
        error = target - backd; // 更新差值
        integral += error;

        if (abs(error) > 10 ||
            integral >= 50) {
            integral = 0;
        }

        p = Kp * error;               // 比例输出
        i = Ki * integral;            // 积分输出
        d = Kd * (prevError - error); // 微分输出

        speed = p + i + d; // 根据差值输出速度

        run(speed, speed);

        prevError = error; // 更新差值

        wait(10, msec);
    }

    stop(); // 停止运动

    return;
}

/**
 * @name turn
 * @brief 旋转一定的角度
 * @param degree (int) 旋转的角度 顺时针为正 逆时针为负
 */
void turn(int degree) {
    float speed = 100;             // 速度
    float target = gyro1 + degree; // 目标角度

    float error = target - gyro1; // 差值
    float prevError = error;      // 上一次的差值
    float integral = 0;           // 积分

    float p, i, d;

    const float Kp = 1.20; // 比例系数
    const float Ki = 0.43; // 积分系数
    const float Kd = 0.10; // 微分系数

    while (abs(error) >= 1) {   // 容许的误差范围 ±1
        error = target - gyro1; // 更新差值

        integral += error;

        if (abs(integral) >= 20 ||
            abs(error) >= 5) {
            integral = 0;
        }

        p = Kp * error;
        i = Ki * integral;
        d = Kd * (error - prevError);

        speed = p + i + d; // 根据差值输出速度

        run(speed, -speed);
        prevError = error;

        wait(10, msec);
    }

    Brain.Screen.newLine();

    stop(); // 停止运动

    return;
}

/**
 * @name findObject
 * @brief 是否检测到绿色
 * @return 检测的结果(bool)
 */
bool findObject(void) {
    Vision2.takeSnapshot(Vision2__IMAN);
    if (Vision2.objectCount >= 1 &&
        Vision2.largestObject.height >= 20) {
        return true;
    } else {
        return false;
    }
}

/**
 * @name flashLight
 * @brief 闪灯
 * @param times 闪灯次数
 */
void flashLight(int times) {
    for (int i = 1; i <= times; i++) {
        TouchLED.setBrightness(100); // 设置亮度为100%
        Brain.playNote(3, 0, 500);   // 发出声音
        TouchLED.setBrightness(0);   // 设置亮度为0%
        wait(0.5, seconds);          // 等待500ms
    }
    return;
}

/**
 * @name runByWallRight
 * @brief 沿墙行驶(使用右测距)
 * @param time 行驶时间
 */
void runByWallRight(float time) {
    float distance = 0;  // 存储测得的距离
    float error = 0;     // 当前误差
    float output = 0;    // PID 控制输出
    int target = 80;     // 目标位置
    float prevError = 0; // 上一次的误差
    float speed = 80;    // 基础速度
    float integral = 0;  // 累计误差
    float tt = Brain.Timer.value() + time;

    int times = 0;

    const float KP = 0.60; // 比例系数
    const float KI = 0.20; // 积分系数
    const float KD = 2.00; // 微分系数

    float P, I, D;

    while (Brain.Timer.value() < tt) {

        distance = rightd;         // 读取传感器测得的距离
        error = target - distance; // 计算误差
        integral += error;         // 更新累计误差

        if (abs(integral) > 50 ||
            abs(error) > 5 ||
            error * integral < 0 ||
            error == 0) { // 当差值过大时 积分清零
            integral = 0;
        }

        P = KP * error;               // 比例输出
        I = KI * integral;            // 积分输出
        D = KD * (error - prevError); // 微分输出

        output = P + I + D; // 计算 PID 控制输出

        if (times == 1 || error != prevError) {
            prevError = error;
            times = 0;
        } else {
            times++;
        }

        // 根据 PID 控制输出调整电机速度
        int LeftSpeed = speed - output;
        int RightSpeed = speed + output;

        printf("%6.1f %6.1f %6.1f %6.1f %4.1f \n", error, P, I, D, output);

        run(LeftSpeed, RightSpeed);

        wait(10, msec);
    }
    stop();
    return;
}

/**
 * @name runByWallLeft
 * @brief 沿墙行驶(使用左测距)
 * @param time 行驶时间
 */
void runByWallLeft(float time) {
    const int target = 80; // 目标位置

    float distance = 0;                    // 存储测得的距离
    float error = 0;                       // 当前误差
    float output = 0;                      // PID 控制输出
    float prevError = 0;                   // 上一次的误差
    float speed = 80;                      // 基础速度
    float integral = 0;                    // 累计误差
    float tt = Brain.Timer.value() + time; // 计时器

    int times = 0;

    const float KP = 0.60; // 比例系数
    const float KI = 0.20; // 积分系数
    const float KD = 2.00; // 微分系数

    float P, I, D; // 比例 积分 微分输出

    while (Brain.Timer.value() < tt) {

        distance = leftd;          // 读取传感器测得的距离
        error = target - distance; // 计算误差
        integral += error;         // 更新累计误差

        if (abs(integral) > 50 ||
            abs(error) > 5 ||
            error * integral < 0 ||
            error == 0) { // 当差值过大时 积分清零
            integral = 0;
        }

        P = KP * error;               // 比例输出
        I = KI * integral;            // 积分输出
        D = KD * (error - prevError); // 微分输出

        output = P + I + D; // 计算 PID 控制输出

        if (times == 1 || error != prevError) {
            prevError = error;
            times = 0;
        } else {
            times++;
        }

        // 根据 PID 控制输出调整电机速度
        int LeftSpeed = speed + output;
        int RightSpeed = speed - output;

        printf("%6.1f %6.1f %6.1f %6.1f %4.1f \n", error, P, I, D, output);

        run(LeftSpeed, RightSpeed); // 驱动机器人

        wait(10, msec);
    }
    stop();
    return;
}

/**
 * @name runBy_gotoWall
 * @brief 沿墙走到墙壁
 * @param direction 墙的位置 (l:左 r:右)
 */
void runBy_gotoWall(char direction, float time) {
    if (direction == 'l') {
        runByWallLeft(time);
    } else {
        runByWallRight(time);
    }
    gotoWall();
    return;
}

/**
 * @name gotoTableWithWall
 * @brief 前进到有墙壁的格子(1格)
 * @param direction 墙的位置 (l:左 r:右)
 */
void gotoTableWithWall(char direction) {
    if (direction == 'l') {    // 左边
        while (leftd >= 150) { // 行走到有墙
            run(50, 50);
        }
        runByWallLeft(0.7);
    } else {                    // 右边
        while (rightd >= 150) { // 行走到有墙
            run(50, 50);
        }
        runByWallRight(0.7);
    }
    return;
}

/**
 * @name gotoWall2
 * @brief 行动到离墙壁一格的距离(36cm)
 */
void gotoWall2(void) {
    float speed = 100;             // 速度
    const int target = 340;        // 目标距离
    float error = target - frontd; // 差值
    float integral = 0;            // 累计误差
    float prevError = error;       // 上一次的误差

    const float Kp = 0.80; // 比例系数
    const float Ki = 0.30; // 积分系数
    const float Kd = 1.00; // 微分系数

    float p, i, d;

    while (abs(error) >= 3) {    // 容许的误差范围 ±2
        error = target - frontd; // 更新差值
        integral += error;

        if (abs(error) > 10 || integral >= 50) { // 积分清零
            integral = 0;
        }

        p = Kp * error;               // 比例输出
        i = Ki * integral;            // 积分输出
        d = Kd * (error - prevError); // 微分输出

        speed = p + i + d; // 根据差值输出速度

        run(-speed, -speed); // 驱动机器人

        prevError = error; // 更新差值

        wait(10, msec); // 等待10ms
    }

    stop(); // 停止运动

    return;
}

/**
 * @name display
 * @brief 显示传感器数值
 */
void display(void) {
    while (true) {
        Brain.Screen.setCursor(3, 1);            // 定位光标到3行1列
        Brain.Screen.print("gyro1:%.3f", gyro1); // 打印陀螺仪数值
        Brain.Screen.setCursor(4, 1);
        Brain.Screen.print("gyro2:%.3f", gyro2);
        wait(20, msec);            // 等待20ms
        Brain.Screen.clearLine(3); // 清空第3行
    }
    return;
}

/**
 * @name getGyroValue
 * @brief 获取陀螺仪数值(废弃)
 * @return 陀螺仪数值(float)
 */
float getGyroValue(void) {
    return gyro1 - (Brain.Timer.value() - 2 - tt) * k;
}

void turnTo(int degree) {
    if (degree == 0) {
        if (315 <= gyro2 && gyro2 < 360)
            turn(360 - gyro2);
        if (0 <= gyro2 && gyro2 < 45)
            turn(-gyro2);
    } else if (degree == 90) {
        if (45 <= gyro2 && gyro2 < 90)
            turn(90 - gyro2);
        if (90 <= gyro2 && gyro2 < 135)
            turn(gyro2 - 90);
    } else if (degree == 180) {
        if (135 <= gyro2 && gyro2 < 180)
            turn(180 - gyro2);
        if (180 <= gyro2 && gyro2 < 225)
            turn(gyro2 - 180);
    } else if (degree == 270) {
        if (225 <= gyro2 && gyro2 < 270)
            turn(270 - gyro2);
        if (270 <= gyro2 && gyro2 < 315)
            turn(gyro2 - 270);
    }
    return;
}

void gotoWall3(void) {
    float speed = 100;             // 速度
    const int target = 650;        // 目标距离
    float error = target - frontd; // 差值
    float integral = 0;            // 累计误差
    float prevError = error;       // 上一次的误差

    const float Kp = 0.80; // 比例系数
    const float Ki = 0.30; // 积分系数
    const float Kd = 1.00; // 微分系数

    float p, i, d;

    while (abs(error) >= 4) {    // 容许的误差范围 ±2
        error = target - frontd; // 更新差值
        integral += error;

        if (abs(error) > 10 || integral >= 50) { // 积分清零
            integral = 0;
        }

        p = Kp * error;               // 比例输出
        i = Ki * integral;            // 积分输出
        d = Kd * (error - prevError); // 微分输出

        speed = p + i + d; // 根据差值输出速度

        run(-speed, -speed); // 驱动机器人

        prevError = error; // 更新差值

        wait(10, msec); // 等待10ms
    }

    stop(); // 停止运动

    return;
}

void light30(void){
    flashLight(30);
    return;
}
```
## 创新亮点
1. 我们发现每个出发点之间的路线可以视为任务以此来推，再在每个分段细节优化
然后从S1到S2后每个出发点都按照该线路来走，不仅优化了编程速度，更使我们在之后的调试环节有了方便一目了然的优势。
在一些转角我们发现让机器转弯掉头的动作实在过于繁琐，所以我们有些地方采用倒车入库的方式，用后方的测距传感器来判断是否到达格点
![输入图片说明](1234JudgingStarting.png)
2. 在转向和直行时我们采用稳定的PID控制并用陀螺仪辅助矫正。理由如下：
- 相较与直接直走PID控制有很好的适应性，几乎所有场景可以应用，速度更快
- PID的算法的值比较好调，无论场地的光暗都有较好的体现
- 在判断每个路口，我们采用陀螺仪矫正到零度方式来找对应位置，这种方式相较于测距去寻墙更加可靠和便捷，在一定程度上解决了在确认路口时的颠簸
3. 平衡
在编写程序时我们也有遇到过困难例如我们发现在我们小车速度加打最大时会翘头导致各个传感器判断不到墙壁，因此我们在小车前方加装了配重块件，来控制小车的基础平衡