sm_access.c

hoap2 robot access source code

/*
 鼎铜メモリ今き哈み/粕み哈み簇眶
*/

#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include "MemMan.h"
#include "sm_access.h"

#define SEPARATOR " ,\t\n"

/*
 矢机误を豺老してデ〖タを藐叫し、パラメ〖タで涂えられた鼎铜メモリの
 バッファにデ〖タを今き哈む。
 鼎铜メモリの面でデ〖タを今き哈み、冯蔡を粕み叫すバッファは、
   シ〖ケンスモ〖ドでは、Data.RepDat
   办忍コマンドモ〖ドでは、Data.IntDat
   シ〖ケンスモ〖ドでは、Data.SeqDat
 を蝗脱する。
*/

int send_sm(char* buf, DUNIT* unit)
{
	int Step, Header;
	int d_len = 0;
	
	if(!unit) return -1;

	/*肌に悸乖されるまでの箕粗粗持*/
	if((d_len = getint(buf, &Step)) < 1) {
		printf("parameter Step error\n");
		return -1;
	}
	buf = buf + d_len;
//	printf("Step: %hu\n", (unsigned short)Step);

	/*コマンドの硷梧(办忍コマンドor年房コマンド)*/
	if((d_len = getint(buf, &Header)) < 1) {
		printf("parameter Header error\n");
		return -1;
	}
	buf = buf + d_len;
//	printf("Header: %hu\n",(unsigned short)Header);

	switch(Header) {
	case 1:
	case 2:
		{
			/*年房コマンド*/
			int i;
			int MtrS[MotorMax];
			char SnsS[SensorMax];

			/*20改のモ〖タへの流慨デ〖タ*/
			for(i = 0; i < MotorMax; i++) {
				if((d_len = getint(buf, &MtrS[i])) < 1) {
					printf("parameter MtrS error\n");
					return -1;
				}
				buf = buf + d_len;
			}
			/*3改のモ〖タへの流慨デ〖タ*/
			for(i = 0; i < SensorMax; i++) {
				while(strchr(SEPARATOR, buf[0]) != NULL) {
					if(buf[0] == '\0') {
						printf("parameter SnsS error\n");
						return -1;
					}
					buf++;
				}
				SnsS[i] =buf[0];
				buf++;
			}
			
//			printf("MtrS = ");
			for(i = 0; i < MotorMax; i++) {
				unit->Ctl.MtrS[i] = (unsigned short)MtrS[i];
//				printf("%hd ", (short)unit->Ctl.MtrS[i]);
			}
//			printf("\nSnsS = ");
			for(i = 0; i < SensorMax; i++) {
				unit->Ctl.SnsS[i] = (unsigned char)SnsS[i];
//				printf("%c ", (char)unit->Ctl.SnsS[i]);
			}
//			printf("\n");
			break;
		}
	case 3:
		{
			/*办忍コマンド:改」のデバイスにコマンドを流ることができる*/
			int DevID;
			char CmdAsc;
			short ArgB[ArgMax];
			short ArgW[ArgMax/2];
//			int ArgL[ArgMax/4];
			
			/*コマンドを流るデバイスID*/
			if((d_len = getint(buf, &DevID)) < 1) {
				printf("parameter DevID error\n");
				return -1;
			}
			buf = buf + d_len;
//			printf("DevID: %hu\n", (unsigned short)DevID);

			/*コマンド(A-z)*/
			while(strchr(SEPARATOR, buf[0]) != NULL) {
				if(buf[0] == '\0') {
					printf("parameter CmdAsc error\n");
					return -1;
				}
				buf++;
			}
			CmdAsc =buf[0];
//			printf("CmdAsc: %c\n",CmdAsc);
			buf= buf + 2;

			/*コマンドによってその稿に鲁くパラメ〖タの眶や硷梧が佰なる*/
			switch (CmdAsc) {
			case 'R': 
				break; 
			case 'S': /*ucharデ〖タ办改*/
				if(sscanf(buf, "%hd", &ArgB[0]) != 1) {
					printf("parameter ArgB error\n");
					return -1;
				}
				unit->Cmd.ArgB[0] = (unsigned char)ArgB[0];
//				printf("ArgB[0]: %hd\n", unit->Cmd.ArgB[0]);
				break;
			case 'X':
				break;
			case 'Y':
				break;
			case 'A':
			case 'B':
			case 'C':
			case 'D':
			case 'E':
			case 'F':
			case 'H':
			case 'L':
				/*ushortデ〖タ办改*/
//				printf("%s\n", buf);
				if((sscanf(buf, "%hd", &ArgW[0])) != 1) {
				printf("parameter ArgW error\n");
				return -1;
				}
				unit->Cmd.ArgW[0] = (unsigned short)ArgW[0];
//				printf("ArgW[0]: %hd\n", unit->Cmd.ArgW[0]);
				break;
			case 'I':
			case 'J':
				break;
			case 'N': 
				/*ushortデ〖タ2改*/
				if(sscanf(buf, "%hd, %hd", &ArgW[0], &ArgW[1]) != 2) {
					printf("parameter ArgW error\n");
					return -1;
				}
				unit->Cmd.ArgW[0] = (unsigned short)ArgW[0];
				unit->Cmd.ArgW[1] = (unsigned short)ArgW[1];
//				printf("ArgW[0]: %hd, ArgW[1]: %hd\n", (short)unit->Cmd.ArgW[0], (short)unit->Cmd.ArgW[1]);
				break;
			case 'a':
			case 'd':
				break;
			case 'e':
			case 'h':
			case 'k': /*ushortデ〖タ办改*/
				if(sscanf(buf, "%hd", &ArgW[0]) != 1) {
					printf("parameter ArgW error\n");
					return -1;
				}	
				unit->Cmd.ArgW[0] = (unsigned short)ArgW[0];
//				printf("ArgW[0]: %hd\n", unit->Cmd.ArgW[0]);
			case 'n':
			case 'o':
				break;

			case 's':
			case 't':
			case 'u':
			case 'v':
				if(sscanf(buf, "%hd", &ArgW[0]) != 1) {
					printf("parameter ArgW error\n");
					return -1;
				}	
				unit->Cmd.ArgW[0] = (unsigned short)ArgW[0];
//				printf("ArgW[0]: %hd\n", unit->Cmd.ArgW[0]);
				break;
			default:
				printf("paramter CmdAsc error\n");
				return -1;
			}
			/*涩妥なデ〖タが路ったら、鼎铜メモリに今き哈む*/
			unit->Cmd.DevID = (unsigned char)DevID;
//		 	printf("write DevID %d\n", unit->Cmd.DevID);
			unit->Cmd.CmdAsc = (unsigned char)CmdAsc;
//		 	printf("write CmdAsc %c\n", unit->Cmd.CmdAsc);
		}
		break;
	default:
		return -1;
	}

	unit->Step = (unsigned short)Step;
// 	printf("write Step %d\n", unit->Step);
	unit->Header = (unsigned char)Header;
// 	printf("write Header %d\n", unit->Header);
	unit->StepOver = 0; /*箕粗柒に姜位しなかったコマンドの眶をカウントするカウンタをクリアする*/
	unit->ResTime = 0; /*レスポンスタイムをクリアする。これに眶机が掐ったら悸乖窗位で冯蔡が鼎铜メモリに掐っている。*/

	return 0;
}

/*矢机误の黎片からセパレ〖タまでの矢机误をintに恃垂する*/
int getint(char* buf, int *x)
{
	char c;
	int i = 0;
	char s[83] = {0};
	char* chk = NULL;
	int n = 0;

//	printf("%s\n", buf);
	while((chk == NULL) || (i == 0)) {
		c = buf[n];
		if(c == '\0' || c == EOF) break;
		if((chk = strchr(SEPARATOR, c)) == NULL) {
//			printf("%c", c);
			s[i] = c;
			i++;
		}
		n++;
	}
	if(i > 0) {
		*x = atoi(s);
//		printf("int %d, buf+%d\n", *x, n);
	}

	return n;
}

/*鼎铜メモリの柒推を粕み叫す*/
int read_sm(DUNIT *unit)
{
	int i, j;

	/*肌の悸乖箕粗までの箕粗粗持*/
	printf("Step = %d\n", unit->Step);
	/*コマンドの房を回年する(办忍コマンドor年房コマンド)*/
	printf("Header = %d\n", unit->Header);

	switch(unit->Header) {
	case 1:
	case 2:
		/*年房コマンド¨链てのモ〖タとセンサに回吾を流る*/
		/*称モ〖タへ流られた回吾猛*/
		printf("MtrS = ");
		for(i = 0; i < MotorMax; i++) {
			printf("%d ", (short)unit->Ctl.MtrS[i]);
		}
		printf("\n");
		/*称センサへ流られた回吾猛*/
		printf("SnsS = ");
		for(i = 0; i < SensorMax; i++) {
			printf("%c ", (char)unit->Ctl.SnsS[i]);
		}
		printf("\n");
		/*提り猛¨称モ〖タのポジションデ〖タ*/
		/*惧の回吾流慨窗位していれば冯蔡が掐っている*/
		printf("MtrRp = ");
		for(i = 0; i < MotorMax; i++) {
			printf("%d ", (short)unit->Ctl.MtrRp[i]);
		}
		printf("\n");
		/*提り猛¨称モ〖タの庐刨デ〖タ*/
		/*惧の回吾流慨窗位していれば冯蔡が掐っている*/
		printf("MtrRv = ");
		for(i = 0; i < MotorMax; i++) {
			printf("%d ", (short)unit->Ctl.MtrRv[i]);
		}
		printf("\n");
		/*提り猛¨称モ〖タの庐刨デ〖タ*/
		/*惧の回吾流慨窗位していれば冯蔡が掐っている*/
		printf("MtrRi = ");
		for(i = 0; i < MotorMax; i++) {
			printf("%d ", (short)unit->Ctl.MtrRi[i]);
		}
		printf("\n");
		/*提り猛¨称センサのデ〖タ*/
		/*惧の回吾流慨窗位していれば冯蔡が掐っている*/
		printf("SnsRB = ");
		for(i = 0; i < SensorMax; i++) {
			for(j = 0; j < SnsResMax; j++) {
				printf("%x ", unit->Ctl.SnsRB[i][j]);
			}
			printf("\n");
		}
		printf("ResTime = %d\n", unit->ResTime);

		printf("\n");
		break;

	case 3: /*办忍コマンド*/
		/*コマンドを流ったデバイスID*/
		printf("Cmd.DevID = %hd\n", unit->Cmd.DevID);
		/*コマンド*/
		printf("Cmd.CmdAsc = %c\n", unit->Cmd.CmdAsc);

		switch(unit->Cmd.CmdAsc) {
		/*コマンドによって提り猛が佰なる*/
		case 'R': 
			/*6改のushort*/
			printf("ResW = ");
			for(i = 0; i < 6; i++) {
				printf("%hd ", (short)unit->Cmd.ResW[i]);
			}
			printf("ResB = %hd\n", unit->Cmd.ResB[12]);
			break;	
				
		case 'S':
			/*办改のuchar*/
			printf("ArgB = %hd\n", unit->Cmd.ArgB[0]);
			break;

		case 'X':
			/*6改のuchar*/
			printf("ResB = ");
			for(i = 0; i < 6; i++) {
				printf("%hd ", unit->Cmd.ResB[i]);
			}
			printf("\n");
			break;	

		case 'A': 
		case 'B':
		case 'C':
		case 'D':
		case 'E':
		case 'F':
		case 'L':
		case 'H':
			/*办改のushort*/
			printf("ArgW = %hd\n", (short)unit->Cmd.ArgW[0]);
			break;
		
		case 'N':
			/*办改のushort*/
			printf("ArgW = %hd, %hd\n", (short)unit->Cmd.ArgW[0], (short)unit->Cmd.ArgW[1]);
			break;
	
		case 'a':
		case 'd':
			/*3改のushort*/
			printf("ResW = ");
			for(i = 0; i < 3; i++) {
				printf("%hd ", (short)unit->Cmd.ResW[i]);
			}
			printf("\n");
			break;	
			
		case 'e':
		case 'h':
		case 'k':
			/*3改のushortと1改のuchar*/
			printf("ResW = ");
			for(i = 0; i < 3; i++) {
				printf("%hd ", (short)unit->Cmd.ResW[i]);
			}
			printf("\n");
			printf("ArgW = %hd\n", (short)unit->Cmd.ArgW[0]);
			break;	
			
		case 'n':
			/*1改のushort*/
			printf("ResW = %hu\n", unit->Cmd.ResW[0]);
			break;	

		default:
			break;	
		}
		/*箕粗柒に借妄が姜らず觅れが叫たコマンドの改眶*/
		printf("StepOver = %d\n", unit->StepOver);
		/*レスポンスタイム*/
		printf("ResTime = %d\n", unit->ResTime);
	}

	return 0;
}