第二次作业

Point.h

 1 #ifndef POINT_H
 2 #define POINT_H
 3 class Point{
 4     private:
 5         double X,Y;
 6     public:
 7         Point();
 8         Point(double a,double b);
 9         void SetPoint(double a,double b);
10         void SetX(double a);
11         void SetY(double b);
12         double GetX();
13         double GetY();     
14 };
15 #endif

Point.cpp

 1 #include"Point.h"
 2 Point::Point()
 3 {
 4     X=0;Y=0;
 5 };
 6 Point::Point(double a,double b)
 7 {
 8     X=a,Y=b;
 9 };
10 void Point::SetPoint(double a,double b)
11 {
12     X=a,Y=b;
13 };
14 void Point::SetX(double a)
15 {
16     X=a;
17 };
18 void Point::SetY(double b)
19 {
20     Y=b;
21 };
22 double Point::GetX()
23 {
24     return X;
25 };
26 double Point::GetY()
27 {
28     return Y;
29 };

Axis.h

 1 #ifndef AXIS_H
 2 #define AXIS_H
 3 
 4 #include"Point.h"
 5 class Axis
 6 {
 7     private:    
 8         Point O;
 9         double Angle;
10     public:
11         Axis();
12         Axis(double a,double b,double ang);
13         Axis(Point p,double ang);
14         void SetAxis(double a,double b,double ang);
15         void SetAxis(Point p,double ang);
16         void SetO(double a,double b);
17         void SetO(Point p);
18         Point GetO();
19         double GetAngle();
20 };
21 
22 class WorldFrame: public Axis
23 {
24     public:
25         WorldFrame();
26 };
27 
28 class TaskFrame: public Axis
29 {
30     public:
31         TaskFrame();
32         TaskFrame(double a,double b,double ang);
33 };
34 
35 class JointFrame
36 {
37     private:
38         double Angle1,Angle2;
39     public:
40         JointFrame();
41         JointFrame(double ang1,double ang2);
42         void SetAngle1(double a);
43         void SetAngle2(double b);
44         double GetAngle1();
45         double GetAngle2();
46 };
47 #endif

Axis.cpp

 1 #include"Axis.h"
 2 #include"Point.h"
 3 
 4 Axis::Axis()
 5 {
 6     O.SetPoint(0,0);
 7     Angle=0;
 8 }
 9 Axis::Axis(double a,double b,double ang)
10 {
11     O.SetPoint(a,b);
12     Angle=ang;
13 };
14 Axis::Axis(Point p,double ang)
15 {
16     O=p;
17     Angle=ang;
18 };
19 void Axis::SetAxis(double a,double b,double ang)
20 {
21     O.SetPoint(a,b);
22     Angle=ang;
23 };
24 void Axis::SetAxis(Point p,double ang)
25 {
26     O=p;
27     Angle=ang;
28 };
29 void Axis::SetO(double a,double b)
30 {
31     O.SetPoint(a,b);
32 };
33 void Axis::SetO(Point p)
34 {
35     O=p;
36 };
37 Point Axis::GetO()
38 {
39     return O;
40 };
41 double Axis::GetAngle()
42 {
43     return Angle;
44 };
45 
46 WorldFrame::WorldFrame()
47 {
48     SetAxis(0,0,0);
49 };
50 
51 TaskFrame::TaskFrame()
52 {
53     SetAxis(0,0,0);
54 };
55 TaskFrame::TaskFrame(double a,double b,double ang):Axis(a,b,ang)
56 {
57     
58 };
59 
60 JointFrame::JointFrame()
61 {
62     Angle1=Angle2=0;
63 };
64 JointFrame::JointFrame(double ang1,double ang2)
65 {
66     Angle1=ang1;
67     Angle2=ang2;
68 };
69 void JointFrame::SetAngle1(double a)
70 {
71     Angle1=a;
72 };
73 void JointFrame::SetAngle2(double b)
74 {
75     Angle2=b;
76 };
77 double JointFrame::GetAngle1()
78 {
79     return Angle1;
80 };
81 double JointFrame::GetAngle2()
82 {
83     return Angle2;
84 };

Solver.h

 1 #ifndef SOLVER_H
 2 #define SOLVER_H
 3 #include"Axis.h"
 4 #include<math.h>
 5 #include<iostream>
 6 class Solver
 7 {
 8     public:
 9         Solver();
10         Solver(double a,double b);
11         SetL(double a,double b);
12         SetP(Point a);
13         SetJF(JointFrame j);
14         Point GetP();
15         JointFrame Getjo();
16     private:
17         Point p;
18         JointFrame jo;
19         double la,lb;
20 };
21 
22 #endif

Solver.cpp

 1 #include "Solver.h"
 2 #define PI 3.1415926
 3 Solver::Solver(){};
 4 Solver::Solver(double a,double b)
 5 {
 6     la=a;
 7     lb=b;
 8 };
 9 Solver::SetL(double a,double b)
10 {
11     la=a;
12     lb=b;
13 };
14 Solver::SetP(Point a)
15 {
16     p=a;
17     double l=sqrt(a.GetX()*a.GetX()+a.GetY()*a.GetY());
18     if(l>la+lb)std::cout<<"error!"<<std::endl;
19     else
20     {
21         jo.SetAngle1((acos((la*la+l*l-lb*lb)/2/la/l)+atan(a.GetY()/a.GetX()))/PI*180);
22         jo.SetAngle2(acos((la*la+lb*lb-l*l)/2/la/lb)/PI*180);
23     }
24 };
25 Solver::SetJF(JointFrame j)
26 {
27     jo=j;
28     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));
29 };
30 Point Solver::GetP()
31 {
32     return p;
33 };
34 JointFrame Solver::Getjo()
35 {
36     return jo;
37 };

Robot.h

 1 #ifndef ROBOT_H
 2 #define ROBOT_H
 3 #include<math.h>
 4 #include<vector>
 5 #include"Solver.h"
 6 class Robot
 7 {
 8     public:
 9         Robot(double la,double lb);
10         JointFrame GetJF();
11         double GetLengthA();
12         double GetLengthB();
13         Point TF2WF(TaskFrame t,Point p);
14         Point WF2TF(TaskFrame t,Point p);
15         JointFrame P2Pmove(TaskFrame t,Point p);
16         JointFrame P2Pmove(WorldFrame t,Point p);
17     private:
18         JointFrame JF;
19         WorldFrame WF;
20         std::vector<TaskFrame> TF;
21         double LengthA,LengthB;
22         Solver s;
23 };
24 
25 #endif

Robot.cpp

 1 #include "Robot.h"
 2 #define PI 3.1415926
 3 Robot::Robot(double la,double lb):s(la,lb)
 4 {
 5     LengthA=la;
 6     LengthB=lb;
 7 };
 8 JointFrame Robot::GetJF()
 9 {
10     return JF;
11 };
12 double Robot::GetLengthA()
13 {
14     return LengthA;
15 };
16 double Robot::GetLengthB()
17 {
18     return LengthB;
19 };
20 Point Robot::TF2WF(TaskFrame t,Point p)
21 {
22     double x=p.GetX()*cos(t.GetAngle()/180*PI)+t.GetO().GetX()-p.GetY()*sin(t.GetAngle()/180*PI);
23     double y=t.GetO().GetY()+p.GetY()*cos(t.GetAngle()/180*PI)+p.GetX()*sin(t.GetAngle()/180*PI);
24     Point temp(x,y);
25     return temp;
26 };
27 Point Robot::WF2TF(TaskFrame t,Point p)
28 {
29     double i=(p.GetX()-t.GetO().GetX())/tan(t.GetAngle()/180*PI);
30     double j=p.GetY()-t.GetO().GetY()-i;
31     double y=j*cos(t.GetAngle()/180*PI);
32     double x=j*sin(t.GetAngle()/180*PI)+i/sin(t.GetAngle()/180*PI);
33     Point temp(x,y);
34     return temp;
35 };
36 JointFrame Robot::P2Pmove(TaskFrame t,Point p)
37 {
38     s.SetP(TF2WF(t,p));
39     return s.Getjo();
40 };
41 JointFrame Robot::P2Pmove(WorldFrame t,Point p)
42 {
43     s.SetP(p);
44     return s.Getjo();
45 };

main.cpp

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