实时控制软件设计第二次编程作业

  1 #include <iostream>
  2 #include"robot.h"
  3 /* run this program using the console pauser or add your own getch, system("pause") or input loop */
  4 
  5 int main(int argc, char** argv) {
  6     Robot Robot(140,200,6,4);
  7     jointframe JF;
  8     worldframe WF;
  9     taskframe TF1(4,3,35),TF2(2,2,0),TF3(0,1,40);
 10     Robot.PTPMove(JF,150,240);
 11     Robot.PTPMove(WF,-1,7);
 12     Robot.PTPMove(TF1,-2,3);
 13     Robot.PTPMove(TF2,-1,5);
 14     Robot.PTPMove(TF3,-1,3);
 15     return 0;
 16 }
 17 #ifndef FRAME_H
 18 #define FRAME_H
 19 #include <iostream>
 20 #include <Eigen/Dense>
 21 #include <cmath>
 22 #include <vector>
 23 #define pi 3.1415926
 24 using Eigen::MatrixXd;
 25 using namespace std;
 26 
 27 class jointframe{
 28 private:
 29     double deg1;
 30     double deg2;
 31 public:
 32     jointframe(double deg=0,double deg4=0);
 33     double getdeg1();
 34     double getdeg2();
 35 };
 36 
 37 class worldframe{
 38 private:
 39     double wfx;
 40     double wfy;
 41     double wfdeg;
 42 public:
 43     worldframe();
 44     //worldframe(double wfx1,double wfy1,double wfdeg1);
 45     //MatrixXd getWF();
 46     //double getX();
 47     //double getY();
 48 };
 49 
 50 
 51 
 52 class taskframe{
 53 private:
 54     double tfx;
 55     double tfy;
 56     double tfdeg;
 57 public:
 58     taskframe();
 59     taskframe(double tfx1,double tfy1,double tfdeg1);
 60     MatrixXd getTF();
 61     double getX();
 62     double getY();
 63 };
 64 #endif
 65 #include "frame.h"
 66 jointframe::jointframe(double deg3,double deg4){
 67     deg1=deg3;
 68     deg2=deg4;
 69 }
 70 double jointframe::getdeg1(){
 71     return deg1;
 72 }
 73 double jointframe::getdeg2(){
 74     return deg2;
 75 }
 76 worldframe::worldframe(){
 77     wfx=wfy=0;
 78     wfdeg=0*pi/180;
 79 }
 80 taskframe::taskframe(){
 81     tfx=tfy=0;
 82     tfdeg=0*pi/180;
 83 }
 84 taskframe::taskframe(double tfx1,double tfy1,double tfdeg1){
 85     tfx=tfx1;
 86     tfy=tfy1;
 87     tfdeg=tfdeg1*pi/180;
 88 }
 89 MatrixXd taskframe::getTF(){
 90     MatrixXd TF(3,3);
 91     TF(0,0)=cos(tfdeg);
 92     TF(0,1)=sin(tfdeg);
 93     TF(0,2)=tfx;
 94     TF(1,0)=-sin(tfdeg);
 95     TF(1,1)=cos(tfdeg);
 96     TF(1,2)=tfy;
 97     TF(2.0)=TF(2,1)=0;
 98     TF(2,2)=1;
 99     return TF;
100 }
101 double taskframe::getX(){
102     return tfx;
103 }
104 double taskframe::getY(){
105     return tfy;
106 }
107 #ifndef SOLVER_H
108 #define SOLVER_H
109 #include"frame.h"
110 
111 class Solver{
112 private:
113     double x,y;
114     double theta3,theta4;
115     worldframe WF1;
116 public:
117     Solver();
118     MatrixXd getXY(double a,double b,double l1,double l2);
119     double getdeg1(double c,double d,double l1,double l2);
120     double getdeg2(double c,double d,double l1,double l2);
121 };
122 #endif 
123 #include"solver.h"
124 Solver::Solver(){
125     x=y=0;
126     theta3=theta4=0;
127 }
128 MatrixXd Solver::getXY(double a,double b,double l1,double l2){
129     MatrixXd T(3,3);
130     double ang1,ang2,arm1,arm2;
131     ang1=a;
132     ang2=b;
133     arm1=l1;
134     arm2=l2;
135     T(0,0)=cos(ang1+ang2);
136     T(0,1)=-sin(ang1+ang2);
137     T(0,2)=arm2*cos(ang1+ang2)+arm1*cos(ang1);
138     T(1,0)=sin(ang1+ang2);
139     T(1,1)=cos(ang1+ang2);
140     T(1,2)=arm2*sin(ang1+ang2)+arm1*sin(ang1); 
141     T(2,0)=T(2,1)=0;
142     T(2,2)=1;
143     return T;
144 }
145 double Solver::getdeg1(double c,double d,double l1,double l2){
146     double theta34;
147     double arm1,arm2,l;
148     x=c;
149     y=d;
150     l=abs(sqrt(x*x+y*y));
151     arm1=l1;
152     arm2=l2;
153     theta34=atan2(y,x)/pi*180;
154     theta3=theta34+acos((-arm2*arm2+arm1*arm1+l*l)/(2*arm1*l))/pi*180;
155     theta4=acos((-l*l+arm1*arm1+arm2*arm2)/(2*arm1*arm2))/pi*180+180;
156     return theta3;  
157 }
158 double Solver::getdeg2(double c,double d,double l1,double l2){
159     double theta34;
160     double arm1,arm2,l;
161     x=c;
162     y=d;
163     l=abs(sqrt(x*x+y*y));
164     arm1=l1;
165     arm2=l2;
166     theta34=atan2(y,x)/pi*180;
167     theta3=theta34+acos((-arm2*arm2+arm1*arm1+l*l)/(2*arm1*l))/pi*180;
168     theta4=acos((-l*l+arm1*arm1+arm2*arm2)/(2*arm1*arm2))/pi*180+180;
169     return theta4;  
170 }
171 #ifndef ROBOT_H
172 #define ROBOT_H
173 #include"solver.h"
174 class Robot{
175 private:
176     double theta1;
177     double theta2;
178     double seg1;
179     double seg2;
180     jointframe JF;
181     worldframe WF;
182     std::vector<taskframe> TF;
183     Solver normal;
184     Solver inverse;
185 public:
186     Robot();
187     Robot(double alpha,double beta,double lenth1,double lenth2);
188     void PTPMove(jointframe JF1,double X,double Y);
189     void PTPMove(worldframe WF1,double X,double Y);
190     void PTPMove(taskframe TF1,double X,double Y);
191 }; 
192 #endif
193 #include"robot.h"
194 Robot::Robot(){
195     theta1=180/pi*180;
196     theta2=270/pi*180;
197     seg1=0;seg2=0;
198 }
199 Robot::Robot(double alpha,double beta,double lenth1,double lenth2){
200     theta1=alpha/pi*180;
201     theta2=beta/pi*180;
202     seg1=lenth1;
203     seg2=lenth2;
204 }
205 void Robot::PTPMove(jointframe JF1,double X,double Y){
206     double x,y,c,d;
207     MatrixXd B(3,1);
208     c=X+JF1.getdeg1();
209     d=Y+JF1.getdeg2();
210     x=c*pi/180;
211     y=d*pi/180;
212     if(c>180||c<100||d<220||d>300)
213     {
214         cout<<"无法旋转到该位置"<<endl; 
215     }
216     else
217     {
218     B=normal.getXY(x,y,seg1,seg2);
219     cout<<"关节坐标为"<<‘(‘<<B(0,0)<<‘,‘<<B(1,0)<<‘)‘<<endl;    
220     }
221     
222 }
223 void Robot::PTPMove(worldframe WF1,double X,double Y){
224     double deg1,deg2,x,y;
225     x=X;
226     y=Y;
227     deg1=inverse.getdeg1(x,y,seg1,seg2);
228     deg2=inverse.getdeg2(x,y,seg1,seg2);
229     if(deg1>180||deg1<100||deg2<220||deg2>300)
230     {
231         cout<<"无法旋转到该位置"<<endl;
232     }
233     else
234     {    
235          cout<<"关节坐标为"<<‘(‘<<deg1<<‘,‘<<deg2<<‘)‘<<endl;
236     }
237 }
238 void Robot::PTPMove(taskframe TF1,double X,double Y){
239     double deg1,deg2,x,y;
240     MatrixXd TFs(3,3),coordt(3,1),anst(3,1);
241     coordt(0,0)=X;
242     coordt(1,0)=Y;
243     coordt(2,0)=1;
244     TFs=TF1.getTF();
245     anst=TFs*coordt;
246     x=anst(0,0);
247     y=anst(1,0);
248     deg1=inverse.getdeg1(x,y,seg1,seg2);
249     deg2=inverse.getdeg2(x,y,seg1,seg2);
250     if(deg1>180||deg1<100||deg2<220||deg2>300)
251     {
252         cout<<"无法旋转到该位置"<<endl;
253     }
254     else
255     {
256         cout<<"关节坐标为"<<‘(‘<<deg1<<‘,‘<<deg2<<‘)‘<<endl;
257     }
258 }