当前位置：编程学习 > C#/ASP.NET >>

求救!请大侠们帮忙解释机器人搬运动作仿真程序的意思,不然毕业不了

1.2  C#编程源码

.h文件
#include "math.h"
#include "stdio.h"
//#include "math.h" 
#define M1 9.6
#define M2 14.4
#define M3 28.5
#define M4 7.5
#define M5 5
#define L1 463
#define L2 450
#define L3 787
#define L4 700
#define Lg1 180
#define Lg2  252
#define Lg3 311
#define Lg4 320
#define A 60
#define B 200 
#define G 9.8
#define PAI 3.1415926
#define RAD  (PAI/180)
#define TRUE 1
#define FALSE 0
#define DELTA_D  0.01
#define DD  0  /*initial robot */
#define SITA1 70//85
#define SITA2 150//95
#define SITA3 -150//90
#define SITA4 -90//-90

#define T01    100000   /*N.mm*/
#define _T01  -100000    /*N.mm*/
#define T02    200000   /*N.mm*/
#define _T02  -200000   /*N.mm*/
#define T03    250000   /*N.mm*/
#define _T03  -250000   /*N.mm*/
#define T04    150000   /*N.mm*/
#define _T04  -150000   /*N.mm*/

void GetTorRForce(double *d,double *T,double *RF);
int RForceCheck(double* RF,double LimValue);
void GetTorRatio(double *T,double *TR);
double GetHeadPos(double *d);
void GetWorkPos(double *d,double *wp);
void MakeOrder(double *data,double *d_Trmax ,int i);
int GetOriMin(double *d_Trmax ,int i);
int TorCheck(double* TR, double times);
double GetZmp(double *d);
void GetJointPos(double *d,double *gjpx,double *gjpy);


.cpp文件
#include "liftup1.h"
void main()
{
double d_d[82][4];
double d_RF[82][4],d_T[82][4],d_TRmax[82],d_TR[82][4],d_dd[82];
double T0[4],TR0[4],RF0[4];
double d[4];
double d01,d02,d03,d04,tr;  // 增加肩关节角度d04;
double dd,max;   
double zmp;
double JointPosX[4],JointPosY[4];
FILE *fp1,*fp2,*fp3,*fp4,*fp5;
int n,l,i,j,m,g,gi,num;  
int EndFlag,TumbFlag,OnChair;
if((fp1=fopen("RF.dat","wt"))==NULL)
printf("can not open file1");
if((fp2=fopen("Tor.dat","wt"))==NULL)
printf("can not open file2");
if((fp3=fopen("sita.dat","wt"))==NULL)
printf("can not open file3");
if((fp4=fopen("TR.dat","wt"))==NULL)
printf("can not open file4");
if((fp5=fopen("JointPos.dat","wt"))==NULL)
printf("can not open file4");

TumbFlag=0;
EndFlag=0;
num=0;
g=0;

d[0]=d01=(double)(SITA1*RAD);
d[1]=d02=(double)(SITA2*RAD);
d[2]=d03=(double)(SITA3*RAD);
d[3]=d04=(double)(SITA4*RAD); 

zmp=GetZmp(d);
   GetJointPos(d,JointPosX,JointPosY);
GetTorRForce(d,T0, RF0);
GetTorRatio(T0, TR0);
tr=TR0[1];
dd=GetHeadPos(d);

    printf("%d %f %f %f %f\n",num,d01/RAD,d02/RAD,d03/RAD,d04/RAD);
fprintf(fp1,"%d %f %f %f\n",num,RF0[0],RF0[1],zmp);
fprintf(fp2,"%d %f %f %f %f\n",num,T0[0],T0[1],T0[2],T0[3]);
fprintf(fp3,"%d %f %f %f %f\n",num,d01/RAD,d02/RAD,d03/RAD,d04/RAD);
fprintf(fp4,"%d %f %f %f %f\n",num,TR0[0],TR0[1],TR0[2],TR0[3]);  
fprintf(fp5,"%d%f%f%f%f%f%f%f%f\n",num,JointPosX[0],JointPosX[1],JointPosX[2],JointPosX[3],JointPosY[0],JointPosY[1],JointPosY[2],JointPosY[3]);

do{
for(i=0;i<82;i++){
d_TRmax[i]= 1000000000;
}
i=0;
gi=0;
TumbFlag=0;

if(OnChair==TRUE && d[0]==d01 && d[1]==d02){
do{
d[0]=d01;
d[1]=d02;
d[2]= d03 - (double)DELTA_D;
//printf("%d %f\n",num,d[2]/RAD);
zmp=GetZmp(d);
GetTorRForce(d, T0, RF0);
GetTorRatio(T0, TR0);
dd = GetHeadPos(d);
GetJointPos(d,JointPosX,JointPosY);
d03=d[2];

num++;
//printf("%d %f\n",num,zmp);
fprintf(fp1,"%d %f %f %f\n",num,RF0[0],RF0[1],zmp);
fprintf(fp2,"%d %f %f %f %f\n",num,T0[0],T0[1],T0[2],T0[3]);
   //printf("%f %f %f %f\n",d_T[g][1],d_T[g][2],d_T[g][3],d_T[g][4]);
fprintf(fp3,"%d%f%f%f%f\n",num,d[0]/RAD,d[1]/RAD,d[2]/RAD,d[3]/RAD);
//printf("%f %f %f %f %f\n",d_d[i][0],d_d[i][1],d_d[i][2],d_d[i][3],d_d[i][4]);
fprintf(fp4,"%d %f %f %f %f\n",num,TR0[0],TR0[1],TR0[2],TR0[3]);
fprintf(fp5,"%d%f%f%f%f%f%f%f%f\n",num,JointPosX[0],JointPosX[1],JointPosX[2],JointPosX[3],JointPosY[0],JointPosY[1],JointPosY[2],JointPosY[3]);
}while((zmp>B||zmp<-A) && d03>-PAI);
OnChair=FALSE;
}

for(n=-1;n<=1;n++){
 d_d[i][0] = d01 + n*(double)DELTA_D;
 for(l=-1;l<=1;l++){
d_d[i][1] = d02 + l*(double)DELTA_D;
for(j=-1;j<=1;j++){
 d_d[i][2] = d03 + j*(double)DELTA_D;
 for(m=-1;m<=1;m++){
 d_d[i][3] = d04 + m*(double)DELTA_D; 

  GetTorRForce(d_d[i], d_T[i], d_RF[i]);
  GetTorRatio(d_T[i], d_TR[i]);
  d_dd[i] = GetHeadPos(d_d[i]);

if(TorCheck(d_TR[i],1.0) && RForceCheck(d_RF[i],0.0)){ 
TumbFlag=1;
if(d_dd[i]>dd){  
max = (double)fabs(d_TR[i][0]);
if(fabs(d_TR[i][1])>=max) max =(double) fabs(d_TR[i][1]);
if(fabs(d_TR[i][2])>=max) max =(double) fabs(d_TR[i][2]);
                 if(fabs(d_TR[i][3])>=max) max =(double) fabs(d_TR[i][3]);
          // 得出四个关节中最大的力矩比率并存储到d_TRmax[i]中;
d_TRmax[i] = max;
i++;
d_d[i][0]=d_d[i-1][0];
d_d[i][1]=d_d[i-1][1];
d_d[i][2]=d_d[i-1][2];
d_d[i][3]=d_d[i-1][3];
}
}
}
}
}
}

if(TumbFlag==1&&i!=0){
g=GetOriMin(d_TRmax,i);
if ( d01==d_d[g][0] && d02==d_d[g][1] && d03==d_d[g][2]){
EndFlag = TRUE; /*角度没有变化，说明已经站起，结束*/
}
d01 = d_d[g][0];
d02 = d_d[g][1];
d03 = d_d[g][2];
    d04 = d_d[g][3];
 dd = d_dd[g];
 tr = d_TR[g][0];

       GetJointPos(d_d[g],JointPosX,JointPosY);
zmp=GetZmp(d_d[g]);
num++;

fprintf(fp1,"%d %f %f %f\n",num,d_RF[g][0],d_RF[g][1],zmp);
fprintf(fp2,"%d %f %f %f %f\n",num,d_T[g][0],d_T[g][1],d_T[g][2],d_T[g][3]);
fprintf(fp3,"%d%f%f%f%f\n",num,d_d[g][0]/RAD,d_d[g][1]/RAD,d_d[g][2]/RAD,d_d[g][3]/RAD);
fprintf(fp4,"%d %f %f %f %f\n",num,d_TR[g][0],d_TR[g][1],d_TR[g][2],d_TR[g][3]);
   fprintf(fp5,"%d%f%f%f%f%f%f%f%f\n",num,JointPosX[0],JointPosX[1],JointPosX[2],
JointPosX[3],JointPosY[0],JointPosY[1],JointPosY[2],JointPosY[3]);
}

   if(TumbFlag==1 && i==0){//不翻倒,肩不上升,说明已经站起来
EndFlag = TRUE;
}

if(TumbFlag==0  ){/*翻倒或力矩超标，结束*/
EndFlag = TRUE;
}

if (d_d[0][2]<-PAI)
EndFlag = TRUE;
if(fabs((L1+L2+L3)-dd)<=0.05)//已站起来
EndFlag = TRUE;

} while (EndFlag == FALSE);
fclose(fp1);
fclose(fp2);
fclose(fp3);
fclose(fp4);
  }

void GetTorRForce(double *d,double *T,double *RF)
{
double  d1=d[0], d2=d[1], d3=d[2],d4=d[3];       
    T[3]=(M4*Lg4+M5*L4)*G*cos(d1+d2+d3+d4);
T[2]=((M4+M5)*L3+M3*Lg3)*G*cos(d1+d2+d3)+T[3];
T[1]=((M3+M4+M5)*L2+M2*Lg2)*G*cos(d1+d2)+T[2];
T[0]=((M2+M3+M4+M5)*L1+M1*Lg1)*G*cos(d1)+T[1];
RF[0]=(-T[0]+B*(M1+M2+M3+M4+M5)*G)/(A+B);       
RF[1]=(T[0]+A*(M1+M2+M3+M4+M5)*G)/(A+B);
}


void GetTorRatio(double* T, double* TR)
{
if (T[0] == fabs(T[0]))
TR[0] = T[0]/T01*100.0;
else
TR[0] = T[0]/_T01*100.0;

if (T[1] == fabs(T[1]))
TR[1] = T[1]/T02*100.0;
else
TR[1] = T[1]/_T02*100.0;

if (T[2] == fabs(T[2]))
TR[2] = T[2]/T03*100.0;
else
TR[2] = T[2]/_T03*100.0;

if (T[3] == fabs(T[3]))
TR[3] = T[3]/T04*100.0;
else
TR[3] = T[3]/_T04*100.0;   
}

double GetHeadPos(double *d)
{
double hp;
hp= L1*sin(d[0])
   + L2*sin(d[0]+d[1])
   + L3*sin(d[0]+d[1]+d[2]);
return hp;
}

int GetOriMin(double *d_Trmax ,int i)
{
int k,s;
double min;
min=10000000000;
for (k=0;k<=i-1;k++){
if (fabs(d_Trmax[k])<=min){
min=fabs(d_Trmax[k]);
s=k;
}
}
return s;
}

int TorCheck(double* TR, double times)
{
if (fabs(TR[0])<=100.0*times && fabs(TR[1])<=100.0*times&&fabs(TR[2])<=100.0*times && fabs(TR[3])<=100.0*times)
return TRUE;      
else return FALSE;
}
int RForceCheck(double*RF,double LimValue)
{
if(RF[0]>=LimValue && RF[1]>=LimValue)
return TRUE;
else return FALSE;
}

double GetZmp(double *d)       
{
double x,x1,x2,x3,x4,x5;
double  d1=d[0], d2=d[1], d3=d[2], d4=d[3];
x1=Lg1*cos(d1);
x2=L1*cos(d1)+Lg2*cos(d1+d2);
x3=L1*cos(d1)+L2*cos(d1+d2)+Lg3*cos(d1+d2+d3);
x4=L1*cos(d1)+L2*cos(d1+d2)+L3*cos(d1+d2+d3)+Lg4*cos(d1+d2+d3+d4);
x5=L1*cos(d1)+L2*cos(d1+d2)+L3*cos(d1+d2+d3)+L4*cos(d1+d2+d3+d4);
x=(M1*x1+M2*x2+M3*x3+M4*x4+M5*x5)/(M1+M2+M3+M4+M5);
return x;
}

void GetJointPos(double *d,double *gjpx,double *gjpy)    
{
double  d0=d[0],d1=d[1], d2=d[2], d3=d[3];
    gjpx[0]=L1*cos(d0);
gjpx[1]=L1*cos(d0)+L2*cos(d0+d1);
gjpx[2]=L1*cos(d0)+L2*cos(d0+d1)+L3*cos(d0+d1+d2);
gjpx[3]=L1*cos(d0)+L2*cos(d0+d1)+L3*cos(d0+d1+d2)+L4*cos(d0+d1+d2+d3);
gjpy[0]=L1*sin(d0);
gjpy[1]=L1*sin(d0)+L2*sin(d0+d1);
gjpy[2]=L1*sin(d0)+L2*sin(d0+d1)+L3*sin(d0+d1+d2);
gjpy[3]=L1*sin(d0)+L2*sin(d0+d1)+L3*sin(d0+d1+d2)+L4*sin(d0+d1+d2+d3);
}

--------------------编程问答-------------------- 不是有注释了吗 --------------------编程问答-------------------- 我不是学电脑专业，所以看不明白，请高手帮忙解释一下什么意思？谢谢！！！ --------------------编程问答-------------------- 到C++板块 --------------------编程问答-------------------- 你这个是C++ 的什么跑C#里发贴了呢？去C++版吧！
补充：.NET技术 ,  C#

上一个：看法
下一个：asp的form中批量提交与修改数据

更多图片编程知识:
更多C#/ASP.NET疑问解答：
C# 数据采集
jqGrid
学C#编程一定要学编译原理吗？（新手提问）
VC2010 Unicode字符集下通过ReadProcessMemory跨进程读取SysListView32控件总是乱码
C#源文件恢复。电脑死机了
asp生成动画，求各位给个技术建议，或者大致方法。。。。
C#中 如何把excel的数据每一一列的值，赋值给一个一位数组呢
关于多线程同步问题.
怎么自动登录网站并填值返回。
怎么做程序小游戏
C#调用C++dll函数出现“尝试读取或写入受保护的内存”
wcf服务更新某业务逻辑需要关闭整个wcf服务吗
数据同步
课程继承要怎么讲
sql 语句合并的问题
asp
php
Delphi
Matlab
JSP
Foxpro
JS
C/C++
C#/ASP.NET
VC++
JAVA
VB
汇编语言
html/css
CGI
XML/UML
wap
网站相关
网页素材
python
微信小程序
thinkphp
如果你遇到编程学习难题：
访问www.zzzyk.com 试试
CopyRight © 2022 站长资源库 编程知识问答 zzzyk.com All Rights Reserved
部分文章来自网络，