control.cpp

来自「机器人运动学动力学及控制的一个小程序,供有兴趣的人参考」· C++ 代码 · 共 54 行

// Control.cpp: implementation of the CControl class.
//
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include "MySDOpenGL.h"
#include "Control.h"

#ifdef _DEBUG
#undef THIS_FILE
static char THIS_FILE[]=__FILE__;
#define new DEBUG_NEW
#endif

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////

CControl::CControl()
{

}

CControl::~CControl()
{

}

//////////////////////////////////////////////////////////////////////
//计算控制规则
//由给定的当前速度，当前位置，及期望加速度，速度，位置，定常增益矩阵，
//指向动力学类的指针，计算出应加力矩
//
// 参数：
// 1、dx    -当前速度
// 2、x     -当前位置
//
// 返回值：CMatrix 型，应加力矩
//////////////////////////////////////////////////////////////////////
CMatrix CControl::ControlTorqueCL(CMatrix dx,CMatrix x)
{CMatrix result;
 CMatrix de;
 CMatrix e;
         de=dx-Designdx;
		 e=x-Designx;

         result=lpDynamics->M*(DesignDdx-Kv*de-Kp*e)+lpDynamics->C*dx+lpDynamics->N;


        return result;

}