第二次作业

Point.h

#ifndef POINT_H
#define POINT_H
class Point{
    private:
        double X,Y;
    public:
        Point();
        Point(double a,double b);
        void SetPoint(double a,double b);
        void SetX(double a);
        void SetY(double b);
        double GetX();
        double GetY();     
};
#endif

Point.cpp

#include"Point.h"
Point::Point()
{
    X=0;Y=0;
};
Point::Point(double a,double b)
{
    X=a,Y=b;
};
void Point::SetPoint(double a,double b)
{
    X=a,Y=b;
};
void Point::SetX(double a)
{
    X=a;
};
void Point::SetY(double b)
{
    Y=b;
};
double Point::GetX()
{
    return X;
};
double Point::GetY()
{
    return Y;
};

Axis.h

#ifndef AXIS_H
#define AXIS_H

#include"Point.h"
class Axis
{
    private:    
        Point O;
        double Angle;
    public:
        Axis();
        Axis(double a,double b,double ang);
        Axis(Point p,double ang);
        void SetAxis(double a,double b,double ang);
        void SetAxis(Point p,double ang);
        void SetO(double a,double b);
        void SetO(Point p);
        Point GetO();
        double GetAngle();
};

class WorldFrame: public Axis
{
    public:
        WorldFrame();
};

class TaskFrame: public Axis
{
    public:
        TaskFrame();
        TaskFrame(double a,double b,double ang);
};

class JointFrame
{
    private:
        double Angle1,Angle2;
    public:
        JointFrame();
        JointFrame(double ang1,double ang2);
        void SetAngle1(double a);
        void SetAngle2(double b);
        double GetAngle1();
        double GetAngle2();
};
#endif

Axis.cpp

#include"Axis.h"
#include"Point.h"

Axis::Axis()
{
    O.SetPoint(0,0);
    Angle=0;
}
Axis::Axis(double a,double b,double ang)
{
    O.SetPoint(a,b);
    Angle=ang;
};
Axis::Axis(Point p,double ang)
{
    O=p;
    Angle=ang;
};
void Axis::SetAxis(double a,double b,double ang)
{
    O.SetPoint(a,b);
    Angle=ang;
};
void Axis::SetAxis(Point p,double ang)
{
    O=p;
    Angle=ang;
};
void Axis::SetO(double a,double b)
{
    O.SetPoint(a,b);
};
void Axis::SetO(Point p)
{
    O=p;
};
Point Axis::GetO()
{
    return O;
};
double Axis::GetAngle()
{
    return Angle;
};

WorldFrame::WorldFrame()
{
    SetAxis(0,0,0);
};

TaskFrame::TaskFrame()
{
    SetAxis(0,0,0);
};
TaskFrame::TaskFrame(double a,double b,double ang):Axis(a,b,ang)
{
    
};

JointFrame::JointFrame()
{
    Angle1=Angle2=0;
};
JointFrame::JointFrame(double ang1,double ang2)
{
    Angle1=ang1;
    Angle2=ang2;
};
void JointFrame::SetAngle1(double a)
{
    Angle1=a;
};
void JointFrame::SetAngle2(double b)
{
    Angle2=b;
};
double JointFrame::GetAngle1()
{
    return Angle1;
};
double JointFrame::GetAngle2()
{
    return Angle2;
};

Solver.h

#ifndef SOLVER_H
#define SOLVER_H
#include"Axis.h"
#include<math.h>
#include<iostream>
class Solver
{
    public:
        Solver();
        Solver(double a,double b);
        SetL(double a,double b);
        SetP(Point a);
        SetJF(JointFrame j);
        Point GetP();
        JointFrame Getjo();
    private:
        Point p;
        JointFrame jo;
        double la,lb;
};

#endif

Solver.cpp

#include "Solver.h"
#define PI 3.1415926
Solver::Solver(){};
Solver::Solver(double a,double b)
{
    la=a;
    lb=b;
};
Solver::SetL(double a,double b)
{
    la=a;
    lb=b;
};
Solver::SetP(Point a)
{
    p=a;
    double l=sqrt(a.GetX()*a.GetX()+a.GetY()*a.GetY());
    if(l>la+lb)std::cout<<"error!"<<std::endl;
    else
    {
        jo.SetAngle1((acos((la*la+l*l-lb*lb)/2/la/l)+atan(a.GetY()/a.GetX()))/PI*180);
        jo.SetAngle2(acos((la*la+lb*lb-l*l)/2/la/lb)/PI*180);
    }
};
Solver::SetJF(JointFrame j)
{
    jo=j;
    p.SetPoint(la*cos(jo.GetAngle1()/180*PI)+lb*cos(jo.GetAngle2()/180*PI),la*sin(jo.GetAngle1()/180*PI)+lb*sin(jo.GetAngle2()/180*PI));
};
Point Solver::GetP()
{
    return p;
};
JointFrame Solver::Getjo()
{
    return jo;
};

Robot.h

#ifndef ROBOT_H
#define ROBOT_H
#include<math.h>
#include<vector>
#include"Solver.h"
class Robot
{
    public:
        Robot(double la,double lb);
        JointFrame GetJF();
        double GetLengthA();
        double GetLengthB();
        Point TF2WF(TaskFrame t,Point p);
        Point WF2TF(TaskFrame t,Point p);
        JointFrame P2Pmove(TaskFrame t,Point p);
        JointFrame P2Pmove(WorldFrame t,Point p);
    private:
        JointFrame JF;
        WorldFrame WF;
        std::vector<TaskFrame> TF;
        double LengthA,LengthB;
        Solver s;
};

#endif

Robot.cpp

#include "Robot.h"
#define PI 3.1415926
Robot::Robot(double la,double lb):s(la,lb)
{
    LengthA=la;
    LengthB=lb;
};
JointFrame Robot::GetJF()
{
    return JF;
};
double Robot::GetLengthA()
{
    return LengthA;
};
double Robot::GetLengthB()
{
    return LengthB;
};
Point Robot::TF2WF(TaskFrame t,Point p)
{
    double x=p.GetX()*cos(t.GetAngle()/180*PI)+t.GetO().GetX()-p.GetY()*sin(t.GetAngle()/180*PI);
    double y=t.GetO().GetY()+p.GetY()*cos(t.GetAngle()/180*PI)+p.GetX()*sin(t.GetAngle()/180*PI);
    Point temp(x,y);
    return temp;
};
Point Robot::WF2TF(TaskFrame t,Point p)
{
    double i=(p.GetX()-t.GetO().GetX())/tan(t.GetAngle()/180*PI);
    double j=p.GetY()-t.GetO().GetY()-i;
    double y=j*cos(t.GetAngle()/180*PI);
    double x=j*sin(t.GetAngle()/180*PI)+i/sin(t.GetAngle()/180*PI);
    Point temp(x,y);
    return temp;
};
JointFrame Robot::P2Pmove(TaskFrame t,Point p)
{
    s.SetP(TF2WF(t,p));
    return s.Getjo();
};
JointFrame Robot::P2Pmove(WorldFrame t,Point p)
{
    s.SetP(p);
    return s.Getjo();
};

main.cpp

#include<iostream>
#include"Robot.h"
using namespace std;
int main()
{
    Robot myrobot(10,10);
    Point p(5,5),q(20,0);
    WorldFrame w;
    TaskFrame tk1(1,5,30),tk2(3,4,50),tk3(2,1,40);
    JointFrame j;
    j=myrobot.P2Pmove(w,p);
    cout<<"第一个关节角度为"<<j.GetAngle1()<<"度，第二个关节角度为"<<j.GetAngle2()<<"度。"<<endl; 
    j=myrobot.P2Pmove(tk1,p);
    cout<<"第一个关节角度为"<<j.GetAngle1()<<"度，第二个关节角度为"<<j.GetAngle2()<<"度。"<<endl; 
    j=myrobot.P2Pmove(tk2,p);
    cout<<"第一个关节角度为"<<j.GetAngle1()<<"度，第二个关节角度为"<<j.GetAngle2()<<"度。"<<endl; 
    j=myrobot.P2Pmove(tk3,p);
    cout<<"第一个关节角度为"<<j.GetAngle1()<<"度，第二个关节角度为"<<j.GetAngle2()<<"度。"<<endl; 
    j=myrobot.P2Pmove(w,q);
    cout<<"第一个关节角度为"<<j.GetAngle1()<<"度，第二个关节角度为"<<j.GetAngle2()<<"度。"<<endl; 
    return 0;
}