define.h

日本V01机器人运动学算法

/********************************/
/* WFMC Class and Fun. Define	*/
/*		Author:	Qiu Tao			*/
/*      Date: 1999-10-26		*/
/********************************/

#include <stdio.h>

#ifndef	RWPDATA
	#define RWPDATA	255
#endif

#ifdef __cplusplus
	#define __CPPARGS ...
#else
	#define __CPPARGS
#endif

// set memory space
#define GetSaveMemory(x)  ((x*)farmalloc(sizeof(x)))
#define AllocMemory(t,h) { h=GetSaveMemory(t); h->Next=NULL; }
#define FreeAllMemory(t,h) { t *p; for(p=h;h->Next!=NULL;p=h){h=p->Next; farfree(p);} farfree(p); }

/* class pre_define */
class JointIO;
class JointSave;
class MJoint;
class PJoint;
class TransMatrix;
class ControlMode;
class NetMode;
class EditMode;
class ErrMessage;

struct movement;

/* joint I/O */
class JointIO
{
public:
	double ioJoint[9];
	JointIO operator=(double);
	JointIO operator=(JointIO);
	JointIO operator+(JointIO);
};

/* joint value save */
class JointSave
{
public:
	char  jsStatus[4];	// control status
	float jsJoint[9];	// joints' value
	float jsMTS;		// manipulator's running time or speed
	float jsPTS;		// positioner's running time or speed
	JointSave *Next;
	JointSave() { Next = NULL; }
};

/* RWP data save */
class RWPData
{
public:
	char rwpDataSegment[RWPDATA];
	RWPData *Next;
	RWPData() {	Next = NULL; }
};

/* manipulator joint value */
class MJoint
{
public:
	double mJoint[6];
	MJoint operator=(MJoint);
	MJoint operator+(MJoint);
	MJoint operator-(MJoint);
	MJoint operator*(int);
};

/* positioner joint value */
class PJoint
{
public:
	double pJoint[3];
	PJoint operator=(PJoint);
	PJoint operator+(PJoint);
	PJoint operator-(PJoint);
	PJoint operator*(double);
};

/* matrix transform */
class TransMatrix
{
public:
	double tmData[3][4];
	TransMatrix operator=(TransMatrix);
	TransMatrix operator+(TransMatrix);
	TransMatrix operator-(TransMatrix);
	TransMatrix operator*(TransMatrix);
	PJoint operator*(PJoint);
};

/* pendant control */
struct ControlMode
{
	int  RorP;
	int  Speed;		// speed setting
	char JWT;		// world selecting
	int  Fun;		// joint No.
	int  IorD;		// increase or decrease
	int  ESC;		// cancel
	int  REC;		// record teaching point
	int  Save;		// save teaching point
	int  MovetoPt;	// move to next point
	int  StepFord;	// move to former point as setting mode
	int  StepBack;	// move to next point as setting mode
	int  PriorPoint;// get former point
	int  NextPoint;	// get next point
	int  PointFlag;
	int  PointExist;
	int  Run;		// continus run
	int  Stop;		// intervening stop
	int  PTP;		// point to point
	int  CP;		// continue path
	int  LINE;		// line mode
	int  ARC;		// arc(circle) mode
	int  Sen1;		// sensor_1 startup
	int  Sen2;		// sensor_2 startup
	int  Utility;	// expended funtion
	int  SysInf;	// system status flag
	int  Back;
	int  Ford;
	int  Enter;		// confirm enter flag
	int  SavedFlag;	// save teaching point flag
	int  JobNo;		// welding file load flag
	int  LCd;		// line coordination flag
	int  CCd;		// circle coordination flag
	int  OTC;		// one teaching point circle coordination
};

/* net control mode */
struct NetMode
{
	int FileLoad;		//RWP data file download
	int SysRun;			//robot systen begin running
	int StatusCheck;	//check systen running status and send them to CMC
	int Interrupt;		//system emergency stop
	int Upload;			//upload teaching data
	int ESC;			//exit network running and control mode
	int GuideJoint;		//send joint-changing to CMC
	int GuideCMD;
};

/* pendant edit */
struct EditMode
{
	int  Back;
	int  Ford;
	int  JobNo;
	int  Modify;
	int  Ins;
	int  Del;
	int  Save;
	int  Reset;
};

/* error message */
struct ErrMessage
{
	int emJoint[9];
	int emJointStatus;
	int emSingular;
	int emSpeed;
	int emCollision;
};

/**** function definition ****/
void interrupt handler(__CPPARGS);
int	NetControl(const int &nRunMode);
int PendantControl(void);
void Beep(void);
int PendComPortInit(void);
void ErrorCodeDisplay(int ErrCode);
unsigned char GetCmdFromPendant(void);
void SendCmdToPendant(const unsigned char &Cmd);
void PendantMoveMode(void);
JointIO SetupJointValue(void);
void ServoPortSelect(int Number);
void ServoComPortInit(void);
unsigned char ReadServoEncoderData(void);
double PulseToAngle(const int &JointNo);
void AngleToPulse(const JointIO &Jio);
void Init8253(void);
void Rest8253(void);
MJoint M_UnitChangeValue(const ControlMode &M,const int &AccSteps,const int &StepCnt,const int &flag);
TransMatrix M_PositiveTransform(const MJoint &JtValue);
MJoint M_InverseTransform(const MJoint &Jp,TransMatrix &Trans,ErrMessage &Err);
TransMatrix P_PositiveTransform(const PJoint &J);
PJoint P_InvertTransform(const PJoint &Jp,const TransMatrix &T,ErrMessage &Err);
TransMatrix DltaValueInWorld(const MJoint &Jt);
TransMatrix MatrixTransform(const TransMatrix &Tr,const TransMatrix &T0,const MJoint &Jt);
void SingularCheck(const MJoint &Jt,ErrMessage &Err);
int CheckJointPositionAndSpeed(const MJoint &JtP,MJoint &JtC,ErrMessage &Err,int Single);
void PendantMoveErrorDisplay(const ErrMessage &Err);
PJoint P_UnitChangeValue(const ControlMode &M,const int &AccSteps,const int &StepCnt,const int &flag);
int P_CheckJointPositionAndSpeed(const PJoint &J0p,PJoint &J,ErrMessage &Err, int single);
void CalculatePulseCounterParameters(void);
int PendantControlSingal(void);
void SystemStatusDisplay(const MJoint &JR, const PJoint &JP, const TransMatrix &TR, const TransMatrix &TP);
void SystemStatusFrame(const ControlMode &M);
int Teach_KeyProcess(ControlMode &Md);
unsigned char GetReplyFromPendant(void);
int SpeedChoice(ControlMode &M);
void SendASCIISpeed(const int &speed);
int Teach_GetFilename(char filename[]);
void SendSystemStatus(const MJoint &JR, const PJoint &JP, const TransMatrix &TR, const TransMatrix &TP,	const ControlMode &M);
double CartesianMoveTime(const TransMatrix &Ta,const TransMatrix &Tb,const double &speed);
unsigned long int M_MoveStep(const MJoint &Dlta_J, MJoint &Acc, double &time, int moveflag[], unsigned long int TaNum[], unsigned long int TNum[]);
MJoint M_JointIncreaseValue(const unsigned long int &RunNum,const int DirectFlag[],const MJoint &AccJoint,const unsigned long int TbNum[],const unsigned long int TNum[]);
int AbnormalStatusDisplay(const ErrMessage &Err);
int PendantStopCmd(void);
void M_AbnormalStop(MJoint &Jp,const JointIO &J_io);
void P_AbnormalStop(PJoint &Jp,const JointIO &J_io);
JointSave *GetTeachData(JointSave *Head,unsigned int k);
unsigned long int M_AccStepsAndTwoControlPoint(const TransMatrix &Ta,const TransMatrix &Tb,double &speed,TransMatrix &Ts,TransMatrix &Te);
TransMatrix Get_Acc(const TransMatrix &Dt,const unsigned long int &num);
TransMatrix Acc_W_B(const TransMatrix &Dt,const TransMatrix &A,const unsigned long int &run_n);
unsigned long int M_ConstantSpeedSteps(const TransMatrix &DltaT,const double &speed);
TransMatrix DriveMatrix(const TransMatrix &DltaT,const double &sita,const double &fi,const double &lamd);
void Get_A_V(const TransMatrix &Dt,const unsigned long int &num,TransMatrix &Acc,TransMatrix &V);
TransMatrix Acc_W_E(const TransMatrix &Dt,const TransMatrix &A,const TransMatrix &V,const unsigned long int &run_n);
TransMatrix T_invert(const TransMatrix &In);
void RobotLineAdjust(MJoint &Jp, JointSave *JV, double speed);
unsigned long int P_MoveStep(const PJoint &Dlta_J,PJoint &Acc,double &time, int moveflag[],unsigned long int TaNum[], unsigned long int TNum[]);
PJoint P_JointIncreaseValue(const unsigned long int &RunNum,const int DirectFlag[],const PJoint &Acc,const unsigned long int TbNum[],const unsigned long int TNum[]);
void PositionerPTPAdjust(PJoint &J_start,JointSave *JV,const double &speed);
char GetMotionFlag(JointSave *JointHead, unsigned int Num);
void RobotPTPAdjust_File(MJoint &J_start,JointSave *JV);
unsigned long int M_Acc_B_E(const TransMatrix &Ta,const TransMatrix &Tb,double &speed,TransMatrix &T,const int &Flag);
void RobotLineIntp_File(MJoint &J_start,PJoint &JP0,JointSave *JV);
MJoint ArcPlane(const TransMatrix &T1,const TransMatrix &T2,const TransMatrix &T3,double &radius);
TransMatrix ArcTrans(const MJoint &Arc,const TransMatrix &T1);
double AngleFlag(const TransMatrix &Tp1,const TransMatrix &Tp2,const TransMatrix &Tp3);
unsigned long int ArcAccNumber(const TransMatrix &Ta,const TransMatrix &Tb,const double &radius,double &speed,TransMatrix &T,double &angle,const double &flag,const int &mark);
TransMatrix Get_Acc(const TransMatrix &Dt,const unsigned long int &num,const double &angle);
TransMatrix Acc_W_B(const TransMatrix &Dt,const TransMatrix &A,const TransMatrix &T1,const double &radius,const unsigned long int &run_n,const double &Flag);
unsigned long int Arcsteps(const TransMatrix &Tp1,const TransMatrix &Tp2,const double &radius,const double &speed,double &angle);
void Pposition(TransMatrix &T,const double &radius,const double &lamd,const double &angle,const double &flag);
void Pposition(TransMatrix &T,const double &radius,const double &lamd,const double &angle0,const double &angle,const double &flag);
TransMatrix ArcPlaneDrive(const TransMatrix &DltaT,const double &sita,const double &fi,const double &lamd);
void Get_A_V(const TransMatrix &Dt,const unsigned long int &num,const double &angle,TransMatrix &Acc,TransMatrix &V);
TransMatrix Acc_W_E(const TransMatrix &Dt,const TransMatrix &A,const TransMatrix &V,const TransMatrix &Tacc,const double &radius,const double &angle,const unsigned long int &run_n,const double &Flag);
void AngleChange(const TransMatrix &DltaT,double &sita,double &fi);
void RobotPTPAdjust_Pendant(MJoint &J_start,JointSave *JV,const double &speed);
void RobotLineIntp_Pendant(MJoint &J_start,PJoint &JP0,JointSave *JV,const double &speed);
void RobotArcIntp_Pendant(MJoint &Jp,PJoint &JP0,JointSave *JV1,JointSave *JV2,const double &speed,const int &nEndFlag);
void RobotArcIntp_File(MJoint &Jp,PJoint &Pp,JointSave *JV1,JointSave *JV2,const int &nEndFlag);
void PointMemSave(JointSave *JV,const MJoint &JM,const PJoint &JP,ControlMode &Mode,unsigned int &DataPointNum);
int Pendant_GetFileName(char filename[]);
int GetFilePointData(char filename[],JointSave *JV);
int GetFilePointData(char filename[],JointSave *JV,unsigned int &DataPointNum);
void PointDiskSave(JointSave *JV, const unsigned int &DataPPointNum);
void SendMotionComToRobot(void);
void FreeMemory(JointSave *JV);
int PendantAccStep(const ControlMode &M);
PJoint PtrCircleCoordinateMove(PJoint &PJ1,PJoint &PJ2,const unsigned long int &Num,const int Flag[]);
TransMatrix SeamValueInWorkpiece(const unsigned long int &RunNum,const unsigned long int &Num,const double &angle,const double &radius,const double Flag);
TransMatrix PGAssemble(TransMatrix &TG,TransMatrix &TP);
void CircleCoordinationInterp(MJoint &JM,PJoint &JP,JointSave *JV1,JointSave *JV2,int Flag);
void MovingDirection(const PJoint &PJ1,const PJoint &PJ2,int Flag[]);
void SystemPTPAdjustment_File(MJoint &JM_start,PJoint &JP_start,JointSave *JV);
void SystemPTPAdjustment_Pendant(MJoint &JM_start,PJoint &JP_start,JointSave *JV,const double &speed);
void Sys_AbnormalStop(MJoint &JMp,PJoint &JPp,const JointIO &Jio);
TransMatrix SeamValueInClipper(const TransMatrix &T1,const TransMatrix &T2,const unsigned long int &RunNum,const unsigned long int &Num);
void LineCoordinationInterp(MJoint &JM0,PJoint &JP0,JointSave *JV);
int CollisionDetecting(void);
void CircleCoordination_Full(MJoint &JM0,PJoint &JP0,const double &speed);
MJoint ClipperCrossLineFunction(PJoint &JP);
PJoint GetTeachCircleCenter(const MJoint &DV,const TransMatrix &TeachPoint,double &radius);
TransMatrix WorkpieceFrame(PJoint &CircleCenter,TransMatrix &T1);
unsigned long int CircleSteps(const double &radius,const double &speed);
void CircleMoveDirection(const PJoint &PJ,int Flag[]);
PJoint CircleCoordinateMove(const unsigned long int &Num,const int Flag[]);
TransMatrix SeamValueInWorkpiece(const unsigned long int &RunNum,const unsigned long int &Num,const double &radius,const int Flag[]);
void CircleCoordination_Full(MJoint &JM0,PJoint &JP0,const double &speed);
int NetControl(void);
int SerialPortInit_RS422(void);
void interrupt RS422_Hander_Single(void);
void interrupt RS422_Hander_Multiple(void);
void interrupt COM_INT_Hander(void);
int RS422_IRQ_Setting(int PortNo, int Counter);
void RS422_Transmit(int PortNo, char *data, int num);
int Net_KeyProcess(NetMode &Md);
int GetRWPData(void);
void SystemStatusFrame(void);
RWPData *RWPPackageMemSave(RWPData *RWPDataHead, RWPData *RWPDataCrnt, unsigned char *RecData);
int RWPDataDiskSave(RWPData *RWPDataHead);
char GetControlModeFlag(JointSave *JointHead, unsigned long int Num, int bit);
double PTPMoveTime(const MJoint &Ja,const MJoint &Jb,const double &speed);
unsigned char CMC_Response(void);
MJoint ArcStart_UnitChangeValue(int Direction);
void RS422_UnPackageZip(const char *Package, float *data);
void RS422_PackageZip(const float *data, char *Package, const int &length);
void SendRunningData(const MJoint &MJ, const PJoint &PJ);
void FreeRWPMemory(RWPData *JV);
unsigned long int M_In_De_Steps(const TransMatrix &DltaT,double &speed,double &LamdPert,int AccFlag);
int CMC_Response_Short(void);
int SendTeachDataToCMC(void);
int CMC_Response_Arc(void);
int CMC_Response_EmStop(void);
unsigned long int GetCurrentMillisecond(void);
unsigned char CMC_Response_Check(void);
RWPData *AllocNewItemRWP(void);
void MessageDisplay(char *msg);
void AbnormalStopDisplay(const int &msg);
double PTPMoveTimePositioner(const PJoint &Ja,const PJoint &Jb,const double &speed);
int NetControl_Guide(void);
void SendRunningData(const float &Delta);
int GetGuideData(float JointDelta[]);
int NetControl_Laser(void);
void LaserMove(MJoint &J_start, PJoint &JP0);
MJoint ToolFrameUnitChangeValue(int Axis, int Direct_Speed);
void RobotLineMove(MJoint &Jp, MJoint &Jd, PJoint &JP0, double speed);