《实时控制软件设计》第二个编程作业

第二个编程作业，为了实现我们的机器人咖啡角，我们将基于第一次作业中的图形旋转，构造一个两轴平面机器人，运动原理见动画
要求：
1）代码中至少包括两个类：一个Robot类和一个Solver类，Robot类中定义每个机械臂的长度，每个关节的转动角度范围和零点，Solver求解器类用于实现机器人的正逆运动学变换，正运动学为把机器人的关节坐标变换成笛卡尔坐标，逆运动学为把机器人的笛卡尔坐标变换成关节坐标。
2）定义坐标系类：机器人的原点为世界坐标系(World Frame)，同时，允许用户定义工件坐标系或任务坐标系(Task Frame)，任务坐标系和世界坐标系之间存在着偏移和旋转关系。机器人各个关节的角度值构成关节坐标系（Joint Frame）。
3）每个机器人允许定义任意多个任务坐标系，请运用Vector容器实现该功能。
4）机器人对象接收来自用户的PTP（Point To Point）运动指令，该指令所给出的坐标值，可能是关节坐标系或世界坐标系或任务坐标系，机器人通过Solver求解器类把世界坐标值和任务坐标值变换成关节坐标并更新关节角度值。
注意：在本次编程中不需要机器人具备连续轨迹运动的功能，只需要能够根据用户的PTP指令直接把内部的关节角度值更新就可以。
5）测试主程序架构如下：

int main()
{
//创建一个机器人对象myRobot，初始化机器人的机械参数，关节坐标系（JF），世界坐标系（WF）。
//给机器人对象创建3个任务坐标系TF1, TF2, TF3。
//控制机器人对象在不同坐标系下的运动，如：
//myRobot.PTPMove(JF,P1);
//myRobot.PTPMove(WF,P2);
//myRobot.PTPMove(TF1,P3);
//myRobot.PTPMove(TF2,P4);
//myRobot.PTPMove(TF3,P5);
}

#include<iostream>
#include<vector>
#include<algorithm>

using namespace std;

class Point
{
public:
    Point()
    {
        x = 0;
        y = 0;
    }
    Point(double x, double y)
    {
        this->x = x;
        this->y = y;
    }
private:
    double x;
    double y;
};

class Line
{
public:
    Line(Point start, Point end)
    {
        this->start = start;
        this->end = end;
    }
private:
    Point start;
    Point end;
};

class Solver
{
    
};

class Frame
{
public:
    Frame()
    {
        delta_theta = 0;
        delta_x = 0;
        delta_y = 0;
    }
    Frame(double delta_theta, double delta_x, double delta_y)
    {
        this->delta_theta = delta_theta;
        this->delta_x = delta_x;
        this->delta_x = delta_x;
    }
private:
    double delta_theta;
    double delta_x;
    double delta_y;
};

class Robot
{
public:
    vector<Frame> TaskFrame;
    Robot()
    {
        ArmLength1 = 5;
        ArmLength2 = 5;
    }
    Robot(double ArmLength1,double Armlength2)
    {
        this->ArmLength1 = ArmLength1;
        this->ArmLength2 = Armlength2;
    }
    void PTPMove(Point start, Point end)
    {

    }
private:
    Frame WorldFrame;
    Frame JointFrame;
    double ArmLength1;
    double ArmLength2;
};
int main()
{
    //创建一个机器人对象myRobot，初始化机器人的机械参数，关节坐标系（JF），世界坐标系（WF）。
    //给机器人对象创建3个任务坐标系TF1, TF2, TF3。
    //控制机器人对象在不同坐标系下的运动，如：
    //myRobot.PTPMove(JF,P1);
    //myRobot.PTPMove(WF,P2);
    //myRobot.PTPMove(TF1,P3);
    //myRobot.PTPMove(TF2,P4);
    //myRobot.PTPMove(TF3,P5);
}