Robot.h
#ifndef H_ROBOT_H
#define H_ROBOT_H
#include <vector>
#include "CFrame.h"
class CRobot
{
private:
double arm1;
double arm2;
//double arm1AngleRange[2];
//double arm2AngleRange[2];
vector<CFrame*> fameVector; //针对所有坐标系
public:
CRobot(){}
CRobot(double armx,double army);
};
#endif
Robot.cpp
#include "Robot.h"
#include "Solver.h"
#include "Point.h"
CRobot::CRobot(double armx,double army)
{
arm1=armx;
arm2=army;
}
Solver.h
#ifndef H_SOLVER_H
#define H_SOLVER_H
#include "WorldFrame.h"
#include "CJointFrame.h"
#include "TaskFrame.h"
class CSolver
{
private:
CJointFrame CJointFrame;
WorldFrame worldframe;
TaskFrame taskFrame;
public:
CPoint TF2WF(TaskFrame fr,CPoint po); //将任务坐标系转化为世界坐标系
CPoint WF2TF(WorldFrame fr,CPoint po); //将世界坐标系转化为任务坐标系
CJointFrame WF2JF(CPoint po,double arm1,double arm2); //在世界坐标系下反算关节坐标
WorldFrame JF2WF(CPoint po,double arm1,double arm2); //在关节坐标下反算世界坐标系
};
#endif
Solver.cpp
#include <iostream>
#include "Solver.h"
#define PI 3.1415926
using namespace std;
CPoint TF2WF(TaskFrame fr,CPoint po) //将任务坐标系转化为世界坐标系
{
CPoint point1=rotate(po,fr.getDegree());
CPoint point2=move(point1,fr.getPoint());
return point1;
}
void CSolver::WF2JF(CPoint 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.setAngle1(rad1+rad11);
joint.setAngle2(rad2+rad22);
//cout<<"关节1应转动角度为："<<joint.getAngle1()<<" 关节2应转动角度为："<<joint.getAngle2()<<endl;
}
}
//将世界坐标系下的关节坐标还原为用户坐标系
/*FrameReturn(CFrame fr)
{
}
JointToFrame(CPoint po)
{
}*/
Frame.h
#ifndef H_FRAME_H
#define H_FRAME_H
#include "Point.h"
#include "Solver.h"
#include "JointFrame.h"
class CFrame
{
private:
CPoint origin;
double degree;
CSolver CSolver;
CJointFrame CJointFrame;
public:
CFrame(){}
CFrame(CPoint orig,double deg);
CPoint getPoint() const;
double getDegree() const;
virtual void PTPmove(CPoint po)=0;
virtual ~CFrame(){}
};
#endif
Frame.cpp
#include"Frame.h"
CFrame::CFrame(CPoint orig,double deg)
{
origin=orig;
degree=deg;
}
CPoint CFrame::getPoint() const
{
return origin;
}
double CFrame::getDegree() const
{
return degree;
}
WorldFrame.h
#include"CFrame.h"
class CWorlfFrame :public CFrame
{
virtual void PTPmove(CPoint po) //实现在自己坐标系下的PTPmove
{
}
}
TaskFrame.cpp
#include"CFrame.h"
class TaskFrame :public CFrame
{
virtual void PTPmove(CPoint po) //实现在自己坐标系下的PTPmove
{
}
}
JointFrame.h
#ifndef H_JOINTfRAME_H
#define H_JOINTfRAME_H
#include"Frame.h"
class CJointFrame :public CFrame
{
private:
double angle1;
double angle2;
public:
CJointFrame(){}
CJointFrame(double ang1,double ang2);
void setAngle1(double ang);
void setAngle2(double ang);
double getAngle1() const;
double getAngle2() const;
virtual void PTPmove(CPoint po) //实现在自己坐标系下的PTPmove
{
}
virtual ~CJointFrame(){}
}
#endif
JointFrame.cpp
#include "JointFrame.h"
CJointFrame::CJointFrame(double ang1,double ang2)
{
angle1=ang1;
angle2=ang2;
}
void CJointFrame::setAngle1(double ang)
{
angle1=ang;
}
void CJointFrame::setAngle2(double ang)
{
angle2=ang;
}
double CJointFrame::getAngle1() const
{
return angle1;
}
double CJointFrame::getAngle2() const
{
return angle2;
}
Point.h
#ifndef H_MYPOINT_H
#define H_MYPOINT_H
class CPoint
{
private:
double x;
double y;
public:
CPoint(){}
CPoint(double xx,double yy);
//CPoint(const CPoint& p);
double getX() const;
double getY() const;
void setX(double xx);
void setY(double yy);
CPoint move(CPoint p1,CPoint p2); //点到点
CPoint rotate(CPoint p,double deg); //绕原点转动deg角度
~CPoint(){}
};
#endif
Point.cpp
#include"Point.h"
CPoint::CPoint(double xx,double yy)
{
x=xx;
y=yy;
}
double CPoint::getX() const
{
return x;
}
double CPoint::getY() const
{
return y;
}
void CPoint::setX(double xx)
{
x=xx;
}
void CPoint::setY(double yy)
{
y=yy;
}
CPoint CPoint::move(CPoint p1,CPoint p2)
{
CPoint CPoint(p1.getX()+p2.getX(),p1.getY()+p2.getY());
return CPoint;
}
CPoint CPoint::rotate(CPoint 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);
CPoint CPoint(tempx,tempy);
return CPoint;
}