ipm.cpp

比赛足球机器人系统中的DCT追球程序

#include "stdafx.h"
#include "ipm.h"
#include <stdlib.h>

///////////////////////////////////////////////////////////////////
//
// Functions and Macros
//

///////////////////////////////////////////////////////////////////
//
// Clear up and return
//
#define Fail(retCode)    \
	do {                     \
	if (axisL)            \
	axisL->Release(); \
	if (axisR)            \
	axisR->Release(); \
	\
	if (ctrl)            \
	ctrl->Release(); \
	\
	if (netif)           \
	netif->Release();\
	\
	if (sys)             \
	sys->Release();  \
	\
	sys = NULL;   \
	netif = NULL;   \
	ctrl = NULL;   \
	axisL = NULL;  \
	axisR = NULL;   \
	return retCode;      \
	} while (0)

/////////////////////////////////////////////////////////////////////

Ipm::Ipm()
{
    bIPMOk = false;

	sys = NULL;
	netif = NULL;
	ctrl = NULL;
	axisL = NULL;
	axisR = NULL;
}

Ipm::~Ipm()
{
}


// 初始化控制板
bool Ipm::initIpm()
{

	////读配置文件设定变量
	//if(!LoadConfig("ipm.cfg"))
	//{
	//	cerr<<"ERROR:控制板读取配置错误。"<<endl;
	//	return false;
	//}

	char str_com[10];
	sprintf(str_com,"COM%d",COM_NUM);
	str_com[9]=0;

	bIPMOk=false;

	HRESULT r;

	/////////////////////////////////////////////////////////
	try {
		if (NULL == (sys = IDCTMotionControlSystemCreate()))
			Fail(false);

		r = sys->get_NetInterface(&netif);
		if (FAILED(r) || !netif)
			Fail(false);

		r = sys->get_MotionController(0, &ctrl);
		if (FAILED(r) || !ctrl)
			Fail(false);

		////////// IDCTMCNetInterface //////////////
		if (FAILED(r = netif->put_NetworkType(_bstr_t("RS232"))))
			Fail(false);
		if (FAILED(r = netif->put_Timeout(50)))
			Fail(false);
		if (FAILED (r = netif->put_PortName(_bstr_t(str_com)))) {
			cerr<<"打开串口失败。"<<endl;
			Fail(false);
		}
		if (FAILED(r = netif->put_BaudRate(115200)))
			Fail(false);

		////////// IDCTMCMotionController //////////////
		ctrl->Reset();

		//kick off
		ctrl->put_OptoOutput(1, 0, 0, 0, 0);

		if (FAILED((r = ctrl->get_Axis(LEFT_AXISID, &axisL))) || !axisL)
			Fail(false);
		if (FAILED((r = ctrl->get_Axis(RIGHT_AXISID, &axisR))) || !axisR)
			Fail(false);

		//{
		//	ctrl->get_Error(&err);
		//	BSTR e;
		//	ctrl->get_ErrorDescription(err, &e);
		//	_bstr_t ed(e);
		//	printf("0x%x\n%s\n", err, (char*)ed);
		//}

		//enable axis
		axisL->put_Enable(1);
		axisR->put_Enable(1);



	} catch(IErrorInfo* e) {
		_com_error err(r, e);
		_bstr_t ei = err.Description();
		printf("\n%s\n", (char*)ei);
	}


	bIPMOk=true;

	cout<<"DCTMC控制板初始化成功。"<<endl;

	return true;
}

bool Ipm::LoadConfig(const char * filename)
{
	char str[100];
	int line_num = 0;
	FILE * fp;

	if((fp = fopen(filename,"r"))== NULL) {
		fprintf(stderr,"read ipm configure file fail\n");
		return false;
	}

	while(fgets(str,99,fp)) {
		line_num++;
		//空行和注释（以＃开头的行为注释行）
		if(str[0] == '#' || str[0] == '\n')
			continue;
		else if(!strncmp(str,"COM",3)) {
			if(sscanf(str+3," %d",&COM_NUM) < 1) {
				printf("IPM configure file error line: %d\n",line_num);
				return false;
			}
		} else if(!strncmp(str,"LEFT_AXISID",11)) {
			if(sscanf(str+11," %d",&LEFT_AXISID) < 1) {
				printf("IPM configure file error line: %d\n",line_num);
				return false;
			}
		} else if(!strncmp(str,"RIGHT_AXISID",12)) {
			if(sscanf(str+12," %d",&RIGHT_AXISID) < 1) {
				printf("IPM configure file error line: %d\n",line_num);
				return false;
			}
		} else if(!strncmp(str,"KICKER_AXISID",13)) {
			if(sscanf(str+13," %d",&KICKER_AXISID) < 1) {
				printf("IPM configure file error line: %d\n",line_num);
				return false;
			}
		} else {
			printf("IPM configure file error line: %d Unknown option %s\n",line_num,str);
			return false;
		}
	}
	return true;
}

void Ipm::axisOff()
{
	if(bIPMOk) {
		axisL->put_Enable(0);
		axisR->put_Enable(0);
	}
}

void Ipm::axisOn()
{
	if(bIPMOk) {
		axisL->put_Enable(1);
		axisR->put_Enable(1);
	}
}

void Ipm::kickerOn()
{
	if(bIPMOk) {
		ctrl->put_OptoOutput(1, 0, 0, 0, 1);
	}
}

void Ipm::kickerOff()
{
	if(bIPMOk) {
		ctrl->put_OptoOutput(1, 0, 0, 0, 0);
	}
}

#define MaxDCSpeed 20000.0
bool Ipm::SendToIPM()
{
	if(!bIPMOk) 
		return false;

	if(lv<-MaxDCSpeed)
		lv=-MaxDCSpeed;
	if(lv>MaxDCSpeed)
		lv=MaxDCSpeed;

	if(rv<-MaxDCSpeed)
		rv=-MaxDCSpeed;
	if(rv>MaxDCSpeed)
		rv=MaxDCSpeed;

	HRESULT res;
	LONG err = 0;
	CComBSTR e;

	LONG l,r;
	l=(LONG)(lv*768);
	r=(LONG)(rv*768);

	//send spd
	res=axisL->Jog(-l);
	if(res!=S_OK) {
		//bIPMOk=false;
		axisL->get_Error(&err);
		axisL->get_ErrorDescription(err, &e);
		_bstr_t ed1(e);
		printf("---左轮错误:0x%x\n%s\n", err, (char*)ed1);
		Sleep(5000);
		return false;
	}

	res=axisR->Jog(r);
	if(res!=S_OK) {
		//bIPMOk=false;
		axisR->get_Error(&err);
		axisR->get_ErrorDescription(err, &e);
		_bstr_t ed1(e);
		printf("========右轮错误:0x%x\n%s\n", err, (char*)ed1);
		Sleep(5000);
		return false;
	}

	cout<<"已发送速度:"<<l<<","<<r<<endl;

	return true;
}

void Ipm::quit()
{
	lv = 0;
	rv = 0;
	SendToIPM();
	
	kickerOff(); //kick off
	axisOff(); //power off

	Sleep(500); 

	if (axisL)  
		axisL->Release();
	if (axisR)            
		axisR->Release(); 
	if (ctrl) 
		ctrl->Release();
	if (netif)  
		netif->Release();
	if (sys)           
		sys->Release(); 

	sys = NULL;
	netif = NULL;
	ctrl = NULL;
	axisL = NULL;
	axisR = NULL;

	//printf("控制板退出\n");
}