// 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;

}