实时软件控制第二次作业

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;

}