Merge pull request RTCSD15#12 from penghuichen/master

Create myprogram.cpp

Author: cbhust

U201210544/myprogram.cpp

@@ -0,0 +1,171 @@
+#include <iostream>
+#include <math.h>
+#define PI 3.1415926
+using namespace std;
+  
+class Point{
+    private:
+        double x;
+        double y;
+    public:
+        Point(){}
+        Point(double a,double b){
+            x=a;
+            y=b;
+        }
+        double getX(){
+            return x;
+        }
+        double getY(){
+            return y;
+        }
+        void setX(double a){
+            x=a;
+        }
+        void setY(double b){
+            y=b;
+        }
+          
+};
+  
+ 
+class Frame{
+    private:
+        Point origin;
+        double degree;
+    public:
+        Frame(){}
+        Frame(Point orig,double deg){
+            origin=orig;
+            degree=deg;
+         }
+        Point getPoint(){
+            return origin;
+        }
+        double getDegree(){
+            return degree;
+        }
+          
+};
+class Joint{
+    private:
+        double deg1;
+        double deg2;
+    public:
+        Joint(){}
+        Joint(int a,int b){
+            deg1=a;
+            deg2=b;
+        }
+        void setDeg1(double rad){
+            deg1=rad*180/PI;
+        }
+        void setDeg2(double rad){
+            deg2=rad*180/PI;
+        }
+        double getDeg1(){
+            return deg1;
+        }
+        double getDeg2(){
+            return deg2;
+        }
+          
+};
+  
+  
+class Solver{
+    private:
+        Joint joint;
+        Frame frame;
+    public:
+        Point move(Point p1,Point p2){
+            double tempx=p1.getX()+p2.getX();
+            double tempy=p1.getY()+p2.getY();
+            Point point(tempx,tempy);
+            return point;
+        }
+        Point rotate(Point p,double deg){
+            double tempx;
+            double tempy;
+            tempx=p.getX()*cos(PI*deg/180)-p.getY()*sin(PI*deg/180);
+            tempy=p.getX()*sin(PI*deg/180)+p.getY()*cos(PI*deg/180);
+            Point point(tempx,tempy);
+            return point;
+        }
+          
+        Point FrameStandardize(Frame fr,Point po){
+            Point point1=rotate(po,fr.getDegree());
+            Point point2=move(point1,fr.getPoint());
+            return point1;
+        }
+        FrameToJoint(Point po,double arm1,double arm2){
+            double len= sqrt(po.getX()*po.getX()+po.getY()*po.getY());
+              
+            if(len>=(arm1+arm2)||len<=abs(arm1-arm2)){
+                cout<<"坐标超范围，机器人无法达到"<<endl;
+            }else{
+                double rad1=acos((arm1*arm1+len*len-arm2*arm2)/(2*arm1*len));
+                double rad2=acos((arm1*arm1+arm2*arm2-len*len)/(2*arm1*arm2));
+                double rad11=atan(po.getY()/po.getX());
+                double rad22=PI;
+                joint.setDeg1(rad1+rad11);
+                joint.setDeg2(rad2+rad22);
+                cout<<"关节1应转动角度为："<<joint.getDeg1()<<" 关节2应转动角度为："<<joint.getDeg2()<<endl;
+            }
+                      
+        }
+        FrameReturn(Frame fr){
+        }
+        JointToFrame(Point po){
+              
+        }
+          
+};
+class Robot{
+    private:
+        double arm1;
+        double arm2;
+        double arm1Range;
+        double arm2Range;
+    public:
+        Robot(){}
+        Robot(double a,double b){
+            arm1=a;
+            arm2=b;
+        }
+        void PTPmove(Frame fr,Point po){
+            Solver solver;
+            Point point;
+              
+            point=solver.FrameStandardize(fr,po);
+            solver.FrameToJoint(point,arm1,arm2);
+                  
+        }
+};
+   
+int main(){
+    Robot myRobot(4,3);  
+      
+    Point origin(0,0);
+    Point origin1(2,3);
+    Point origin2(6,1);
+    Point origin3(5,9);  
+      
+    Point point1(-5,1);
+    Point point2(-3,-1);
+    Point point3(4,2);
+    Point point4(3,7);  
+      
+    Frame WF(origin,0);
+    Frame TF1(origin1,30);
+    Frame TF2(origin2,60);
+    Frame TF3(origin3,90);
+      
+    myRobot.PTPmove(WF,point1);
+    myRobot.PTPmove(TF1,point2);
+    myRobot.PTPmove(TF2,point3);
+    myRobot.PTPmove(TF3,point4);
+      
+      
+    return 0;
+}