interpol.c~

hoap2 robot access source code

/*
  ロボットのモ〖タの附哼猛から、肌に涂えるシ〖ケンスモ〖ドファイルの
  介袋猛との汗を输粗するCSVファイルを侯喇するプログラム
  CSVファイルの叹涟はパラメ〖タとして回年する。回年しなければ
  "interpol.csv"になる。肌に涂えるシ〖ケンスモ〖ドファイルは
  リダイレクトで涂える。リダイレクトを蝗脱せずに、呵姜誊筛疤弥を
  筛洁掐蜗から掐蜗することもできる。
 */

#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
#define OUTPUTFILE "interpol.csv"
void usage();
int waitret();

int main(int argc, char *argv[])
{
	char buf[COMMANDLEN];
	int ret;
	DUNIT* unit;
	DUNIT endunit;
	short mtrp[MotorMax];
	int l,m,i;
	double add_pls;
	double add_pls1;
	int pls;
	FILE* fdo;
	char *outfilename;

	/*アウトプットファイル叹を回年*/
	switch(argc) {
	case 2:
	  outfilename = argv[1];
	  break;
	default:
	  outfilename = OUTPUTFILE;
	}
	if((fdo = fopen(outfilename, "w")) == NULL) {
		perror("open writefile");
		return -1;
	}

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

	/*筛洁掐蜗からのコマンド掐蜗を略つ*/
	printf("command:");			
	if( NULL == fgets(buf, COMMANDLEN, stdin)) {
		usage();
		mbuff_free(NAME_OF_MEMORY, (void*)pSM);
		fclose(fdo);
		return -1;
	}
//	printf("%s", buf);
	printf("\n");

	/*筛洁掐蜗からモ〖タの呵姜誊筛猛を评る*/
	ret = send_sm(buf, &endunit);
	if(endunit.Header != 2 || ret < 0) {
		printf("endpoint format error\n");
		mbuff_free(NAME_OF_MEMORY, (void*)pSM);
		fclose(fdo);
		return -1;
	}

	/*モ〖タ附哼疤弥を评る*/
	printf("Current motor potision:");
 	for(i = 0; i < MotorMax; 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");
				mbuff_free(NAME_OF_MEMORY, (void*)pSM);
				return -1;
			}

			mtrp[i] = (short)unit->Cmd.ResW[0]; //モ〖タポジションを评る
			printf("%hd, ", mtrp[i]);
//		}
	}
	printf("\n");

	/*附哼猛と呵姜誊筛猛との粗を1000尸充して输粗したファイルを侯喇する*/
	for(l = 0; l < 1000; l++) {
		fprintf(fdo, "2,2,"); //Step, Header
//		printf("2,2,"); //Step, Header
		for(m = 0; m < MotorMax-1; m++) {
			/*俐妨输粗¨介袋猛と呵姜猛との粗を1000尸充して1ステップとする*/
			add_pls = (double)((short)(endunit.Ctl.MtrS[m]) - (short)(mtrp[m]))/1000;
//			printf("%lf ", (double)add_pls);
			add_pls1 = l * add_pls;
			/*煌嘉皋掐*/
			if(add_pls1 > 0) add_pls1 += 0.5;
			if(add_pls1 < 0) add_pls1 -= 0.5;
			/*输粗猛の纷换*/
			pls = mtrp[m] + (short)add_pls1;
			/*ファイル叫蜗*/
			fprintf(fdo, "%hd,", (short)pls);
//			printf("%hd,", (short)pls);
		}
		fprintf(fdo, "60,"); //fprintf(fdo, "R,R,R,R\n");
		fprintf(fdo, "R,R,R\n"); //fprintf(fdo, "R,R,R,R\n");
//		printf("R,R,R,R\n");
	}
	fprintf(fdo, "2,2,"); //Step, Header
//	printf("2,2,"); //Step, Header
	/*输粗デ〖タの呵稿に、呵姜誊筛猛(肌に掐蜗するファイルの介袋猛)を掐れる*/
	for(m = 0; m < MotorMax-1; m++) {
		fprintf(fdo, "%hd,", (short)endunit.Ctl.MtrS[m]);
//		printf("%hd,", (short)endunit.Ctl.MtrS[m]);
	}
	fprintf(fdo, "60,");  //fprintf(fdo, "R,R,R,R\n");
	fprintf(fdo, "R,R,R\n");  //fprintf(fdo, "R,R,R,R\n");
//	printf("R,R,R,R\n");

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

	return 0;
}

void usage()
{
	printf("interpol outputfile < next_csvfile\n");
}


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;
		}
	}
*/
//	int i;
//	for(i = 0; i < 100000; i++) {
	while(1) {
		if(pSM->VarIF.ResInt) {
			break;
		}
	}
	return 0;
}