setpos.c

hoap2 robot access source code

/*
  モ〖タの附哼疤弥を评て、办忍コマンドで嫡にこれをセットするプログラム
*/
#include <stdio.h>
#include <string.h>
#include <stdlib.h>

#include <sys/time.h>
#include <sys/types.h>
#include <unistd.h>

/*鼎铜メモリ脱*/
#include <mbuff.h>
#include <MemMan.h>
#define NAME_OF_MEMORY "DataSharedMemory"
SM* pSM;

#include "sm_access.h"

#define COMMANDLEN 4096

int waitret();

int main(int argc, char *argv[])

{
	DUNIT *unit;
	int i;
	unsigned short mtrp;

	printf("MotorMax=%d\n",MotorMax);
	//鼎铜メモリアロケ〖ション
	pSM = (SM*) mbuff_alloc(NAME_OF_MEMORY, \
						sizeof(SM));
	if(pSM == NULL) {
		perror("mbuff_alloc failed");
		return -1;
	}
	
//	pSM->VarIF.ResInt = 1;

//HOAP2NEW  MotorMax --> MotorMax-1
 	for(i = 0; i < MotorMax-1; i++) {
//		/*办忍コマンドデ〖タ奶慨窗位フラグをチェック*/
//		if(pSM->VarIF.ResInt == 1) {
			unit = (DUNIT*)&(pSM->Data.IntDat);
			unit->Step = (unsigned short)2;
			unit->Header = (unsigned char)3;
			unit->Cmd.DevID = (unsigned char)(i+1);
			/*モ〖タステ〖タス艰评コマンド*/
			unit->Cmd.CmdAsc = (unsigned char)'a';
			unit->StepOver = 0;
			unit->ResTime = 0;
			/*办忍コマンドデ〖タ奶慨窗位フラグをクリア*/
			pSM->VarIF.ResInt = 0;
			pSM->VarIF.Mode = IDLE;
			/*インタ〖ラプトフラグをONにして、
			ダイレクトコントロ〖ルモ〖ドの悸乖を回绩する*/
			pSM->VarIF.InterruptSend = TRUE;
			printf("send Motor %d to 'a'\n", i+1);

//			while(pSM->VarIF.ResInt == 0) {
//				sleep(2);
//			}
			//悸乖窗位して猛が掐るまで略つ
			if(waitret() != 0) {
				printf("cannot get current motor position\n");
				return -1;
			}

			/*艰评したモ〖タ疤弥デ〖タを、 eコマンドでセットする。*/
			mtrp = unit->Cmd.ResW[0];
			unit->Step = (unsigned short)2;
			unit->Header = (unsigned char)3;
			unit->Cmd.DevID = (unsigned char)(i+1);
			unit->Cmd.CmdAsc = (unsigned char)'e';
			unit->Cmd.ArgW[0] = (unsigned short)mtrp;
			unit->StepOver = 0;
			unit->ResTime = 0;
			/*办忍コマンドデ〖タ奶慨窗位フラグをクリア*/
			pSM->VarIF.ResInt = 0;
			pSM->VarIF.Mode = IDLE;
			/*インタ〖ラプトフラグをONにして、
			ダイレクトコントロ〖ルモ〖ドの悸乖を回绩する*/
			pSM->VarIF.InterruptSend = TRUE;
			printf("send Motor %d to 'e', %hd\n", i+1, (short)mtrp);
//			while(pSM->VarIF.ResInt == 0) {
//				sleep(2);
//			}
			//悸乖窗位して猛が掐るまで略つ
			if(waitret() != 0) {
				printf("cannot get current motor position\n");
				return -1;
			}

//		}
	}

	/*メモリ倡庶*/
	mbuff_free(NAME_OF_MEMORY, (void*)pSM);

	return 0;
}

int waitret()
{
	/*ダイレクトコントロ〖ルモ〖ドの窗位を略つ*/
	fd_set rfds;
	struct timeval tv;
	int retval;

	for(;;) {
		FD_ZERO(&rfds);
		FD_SET(0, &rfds);
		tv.tv_sec = 0;
		tv.tv_usec = 0;
		retval = select(1, &rfds, NULL, NULL, &tv);
		if(retval) {
//			break;
			return -1;
		}
		if(pSM->VarIF.ResInt) {
			break;
		}
	}
	return 0;
}