stecx930.c

机械手（基于日立2398CPU）程序

/**********************************************************************/
/* System      : 俽俹梡僐儞僩儘乕儔 俽俿俤俠亅倃倃倃                  */
/* File Name   : STECX930.C                                           */
/* Description : 儊儞僥僣乕儖捠怣張棟                                 */
/*                                                                    */
/* ------------------------------------------------------------------ */
/*   Copyright(C) Eikosha Co.Ltd. 2002/07/01                          */
/* ------------------------------------------------------------------ */
/* History     : 2002/07/01 Created                                   */
/**********************************************************************/
#include	<machine.h>
#include	<string.h>
#include	"iodefine.h"
#include	"vect.h"
#include	"StecX.h"
#include	"E2Prom.h"
#include	"StecSeq.h"

static unsigned char	*rp, ans, sum, *sp;
static unsigned char	rbuff[256];
static unsigned char	sbuff[256];
static unsigned char	E2buff[256];			/* 俤俤俹俼俷俵彂崬僨乕僞僶僢僼傽 */
static unsigned char	MonNum[2];				/* 儌僯僞搊榐悢 */

extern unsigned char	FLASHTOP;				/* 俠俹倀撪憼俥亅俼俷俵愭摢傾僪儗僗 */
extern unsigned char	PDFRomData[128];		/* 儁儞僟儞僩奜晹F-ROM僨乕僞憲庴怣僄儕傾 */
extern unsigned char	PDFRomAnsw;				/* 儁儞僟儞僩奜晹F-ROM僨乕僞張棟墳摎 */
extern unsigned char	PDFRomKind;				/* 儁儞僟儞僩奜晹F-ROM僨乕僞庬暿 */

static unsigned short	rlen;
static unsigned short	slen;
static unsigned short	MonOff[2][60];			/* 儌僯僞搊榐僆僼僙僢僩 */

extern unsigned short	PDFRomLen;				/* 儁儞僟儞僩奜晹F-ROM懳徾挿偝 */
extern signed short		SVParam[55];			/* 僒乕儃僷儔儊乕僞憲庴怣梡僄儕傾 */

extern unsigned long	PDFRomAddr;				/* 儁儞僟儞僩奜晹F-ROM懳徾傾僪儗僗*/

extern struct _CONDCOMM	CondComm;				/* 抜庢姺儕僗僩昞帵梡 */

static enum {rsIDLE, rsLEN, rsDATA, rsSUM, rsREND, rsWRITE, rsTRAN} Ser1Stat;

void SendStart(unsigned short len);
void SVQueueCode(char Code);

#define	USEDMA	1

/*--------------------------------------------------------------------*
 * MODULE   : INT_ERI1_SCI1
 * ABSTRACT : 庴怣僨乕僞僄儔乕妱崬傒
 * FUNCTION : vector 84 ERI1 SCI1 庴怣僄儔乕妱崬傒
 *            僆乕僶乕儔儞丄僼儗乕儈儞僌丄僷儕僥傿僄儔乕帪偵奺價僢僩偺儕僙僢僩
 * NOTE     : 
 * RETURN   : void
 * CREATE   : 00.11.01 A.Murakami
 * UPDATE   : 
 *--------------------------------------------------------------------*/
void INT_ERI1_SCI1(void)
{
	if (SCI1.SSR.BIT.ORER)	/* 僆乕僶乕儔儞丒僄儔乕	*/
		SCI1.SSR.BIT.ORER = 0;
	if (SCI1.SSR.BIT.FER)	/* 僼儗乕儈儞僌僄儔乕		*/
		SCI1.SSR.BIT.FER = 0;
	if (SCI1.SSR.BIT.PER)	/* 僷儕僥傿僄儔乕		*/
		SCI1.SSR.BIT.PER = 0;
	Ser1Stat = rsIDLE;
}

/*--------------------------------------------------------------------*
 * MODULE   : INT_RXI1_SCI1
 * ABSTRACT : SCI1偺庴怣妱崬傒
 * FUNCTION : 僷僜僐儞偐傜偺僐儅儞僪庴怣
 * NOTE     : vector 85 RXI1 SCI1 庴怣僨乕僞僼儖妱崬傒
 * RETURN   : void
 * CREATE   : 00.11.01 A.Murakami
 * UPDATE   : 
 *--------------------------------------------------------------------*/
void INT_RXI1_SCI1(void)
{
	unsigned char	c;  
	
	

	SCI1.SSR.BIT.RDRF = 0;
	c = SCI1.RDR;	/* 庴怣僨乕僞 */
	switch (Ser1Stat){
		case rsIDLE:
			if (c == 0x02){	/* STX */
				Ser1Stat = rsLEN;
				rp = &rbuff[0];
				*(rp++) = c;
				rlen = 2;
			}
			break;
		case rsLEN:
			*(rp++) = c;
			if (--rlen == 0){
				rlen = *(rp - 2) * 0x100 + *(rp - 1);
				if (rlen<sizeof(rbuff))
					Ser1Stat = rsDATA;
				else
					Ser1Stat = rsIDLE;
				sum = 0;
			}
			break;
		case rsDATA:
			*(rp++) = c;
			sum += c;
			if (--rlen == 0){
				Ser1Stat = rsSUM;
			}
			break;
		case rsSUM:
			*(rp++) = c;
			if (sum == c)
				ans = 0;
			else
				ans = 0xf0;
			Ser1Stat = rsREND;
			break;
		default:
			Ser1Stat = rsIDLE;
			break;
	}
}

/*--------------------------------------------------------------------*
 * MODULE   : CheckSum
 * ABSTRACT : 僠僃僢僋僒儉寁嶼
 * FUNCTION : 
 * NOTE     : 
 * RETURN   : void
 * CREATE   : 00.11.01 A.Murakami
 * UPDATE   : 
 *--------------------------------------------------------------------*/
static unsigned char CheckSum(void)
{
	unsigned char	sum, *p;
	unsigned short	n;
	sum = 0;
	p = &sbuff[3];
	for (n = sbuff[1] * 0x100 + sbuff[2]; n ; n--, p++)
		sum += *p;
	return sum;
}

/*--------------------------------------------------------------------*
 * MODULE   : AnsErr
 * ABSTRACT : 僄儔乕僐乕僪墳摎
 * FUNCTION : 
 * NOTE     : 
 * RETURN   : void
 * CREATE   : 00.11.01 A.Murakami
 * UPDATE   : 
 *--------------------------------------------------------------------*/
static void AnsErr(unsigned char rc)
{
	sbuff[0] = 0x02;
	sbuff[1] = 0x0002 / 0x100;
	sbuff[2] = 0x0002 & 0x0FF;
	sbuff[3] = rbuff[3];
	sbuff[4] = rc;
	sbuff[5] = CheckSum();
	SendStart(6);
}

/*--------------------------------------------------------------------*
 * MODULE   : CPUDataRead
 * ABSTRACT : 俠俹倀僨乕僞撉弌偟
 * FUNCTION : 
 * NOTE     : 
 * RETURN   : void
 * CREATE   : 2003.04.02 S.Izumi
 * UPDATE   : 
 *--------------------------------------------------------------------*/
static void CPUDataRead(void)
{
	unsigned char	n, *s, *d;
	sbuff[0] = 0x02;
	sbuff[1] = ((rbuff[6] * 2) + 2) / 0x100;
	sbuff[2] = ((rbuff[6] * 2) + 2) & 0x0FF;
	sbuff[3] = rbuff[3];
	sbuff[4] = ans;
	d = &sbuff[5];
	s = (unsigned char *)&FLASHTOP + ((rbuff[4] * 0x100 + rbuff[5]) & 0x0000FFFF);
	for (n = rbuff[6] * 2; n; n--)
		*(d++) = *(s++);
	*(d++) = CheckSum();
	SendStart(d - &sbuff[0]);
}

/*--------------------------------------------------------------------*
 * MODULE   : SeqDataRead
 * ABSTRACT : 僔乕働儞僗僨僶僀僗僄儕傾撉弌偟
 * FUNCTION : 
 * NOTE     : 
 * RETURN   : void
 * CREATE   : 00.11.01 A.Murakami
 * UPDATE   : 
 *--------------------------------------------------------------------*/
static void SeqDataRead(void)
{
	unsigned char	n, *s, *d;
	sbuff[0] = 0x02;
	sbuff[1] = ((rbuff[6] * 2) + 2) / 0x100;
	sbuff[2] = ((rbuff[6] * 2) + 2) & 0x0FF;
	sbuff[3] = rbuff[3];
	sbuff[4] = ans;
	d = &sbuff[5];
	s = (unsigned char *)&SeqData + (rbuff[4] * 0x100 + rbuff[5]);
	for (n = rbuff[6] * 2; n; n--)
		*(d++) = *(s++);
	*(d++) = CheckSum();
	SendStart(d - &sbuff[0]);
}

/*--------------------------------------------------------------------*
 * MODULE   : CondDataRead
 * ABSTRACT : 抜庢姺僨乕僞撉弌偟
 * FUNCTION : 
 * NOTE     : 
 * RETURN   : void
 * CREATE   : 02.07.01 A.Murakami
 * UPDATE   : 
 *--------------------------------------------------------------------*/
static void CondDataRead(void)
{
	unsigned char	n, *d;
	sbuff[0] = 0x02;
	sbuff[3] = rbuff[3];
	sbuff[4] = ans;
	sbuff[5] = rbuff[4];
	sbuff[6] = rbuff[5];
	d = &sbuff[7];
	d += E2CondReadP(rbuff[4] & 0x1F, rbuff[5], d);
	sbuff[1] = (d - &sbuff[3]) / 0x100;
	sbuff[2] = (d - &sbuff[3]) & 0xFF;
	*(d++) = CheckSum();
	SendStart(d - &sbuff[0]);
}

/*--------------------------------------------------------------------*
 * MODULE   : SysPrmRead
 * ABSTRACT : 僔僗僥儉僷儔儊乕僞撉弌偟
 * FUNCTION : 
 * NOTE     : 
 * RETURN   : void
 * CREATE   : 2003.04.05 S.Izumi
 * UPDATE   : 
 *--------------------------------------------------------------------*/
static void SysPrmDataRead(void)
{
	unsigned char *d;
	sbuff[0] = 0x02;
	sbuff[3] = rbuff[3];
	sbuff[4] = ans;
	d = &sbuff[5];
	d += E2SysPrmReadP(d);
	sbuff[1] = (d - &sbuff[3]) / 0x100;
	sbuff[2] = (d - &sbuff[3]) & 0xFF;
	*(d++) = CheckSum();
	SendStart(d - &sbuff[0]);
}

/*--------------------------------------------------------------------*
 * MODULE   : SeqPrgRead
 * ABSTRACT : 僔乕働儞僗僾儘僌儔儉僄儕傾撉弌偟
 * FUNCTION : 
 * NOTE     : 
 * RETURN   : void
 * CREATE   : 02.07.01 A.Murakami
 * UPDATE   : 
 *--------------------------------------------------------------------*/
static void SeqPrgRead(void)
{
	unsigned short	n;
	unsigned char	*s, *d;

	n = rbuff[8] * 0x100+ rbuff[9];
	sbuff[0] = 0x02;
	sbuff[1] = (n+2) / 0x100;
	sbuff[2] = (n+2) & 0x0FF;
	sbuff[3] = rbuff[3];
	sbuff[4] = ans;
	d = &sbuff[5];
	s = (unsigned char *)&FLASHTOP + (unsigned long)(rbuff[5] * (unsigned long)0x10000 + rbuff[6] * (unsigned long)0x100 + rbuff[7]);
	for (; n; n--)
		*(d++) = *(s++);
	*(d++) = CheckSum();
	SendStart(d - &sbuff[0]);
}

/*--------------------------------------------------------------------*
 * MODULE   : SeqMonEntry
 * ABSTRACT : 儌僯僞懳徾搊榐
 * FUNCTION : 
 * NOTE     : 
 * RETURN   : void
 * CREATE   : 02.07.01 A.Murakami
 * UPDATE   : 
 *--------------------------------------------------------------------*/
static void SeqMonEntry(void)
{                     
	unsigned short	num;
	num = ((rbuff[1] * 0x100 + rbuff[2]) / 2) - 1;
	if (rbuff[4] < 2 && num <= 60){
		unsigned char	*s = &rbuff[5];
		unsigned short	*d = &MonOff[rbuff[4]][0];
		MonNum[rbuff[4]] = num;
		for (; num; num--, s+=2)
			*(d++) = *s * 0x100 + *(s + 1);
		AnsErr(0x00);
	}
	else
		AnsErr(0x04);	/* 僨乕僞晄惓(斖埻奜)*/
}

/*--------------------------------------------------------------------*
 * MODULE   : SeqMonRead
 * ABSTRACT : 儌僯僞搊榐僨乕僞撉弌偟
 * FUNCTION : 
 * NOTE     : 
 * RETURN   : void
 * CREATE   : 02.07.01 A.Murakami
 * UPDATE   : 
 *--------------------------------------------------------------------*/
static void SeqMonRead(void)
{
	if (rbuff[4] < 2){
		unsigned char	n, num = MonNum[rbuff[4]];
		unsigned char	*d;
		unsigned short	*off, monData;

		sbuff[0] = 0x02;
		sbuff[1] = (num * 2 + 2) / 0x100;
		sbuff[2] = (num * 2 + 2) & 0xff;
		sbuff[3] = rbuff[3];
		sbuff[4] = 0x00;
		off = &MonOff[rbuff[4]][0];
		d = &sbuff[5];
		for(n=0; n<num; n++){
			monData = *((unsigned short *)((unsigned char *)&SeqData + *off)); 
			*(d++) = monData >> 8;
			*(d++) = monData & 0xFF;
			off++;
		}
		*(d++) = CheckSum();
		SendStart((unsigned char *)d-&sbuff[0]);
	}
	else
		AnsErr(0x04);	/* 僨乕僞晄惓(斖埻奜)*/
}

/*--------------------------------------------------------------------*
 * MODULE   : SvParamSend
 * ABSTRACT : 僒乕儃僷儔儊乕僞撉弌偟僨乕僞憲怣
 * FUNCTION : 
 * NOTE     : 
 * RETURN   : void
 * CREATE   : 02.07.01 A.Murakami
 * UPDATE   : 
 *--------------------------------------------------------------------*/
static void SvParamSend(void)
{
	unsigned char	n;
	unsigned char	*d, *s;
	sbuff[0] = 0x02;
	sbuff[1] = (55 * 2 + 2) / 0x100;
	sbuff[2] = (55 * 2 + 2) & 0xFF;
	sbuff[3] = 'Q';
	sbuff[4] = 0x01;
	s = (unsigned char *)&SVParam[0];
	d = &sbuff[5];
	for(n = 55 * 2; n; n--)
		*(d++) = *(s++);
	*(d++) = CheckSum();
	SendStart((unsigned char *)d-&sbuff[0]);

}

/*--------------------------------------------------------------------*
 * MODULE   : PDFRomSend
 * ABSTRACT : 儁儞僟儞僩奜晹F-ROM撉弌偟僨乕僞憲怣
 * FUNCTION : 
 * NOTE     : 
 * RETURN   : void
 * CREATE   : 02.07.01 A.Murakami
 * UPDATE   : 
 *--------------------------------------------------------------------*/
static void PDFRomSend(void)
{
	unsigned short	n;
	unsigned char	*d, *s;
	sbuff[0] = 0x02;
	sbuff[1] = (PDFRomLen + 2) / 0x100;
	sbuff[2] = (PDFRomLen + 2) & 0xFF;
	sbuff[3] = 'Y';
	sbuff[4] = PDFRomAnsw;
	s = &PDFRomData[0];
	d = &sbuff[5];
	for(n = PDFRomLen; n; n--)
		*(d++) = *(s++);
	*(d++) = CheckSum();
	SendStart((unsigned char *)d - &sbuff[0]);

}

/*--------------------------------------------------------------------*
 * MODULE   : AnsStatus
 * ABSTRACT : 僔乕働儞僗幚峴忬懺墳摎
 * FUNCTION : 
 * NOTE     : 
 * RETURN   : void
 * CREATE   : 02.10.25 A.Murakami
 * UPDATE   : 
 *--------------------------------------------------------------------*/
static void AnsStatus(void)
{
	sbuff[0] = 0x02;
	sbuff[1] = 0x0003/0x100;
	sbuff[2] = 0x0003&0x0FF;
	sbuff[3] = rbuff[3];
	sbuff[4] = 0;
	sbuff[5] = SysInf.Flag.bSeqEnable ? 0x01 : 0x00;
	sbuff[6] = CheckSum();
	SendStart(7);
}

static void SeqPrgErase(void);
static void SeqPrgWrite(void);

/*--------------------------------------------------------------------*
 * MODULE   : ComAns
 * ABSTRACT : 僷僜僐儞偐傜偺巜帵墳摎張棟
 * FUNCTION : 
 * NOTE     : 
 * RETURN   : void
 * CREATE   : 02.07.01 A.Murakami
 * UPDATE   : 
 *--------------------------------------------------------------------*/
void ComAns(void)
{
	static unsigned char	WriteStep;
	static unsigned short	datalen;

	if (Ser1Stat == rsREND){
		if (ans == 0){
			switch(rbuff[3]){
				case 'A':	/* 俠俹倀僨乕僞撉弌偟僐儅儞僪 */
					Ser1Stat = rsIDLE;
					CPUDataRead();
					break;
				case 'D':	/* 僔乕働儞僗僨僶僀僗撉弌偟僐儅儞僪 */
					Ser1Stat = rsIDLE;
					SeqDataRead();
					break;
				case 'M':	/* 儌僯僞僨乕僞搊榐 */
					Ser1Stat = rsIDLE;
					SeqMonEntry();
					break;
				case 'N':	/* 搊榐僨乕僞儌僯僞 */
					Ser1Stat = rsIDLE;
					SeqMonRead();
					break;
				case 'C':	/* 抜庢姺忣曬徚嫀僐儅儞僪	*/
					if (SysInf.Flag.bSeqRun == ON || rbuff[4] > 30){
						AnsErr(0x01);		/* 彂姺晄壜 */
						Ser1Stat = rsIDLE;
					}
					else{
						WriteStep = 1;		/* 徚嫀張棟 */
						Ser1Stat = rsWRITE;
					}
					break;
				case 'R':	/* 抜庢姺忣曬撉弌偟僐儅儞僪	*/
					CondDataRead();
					Ser1Stat = rsIDLE;