实时软件控制第二次作业

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;

}