micro_servers/lci/include/hd/Robot.hpp

#pragma once
#include <cmath>
#include <fstream>
#include <sstream>
#include <iostream>
#include <vector>
#include <unistd.h>
#include "cn/中文化.hpp"
#include "Dashun_linux.hpp"
命名空间 qing{
类 伺服电机{  /* 180度舵机 */
公开的:

    /* FIXME: min只能比max小 */
    伺服电机(整数 通道, 单精度小数 上限=180, 单精度小数 下限=0, 逻辑型 逆向=false)
    : 通道(通道), 上限(上限), 下限(下限), 逆向(逆向) {}

    /* 控制舵机运行至一个角度 */
    空类型 转动至(单精度小数 角度) {
        角度 = std::min(上限, 角度);
        角度 = std::max(下限, 角度);
        PCA9685_调整角度(
		通道, (逆向) ? 180 - 角度 : 角度
	);
    }

    /* FIXME: 返回最大值 */
    单精度小数 取上限() {
        返回 上限;
    }


私有的:
    整数 通道; /* 通道编号 */
    单精度小数 上限;   /* 角度上限 */
    单精度小数 下限;   /* 角度下限 */
    逻辑型 逆向; /* 是否反向 */
};
}

命名空间 qing {
类 ActParser{  /* 动作脚本解析器 */
公开的:

    /* 解析标准输入中的动作脚本 */
    空类型 fromStdin() {
        计数  = 0;
        清除所有();
        解析(std::cin);
    }

    /* 解析文件输入流中的动作脚本 */
    空类型 fromFile(const std::string& 路径) {
        计数  = 0;
        std::ifstream 文件(路径.c_str(), std::ios::in);
        如果 (!文件.is_open())
            抛出异常 std::runtime_error("打开文件失败");
        清除所有();
        解析(文件);
        文件.close();
    }

    /* 解析字符串流中的动作脚本 */
    空类型 fromStr(const std::string& 脚本) {
        std::stringstream 流(脚本);
        计数 = 0;
        清除所有();
	解析(流);
    }

    /* 执行刚刚解析出来的机器人和动作 */
    空类型 play() {
        如果 (机组.empty() || 矩阵.empty())
            抛出异常 std::runtime_error("没动作或机组");
        机器人扮演(机组, 矩阵);
    }

    /* 获得机器人的最大值向量， FIXME: 把逻辑写在外面 */
    std::vector<float> get() {
        适应 结果  = std::vector<float>();
        计次循环 (适应& 项目: this->机组) {
            结果.push_back(项目.取上限());
        }
        /* 这个数组中包含机器人每个舵机的角度上限 */
        返回 结果;
    }

    /* 获取当前这一组动作 */
    std::vector<float> now() {
        如果 (计数 >= 矩阵.size())
            抛出异常 std::runtime_error("now finished");
        返回 矩阵[计数];
    }

    /* 执行下一组动作 */
    空类型 next() {
        如果 (计数 >= 矩阵.size())
            抛出异常 std::runtime_error("没下一个动作了");
        适应 临时向量 = 矩阵[计数++];
        适应 临时矩阵 = std::vector<std::vector<float>> ({临时向量});
        机器人扮演(机组, 临时矩阵);
    }

    /* 输入下一个动作执行 */
    空类型 next(std::vector<float>& act) {
        计数++;
        auto vv = std::vector<std::vector<float>> ({act});
        机器人扮演(机组, vv);
    }
private:
    字符 句[16] = {0}; /* FIXME: 这个栈有越界风险 */
    整数 顶 = 0;
    整数 计数 = 0;
    std::vector<float> 数组;  /* 临时使用 */
    std::vector<std::vector<float>> 矩阵; /* 解析出的整套动作 */
    std::vector<伺服电机> 机组; /* 机器人抽象 */
    空类型 解析(std::istream& 源) {  /* NOTE: 修改为非静态成员函数能够减少行数 FIXME: 改为使用C++风格的文件操作 */
        清除临时();
        字符 c = 0;
        布尔 end_flag  = true;
        布尔 comment_flag  = false;
        布尔 obj_flag = false;
        判断循环 (end_flag) { /* 255 for arm */
            end_flag = (bool)源.get(c);
            //std::cout<<c;
            如果 ((!comment_flag && c >= '0' && c <= '9') || c == '+' || c == '-') {  /* 读取数字 */
                句[顶++] = c;
                std::cout<<c;
            }
            否则 如果 (!comment_flag && c == ' ') {  /* 每个角度以空格结尾 */
                句[顶] = '\0';
                递交舵机();
            }
            否则 如果 (!comment_flag && (c == '\r' || c == '\n')) {  /* 忽略回车 */
                ;
            }
            否则 如果 (!comment_flag && c == '#') {  /* 注释以#开头 */
                comment_flag = 真;
            }
            否则 如果 (comment_flag && c == '\n') {  /* 注释以换行结尾 */
                comment_flag = 假;
            }
            否则 如果 (comment_flag && c != '\n') {  /* 忽略注释 */
                ;
            }
            否则 如果 (!comment_flag && !obj_flag && 顶 == 0 && c == '!') {  /* 构建模式 */
                obj_flag = 真;
            }
            否则 如果 (!comment_flag && obj_flag && (c==';' || end_flag)) {  /* 构建机器人 */
                句[顶] = '\0';
                递交舵机();
                递交机组();
                obj_flag = 假;
            }   
            否则 如果 (!comment_flag && !obj_flag && (c == ';' || end_flag)) {   /* 每个动作以分号结尾 */
                句[顶] = '\0';
                递交舵机();
                递交动作();
            }
            否则 {
                抛出异常 std::runtime_error(
                    "Invalid action: " + std::to_string(int(c)));
            }
        }
    }

    空类型 递交舵机() {
        如果 (顶 > 0) {
            单精度小数 角度 = (单精度小数)atoi(句);
            数组.push_back(角度);
            std::cout<<角度<<std::endl;
        }
        句[0] = '\0';
        顶 = 0;
    }

    空类型 递交动作() {
        如果 (!数组.empty()) {
            std::cout<<"接收到动作： " << 数组.size() << std::endl;
            矩阵.push_back(数组);
            数组 =  std::vector<float>{};
        }
    }

    空类型 递交机组() {
        如果 (!数组.empty()) {
            std::cout << "接收到机组： " << 数组.size() << std::endl;
            机组 = 机组工厂(数组);
            数组 = std::vector<float>{};
        }
    }

    空类型 清除临时() {
        句[0] = '\0';
        顶 = 0;
        数组 = std::vector<float> {};
    }

    空类型 清除所有() {
        句[0] = '\0';
        顶 = 0;
        数组 = std::vector<float> {};
        矩阵 = std::vector<std::vector<float>> {};
        机组 = std::vector<伺服电机> {};
    }

    /* 机器人工厂，FIXME: 这里可以做成舵机工厂就可以了， */
    静态的 std::vector<伺服电机> 机组工厂(const std::vector<float>& 上限集) {
        std::vector<伺服电机> 机组;
        计次循环 (整数 甲=0; 甲 < 上限集.size(); ++甲)
            机组.push_back(
                伺服电机(甲, std::abs(上限集[甲]), 0, (上限集[甲] >= 0) ? 假 : 真));
        返回 机组;
    }

    /* 机器人移动，输入一组舵机对象和一组目标角度 */
    静态的 空类型 机器人行动(std::vector<伺服电机>& 机组, std::vector<float>& 动作) {
        如果 (机组.size() + 1 != 动作.size())
            抛出异常 std::runtime_error(
                "动作与机组不符合， 我们有"
                + std::to_string(机组.size()) + "和" + std::to_string(动作.size())
            );

	/* FIXME: 随机下标以消除先后时间差 */
        计次循环 (int 甲=0; 甲<机组.size(); ++甲) {
            机组[甲].转动至(动作[甲]);
        }
	/* 最后一组数据是毫秒延时 */
        预求值的 整数 缩放  = 1000;
	usleep(动作[动作.size()-1] * 缩放);
    }
    
    /* 机器人整套动作 */
    静态的 空类型 机器人扮演(
        std::vector<伺服电机>& 机组, std::vector<std::vector<float>>& 动作集)
    {
        计次循环 (适应 动作: 动作集)
            机器人行动(机组, 动作);
    }
};
}