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

  1 #include <iostream>
  2 #include<Eigen/Dense>
  3 #include<cmath>
  4 #define PI 3.1415926
  5 /* run this program using the console pauser or add your own getch, system("pause") or input loop */
  6 using namespace std;
  7 using namespace Eigen;
  8 class Point{
  9     public:
 10         double x;
 11         double y;
 12         
 13         Point(double a=0,double b=0);
 14         void SetP(double a,double b);
 15         void PrintP();
 16         
 17 };
 18 Point::Point(double a,double b){
 19     x=a;
 20     y=b;
 21 }
 22 void Point::SetP(double a,double b){
 23     x=a;
 24     y=b;
 25 }
 26 void Point::PrintP(){
 27     cout<<'('<<x<<','<<y<<')'<<endl;
 28 }
 29 
 30 
 31 class WorldFrame{
 32     
 33 };
 34 class TaskFrame{
 35 //    private:
 36     public:
 37         double x;
 38         double y;
 39         double deg;
 40     
 41         TaskFrame(double a=0,double b=0,double d=0);
 42 };
 43 
 44 TaskFrame::TaskFrame(double a,double b,double d){
 45     x=a;
 46     y=b;
 47     deg=d;
 48 }
 49 class JointFrame{
 50 
 51 };
 52 
 53 
 54 
 55 class Solver{
 56     public:
 57         void WtoT(TaskFrame t,Point &p);
 58         void TtoW(TaskFrame t,Point &p);
 59         void TtoJ(Point &p,double a1,double a2);
 60         void JtoT(Point &p,double a1,double a2);
 61 };
 62 
 63 void Solver::WtoT(TaskFrame t,Point &p){
 64     MatrixXd m(3,3);
 65     MatrixXd pt(1,3);
 66     m(0,0)=cos(t.deg*PI/180);
 67     m(0,1)=sin(t.deg*PI/180);
 68     m(0,2)=0;
 69     m(1,0)=-sin(t.deg*PI/180);
 70     m(1,1)=cos(t.deg*PI/180);
 71     m(1,1)=0;
 72     m(2,0)=t.x,
 73     m(2,1)=t.y;
 74     m(2,2)=0;
 75     pt(0,0)=p.x;
 76     pt(0,0)=p.y;
 77     pt(0,2)=1;
 78     pt*=m;
 79     p.x=pt(0,0);
 80     p.y=pt(0,1);    
 81 }
 82 void Solver::TtoW(TaskFrame t,Point &p){
 83     MatrixXd m(3,3);
 84     MatrixXd pt(1,3);
 85     m(0,0)=cos(-t.deg*PI/180);
 86     m(0,1)=sin(-t.deg*PI/180);
 87     m(0,2)=0;
 88     m(1,0)=-sin(-t.deg*PI/180);
 89     m(1,1)=cos(-t.deg*PI/180);
 90     m(1,1)=0;
 91     m(2,0)=t.x;
 92     m(2,1)=t.y;
 93     m(2,2)=0;
 94     pt(0,0)=-p.x;
 95     pt(0,0)=-p.y;
 96     pt(0,2)=1;
 97     pt*=m;
 98     p.x=pt(0,0);
 99     p.y=pt(0,1);
100 }
101 void Solver::TtoJ(Point &p,double a1,double a2){
102     double l,deg1,deg2,deg3;
103     l=sqrt(p.x*p.x+p.y*p.y);
104     deg1=atan(p.y/p.x);
105     deg2=acos((a1*a1+l*l-a2*a2)/(2*a1*l));
106     deg3=acos((a1*a1+a2*a2-l*l)/(2*a1*a2));
107     p.x=(deg1+deg2)*180/PI;
108     p.y=deg3*180/PI+180;
109 }
110 void Solver::JtoT(Point &p,double a1,double a2){
111     p.x=a1*cos(p.x)+a2*cos(p.y);
112     p.y=a1*sin(p.x)+a2*sin(p.y);
113 }
114 class Robot{
115     private:
116         double arm1,arm2,deg1min,deg2min,deg1max,deg2max;
117     public:
118         Robot(double a1=1,double a2=1,double d1min=0,double d2min=0,double d1max=180,double d2max=360);
119         void SetRobot(double a1=1,double a2=1,double d1min=0,double d2min=0,double d1max=180,double d2max=360);
120         void PTPMove(WorldFrame wf,TaskFrame tf,Point p);
121         void PTPMove(TaskFrame tf,Point P);
122         void PTPMove(JointFrame jf,Point P);
123         void print(Point &p);
124 };
125 
126 Robot::Robot(double a1,double a2,double d1min,double d2min,double d1max,double d2max){
127     arm1=a1;
128     arm2=a2;
129     deg1min=d1min;
130     deg2min=d2min;
131     deg1max=d1max;
132     deg2max=d2max;
133 }
134 void Robot::SetRobot(double a1,double a2,double d1min,double d2min,double d1max,double d2max){
135     arm1=a1;
136     arm2=a2;
137     deg1min=d1min;
138     deg2min=d2min;
139     deg1max=d1max;
140     deg2max=d2max;
141 }
142 void Robot::PTPMove(WorldFrame wf,TaskFrame tf,Point p){
143     Solver s;
144     s.WtoT(tf,p);
145     s.TtoJ(p,arm1,arm2);
146     print(p);
147 }
148 void Robot::PTPMove(TaskFrame tf,Point p){
149     Solver s;
150     s.TtoJ(p,arm1,arm2);
151     print(p);
152 }
153 void Robot::PTPMove(JointFrame jf,Point p){
154     print(p);
155 }
156 void Robot::print(Point &p){
157     if(p.x>=deg1min||p.y<=deg1max){
158         cout<<"关节坐标为：("<<p.x<<','<<p.y<<')'<<endl;
159     }
160     else cout<<"无法旋转到该位置"<<endl; 
161 }
162 
163 
164 
165 int main(int argc, char** argv) {
166     Robot myRobot(8,8);
167     WorldFrame WF;
168     TaskFrame TF1(0,0,0),TF2(1,2,30),TF3(3,5,90);
169     JointFrame JF;
170     Point P1(0,0),P2(1,3),P3(2,2),P4(2,3),P5(0,3);
171     myRobot.PTPMove(JF,P1);
172     myRobot.PTPMove(WF,TF1,P2);
173     myRobot.PTPMove(TF1,P3);
174     myRobot.PTPMove(TF2,P4);
175     myRobot.PTPMove(TF3,P5);
176     return 0;
177 
178 }