test6-15.c

固高GT400运动控制卡

#include "userlib.h"
#include <math.h>
#include <stdio.h>

void initial(void);
void space_arc(double a,double b,double c,double x0,double y0,double z0,double r,double angle);
void error(short);
short rtn;

void initial()
{
  rtn=GT_Open();             error(rtn);
  rtn=GT_Reset();            error(rtn);
  rtn=GT_LmtSns(0);          error(rtn);
  rtn=GT_EncSns(0);          error(rtn);

  rtn=GT_Axis(1);            error(rtn);
  rtn=GT_ClrSts();           error(rtn);
  rtn=GT_SetKp(20);          error(rtn);
  rtn=GT_SetKi(2);           error(rtn);
  rtn=GT_SetKd(60);          error(rtn);
  rtn=GT_SetKvff(0);         error(rtn);
  rtn=GT_SetKaff(0);         error(rtn);
  rtn=GT_SetILmt(100);       error(rtn);
  rtn=GT_Update();           error(rtn);
  rtn=GT_AxisOn();           error(rtn);

  rtn=GT_Axis(2);            error(rtn);
  rtn=GT_ClrSts();           error(rtn);
  rtn=GT_SetKp(20);          error(rtn);
  rtn=GT_SetKi(2);           error(rtn);
  rtn=GT_SetKd(60);          error(rtn);
  rtn=GT_SetKvff(0);         error(rtn);
  rtn=GT_SetKaff(0);         error(rtn);
  rtn=GT_SetILmt(100);       error(rtn);
  rtn=GT_Update();           error(rtn);
  rtn=GT_AxisOn();           error(rtn);

  rtn=GT_Axis(3);            error(rtn);
  rtn=GT_ClrSts();           error(rtn);
  rtn=GT_SetKp(20);          error(rtn);
  rtn=GT_SetKi(2);           error(rtn);
  rtn=GT_SetKd(60);          error(rtn);
  rtn=GT_SetKvff(0);         error(rtn);
  rtn=GT_SetKaff(0);         error(rtn);
  rtn=GT_SetILmt(100);       error(rtn);
  rtn=GT_Update();           error(rtn);
  rtn=GT_AxisOn();           error(rtn);

 }

void space_arc(double a,double b,double c,double x0,double y0,double z0,double r,double angle)
{
   double m,n,k;
   double cnt1[5],cnt2[5],cnt3[5];
   k=2000;
   m=-atan(a/b);
   n=acos(c/(sqrt(pow(a,2)+pow(b,2)+pow(c,2))));
   cnt1[0]=k*cos(m);
   cnt1[1]=-k*sin(m)*cos(n);
   cnt1[2]=k*sin(m)*sin(n);
   cnt1[3]=k*0;
   cnt1[4]=k*x0;
   cnt2[0]=k*sin(m);
   cnt2[1]=k*cos(m)*cos(n);
   cnt2[2]=-k*cos(m)*sin(n);
   cnt2[3]=k*0;
   cnt2[4]=k*y0;
   cnt3[0]=k*0;
   cnt3[1]=k*sin(n);
   cnt3[2]=k*cos(n);
   cnt3[3]=k*0;
   cnt3[4]=k*z0;
   rtn=GT_MapAxis(1,cnt1);          error(rtn);
   rtn=GT_MapAxis(2,cnt2);          error(rtn);
   rtn=GT_MapAxis(3,cnt3);          error(rtn);
   rtn=GT_StrtList();               error(rtn);
   rtn=GT_MvXYZ(0,-r,0,2/k,0.5/k);  error(rtn);
   rtn=GT_ArcXY(0,0,angle);         error(rtn);
   rtn=GT_EndList();                error(rtn);
   rtn=GT_StrtMtn();                error(rtn);
}

void error(short rtn)
{
   switch (rtn)
   {
      case -1:
	   printf("error: communciation error\n");
	   break;
      case 0:
	   /* No error */
	   break;
      case 1:
	   printf("error: command error\n");
	   break;
      case 2:
	   printf("error: radius is zero or too large\n");
	   break;
      case 3:
	   printf("error: the length of line is zero or too large\n");
	   break;
      case 4:
	   printf("error: acceleration or velocity is zero or too large\n");
	   break;
      case 5:
	   printf("error: parameter conflict\n");
	   break;
      case 6:
	   printf("error: nonexistence solution for the equation\n");
	   break;
      case 7:
	   printf("error: process parameter error\n");
	   break;
      default:
	   break;
   }
}


void main()
{
  initial();
  space_arc(1,-1,0,10,10,30,50,360);
  rtn=GT_Close();               error(rtn);
}