最后一次编辑于2020-11-24

arduino主程序"Main.ino"（文件名可修改）：

#include "config.h"

void setup(){
  Serial.begin(9600);
  Motor_init();
  work_init();
}

void loop(){
  move_patient();
}

机器人搬运动作代码"work.cpp"：

#include "config.h"
#include "HC_SR04.h"
#include "Servo_5.h"
#include <Arduino.h>


/****************************确定左右半场*****************************/
//左右半场
char left_or_right = LEFT_FIELD;
//判断左右半场开关引脚
const int left_or_right_indicator = 31;

//确定左右半场，HIGH-左半场，LOW-右半场
void Decide_left_or_right(){
    if (digitalRead(left_or_right_indicator) == HIGH)
    {
        left_or_right = LEFT_FIELD;
    }
    else
    {
        left_or_right = RIGHT_FIELD;
    }
    //将半场信息发送给树莓派
    send_left_or_right_info();
}
/****************************确定左右半场*****************************/



/****************************主巡线上移动*****************************/
//当前机器人所在岔路编号
//cur_crossroad_num = -1：位于启动区
//cur_crossroad_num = i(0 <= i <= 7)：刚刚经过岔路i，尚未到达岔路i+1
//cur_crossroad_num = 7：到达岔路7
int cur_crossroad_num = -1;


//记录前一次tracing_sensor_A[0],tracing_sensor_A[0]的值
int pre_tracing_sensor_A0 = LOW;
int pre_tracing_sensor_A3 = LOW;

//判断是否到达岔路
//0-未到达岔路;1-到达岔路
int if_reach_crossroad(){
    //记录当前tracing_sensor_A[0],tracing_sensor_A[3]的值
    int cur_tracing_sensor_A0 = digitalRead(tracing_sensor_A[0]);
    int cur_tracing_sensor_A3 = digitalRead(tracing_sensor_A[3]);
    //LOW-白，HIGH-黑
    //若此前为白，此时为黑，认为遇到岔路
    if (cur_tracing_sensor_A0 == HIGH || cur_tracing_sensor_A3 == HIGH
        && (pre_tracing_sensor_A0 == LOW && pre_tracing_sensor_A3 == LOW))
    {
        //更新记录的tracing_sensor_A[0],tracing_sensor_A[3]的值
        pre_tracing_sensor_A0 = cur_tracing_sensor_A0;
        pre_tracing_sensor_A3 = cur_tracing_sensor_A3;
        return 1;
    }
    else
    {
        pre_tracing_sensor_A0 = cur_tracing_sensor_A0;
        pre_tracing_sensor_A3 = cur_tracing_sensor_A3;
        return 0;
    }
}

//主巡线上移动到目标位置
void move_on_main_track(int dest_crossroad_num){
    //此时机器人在目的岔路下方
    while (cur_crossroad_num < dest_crossroad_num)
    {
        //向前巡线
        moving_direction = FORWARD;
        Straight_tracing();
        Forward_tracing_rectification();
        if (if_reach_crossroad() == 1)
        {
            cur_crossroad_num++;
        }
    }
    //此时机器人在目的岔路上方
    while (cur_crossroad_num > dest_crossroad_num)
    {
        //向后巡线
        moving_direction = BACKWARD;
        Straight_tracing();
        Backward_tracing_rectification();
        if (if_reach_crossroad() == 1)
        {
            cur_crossroad_num++;
        }
    }
    Brake();
}
/****************************主巡线上移动*****************************/


/****************************获取场地信息*****************************/
//非空病床位置
int occupied_bed_pos = -1;

//获取病床占空情况
void Get_bed_info(){
    if (left_or_right == LEFT_FIELD)
    {
        float distance = HC_SR04[0].Get();
        if (distance <= Patient_On_Distance)
        {
            occupied_bed_pos = cur_crossroad_num;
        }
    }
    else
    {
        float distance = HC_SR04[1].Get();
        if (distance <= Patient_On_Distance)
        {
            occupied_bed_pos = cur_crossroad_num;
        }
    }
}

//黄病人位置
int yellow_pos0 = -1;
int yellow_pos1 = -1;
//红病人位置
int red_pos = -1;

//记录病人颜色信息
void record_patient_color(){
    take_a_photo();
    char patient_color = get_patient_color();
    if (patient_color == 'y')
    {
        //将当前岔路编号赋给尚未赋值的一个
        if (yellow_pos0 == -1)
        {
            yellow_pos0 = cur_crossroad_num;
        }
        else
        {
            yellow_pos1 = cur_crossroad_num;
        }
        
    }
    if (patient_color == 'r')
    {
        red_pos = cur_crossroad_num;
    }
}

//获取全部岔路信息
void get_all_crossroad_info(){
    //运动到第一个岔路
    move_on_main_track(1);
    while (true)
    {
        //标识是否获取全部岔路信息，包括红色、黄色病人位置，病床占空情况
        //0-未获取全部岔路信息；1-已获取全部岔路信息
        int if_get_all_info = 1;

        //尚未完全获得颜色信息
        if (yellow_pos0 == -1 || yellow_pos1 == -1 || red_pos == -1)
        {
            record_patient_color();
            if_get_all_info = 0;
        }

        //病床占空情况未知
        if (occupied_bed_pos == -1)
        {
            if_get_all_info = 0;
            //机器人位于岔路编号2，3，4，获取病床占空情况
            if (cur_crossroad_num >= 2 && cur_crossroad_num <= 4)
            {
                Get_bed_info();
            }
        }

        //已获取全部岔路信息，或已到达6号岔路，结束循环
        if (if_get_all_info == 1 || cur_crossroad_num >= 6)
        {
            break;
        }
        //移动到下一个岔路，获取其信息
        move_on_main_track(cur_crossroad_num + 1);
    }
}
/****************************获取场地信息*****************************/



/******************************取放病人******************************/
//使病人爪正对左(右)岔路
void arm_aim_at_crossroad(){
    //LOW-白，HIGH-黑
    //抓取动作即将开始时，tracing_sensor_A[0](或tracing_sensor_A[3])一定位于支路上
    //故此时应该向前直行直到tracing_sensor_B[1](或tracing_sensor_C[1])位于支路上
    while (digitalRead(tracing_sensor_B[1]) == LOW && digitalRead(tracing_sensor_C[1]) == LOW)
    {
        Straight_tracing();
        Forward_tracing_rectification();
    }
    Brake();
}

//抓取病人
void catch_patient(){
    arm_aim_at_crossroad();
    //位于左半场
    if (left_or_right == LEFT_FIELD)
    {
        //开爪，放水平
        Open(Clawservo[1]);
        TURN_TO_LEVEL(Armservo[1]);
        //机器人向右移动到病人距离小于Catch_Distance的位置
        while (HC_SR04[1].Get() > Catch_Distance)
        {
            Rightward_tracing();
            Rightward_tracing_rectification();
        }
        Brake();
        Close(Clawservo[1]);
        TURN_TO_VERTICAL(Armservo[1]);
        TURN_TO_0(Yuntaiservo);     //抓取后云台直接转0度方便放置病人
        //机器人向左移动至返回主巡线
        while (digitalRead(tracing_sensor_A[2]) == LOW)
        {
            Leftward_tracing();
            Leftward_tracing_rectification();
        }
        Brake();
    }
    else
    {
        //开爪，放水平
        Open(Clawservo[1]);
        TURN_TO_LEVEL(Armservo[1]);
        //机器人向左移动到病人距离小于Catch_Distance的位置
        while (HC_SR04[0].Get() > Catch_Distance)
        {
            Leftward_tracing();
            Leftward_tracing_rectification();
        }
        Brake();
        Close(Clawservo[1]);
        TURN_TO_VERTICAL(Armservo[1]);
        TURN_TO_180(Yuntaiservo);     //抓取后云台直接转180度方便放置病人
        //机器人向右移动至返回主巡线
        while (digitalRead(tracing_sensor_A[1]) == LOW)
        {
            Rightward_tracing();
            Rightward_tracing_rectification();
        }
        Brake();
    }
}

//放置黄色病人
void place_yellow_patient(){
    arm_aim_at_crossroad();
    //位于左半场
    if (left_or_right == LEFT_FIELD)
    {
        //机器人向左移动到床柱距离小于Catch_Distance的位置
        while (HC_SR04[0].Get() > Catch_Distance)
        {
            Leftward_tracing();
            Leftward_tracing_rectification();
        }
        Brake();
        //爪放平，开爪
        TURN_TO_LEVEL(Armservo[1]);
        Open(Clawservo[1]);
        //机器人向右移动至返回主巡线
        while (digitalRead(tracing_sensor_A[1]) == LOW)
        {
            Rightward_tracing();
            Rightward_tracing_rectification();
        }
        Brake();
        TURN_TO_VERTICAL(Armservo[1]);
        TURN_TO_0(Yuntaiservo);      //准备下次抓取病人
    }
    else
    {
        //机器人向右移动到床柱距离小于Catch_Distance的位置
        while (HC_SR04[1].Get() > Catch_Distance)
        {
            Rightward_tracing();
            Rightward_tracing_rectification();
        }
        Brake();
        //爪放平，开爪
        TURN_TO_LEVEL(Armservo[1]);
        Open(Clawservo[1]);
        //机器人向左移动至返回主巡线
        while (digitalRead(tracing_sensor_A[2]) == LOW)
        {
            Leftward_tracing();
            Leftward_tracing_rectification();
        }
        Brake();
        TURN_TO_VERTICAL(Armservo[1]);
        TURN_TO_180(Yuntaiservo);      //准备下次抓取病人
    }
}

//搬运黄病人
//patient_pos-病人所在岔路编号，bed_pos-空床位所在岔路编号
void move_yellow_patient(int patient_pos, int bed_pos){
    //抓病人
    move_on_main_track(patient_pos);
    catch_patient();
    //放病人
    move_on_main_track(bed_pos);
    place_yellow_patient();
}

void place_red_patient(){
    //待完成
}

//搬运红病人
void move_red_patient(){
    //抓病人
    move_on_main_track(red_pos);
    catch_patient();
    //放病人
    move_on_main_track(7);
    place_red_patient();
}

//确定机器人搬运黄病人时目的地顺序
int dest_order[4]={-1,-1,-1,-1};

void gen_dest_order(){
    /*
    由于机器人遍历完时位置不低于需要搬运的病人，故路线为：
    位置最高的黄色病人->位置最高的空病床->另一个黄色病人->另一个空病床
    */
    dest_order[0] = yellow_pos1;
    dest_order[2] = yellow_pos0;
    switch (occupied_bed_pos)
    {
    case 2:
        dest_order[1] = 4;
        dest_order[3] = 3;
        break;
    case 3:
        dest_order[1] = 4;
        dest_order[3] = 2;
        break;
    case 4:
        dest_order[1] = 3;
        dest_order[3] = 2;
        break;
    }
}

//搬运病人
void move_patient(){
    gen_dest_order();
    move_yellow_patient(dest_order[0], dest_order[1]);
    move_yellow_patient(dest_order[2], dest_order[3]);
    move_red_patient();
}
/******************************取放病人******************************/


//初始化
void work_init(){
    pinMode(left_or_right_indicator, INPUT);
    Decide_left_or_right();
}