这两天的作品了，一个小算法，耽误了两天GUI 的功夫，VC6.0写的

5498折戟沉沙 · 发表于 2013-1-22 20:15:08

#include "stdio.h"
#include "math.h"

#define u16 unsigned short int
#define s16 signed short int
#define u32 unsigned int
#define s32 int

/***************************************************
描述：此程序用于具有三自由度的胳膊，有两个可供调用的函数，分别适合左右两臂

原理：结合三自由度的特点，利用空间几何的关系，同时参考人胳膊完成动作时各关节的运动状态，来模仿胳膊的运动。
由于自由度较少，所以部分动作，可能相对比较难以理解，但是符合人们的习惯。

作者： 2013.1.22

参考动作：双臂平伸向前方，x轴正向，以下参数为根据初始条件确定
左臂：1000 -200 0
右臂：1000 200 0
****************************************************/

typedef struct{
s32 x; //x轴方向坐标,指向前方为正
s32 y; //y轴方向坐标,指向右方为正
s32 z; //z轴方向坐标,指向上方为正
}ARMCOORDINATIONSTRUCT;

typedef struct{
u16 shoulder; //胳膊的肩膀长，在程序中应为0，以mm为单位
u16 boom; //胳膊的大臂长，以mm为单位
u16 forearm; //胳膊的小臂长，以mm为单位
}ARMLENGTHSTRUCT;
typedef struct{
s16 shoulder; //肩膀部位电机旋转的角度，立正状态为0，点击绕y轴旋转，沿y轴正向看时，顺时针旋转，角度增加
s16 boom; //大臂部位电机旋转的角度，立正状态为90，此时旋转方向垂直为X轴，沿x轴正向看时，顺时针方向旋转，角度减小
s16 forearm; //小臂部位电机旋转的角度，立正状态为180，此时旋转方向垂直为X轴，沿x轴正向看时，顺时针方向旋转，角度减小
}ARMANGLESTRUCT;

#define PI 3.1415926 //由弧度转为角度时的变量，三角函数返回值为弧度值，需转化为角度值
ARMANGLESTRUCT ArmAngleLeft={0,90,180},ArmAngleRight={0,90,180};//立正状态，左右两胳膊各电机的角度
ARMLENGTHSTRUCT ArmLengthLeft={0,500,500},ArmLengthRight={0,500,500};//左右两胳膊的长度，以mm为单位，肩膀长度应为0
ARMCOORDINATIONSTRUCT ArmCoordinationLeft={707,-200,0},ArmCoordinationRight={707,200,0};//要求左右胳膊的终点需要达到的位置
ARMCOORDINATIONSTRUCT ArmShoulderCoordinationLeft={0,-200,0},ArmShoulderCoordinationRight={0,200,0};//左右肩膀相对于坐标零点的空间位置，两者处于同一坐标系中

u16 LLLToAngle(u16 x0,u16 x1,u16 y)//利用三角定理，一个角的两条边为x0,x1,对边为y,求出此角
{
return (u16)((acos((x0*x0+x1*x1-y*y)/(2.0*x0*x1)))*180/PI);
}
/********************************************************************
函数名：ArmCoordination
参数：返回参数：ArmAngle 为各电机旋转角度的结构体
  输入参数： ArmCoordination 最终要达到的坐标点
     ArmShoulderCoordination 肩膀相对于坐标零点的坐标值
     ArmLength 胳膊各部位的长度
     AngleEx 可选参数，当终点在y轴时，x,z值均为0，此时胳膊与水平面的夹角由此参数决定，即肩膀处电机的旋转角度，此时默认为90°即水平方向
********************************************************************/
static void ArmCoordination(ARMANGLESTRUCT *ArmAngle,ARMCOORDINATIONSTRUCT ArmCoordination,ARMCOORDINATIONSTRUCT ArmShoulderCoordination,ARMLENGTHSTRUCT ArmLength,unsigned char AngleEx)
{
u32 Length = 0 ;
u16 AngleShoulderEx = 0,AngleBoomEx = 0;

ArmCoordination.x = ArmCoordination.x - ArmShoulderCoordination.x;
ArmCoordination.y = ArmCoordination.y - ArmShoulderCoordination.y;
ArmCoordination.z = ArmCoordination.z - ArmShoulderCoordination.z;

// printf("ArmCoordination:    x = %d y = %d z = %d\n",ArmCoordination.x,ArmCoordination.y,ArmCoordination.z);
Length = (u32)sqrt(ArmCoordination.x*ArmCoordination.x+ArmCoordination.y*ArmCoordination.y+ ArmCoordination.z*ArmCoordination.z);

printf("Length==%d\n",Length);

if (Length > (ArmLength.forearm +ArmLength.boom)*1.01)//不允许1m长的胳膊误差大于1cm
{
  ArmCoordination.x = ArmCoordination.x * (ArmLength.boom + ArmLength.forearm) / Length;
  ArmCoordination.y = ArmCoordination.y * (ArmLength.boom + ArmLength.forearm) / Length;
  ArmCoordination.z = ArmCoordination.z * (ArmLength.boom + ArmLength.forearm) / Length;
  Length = ArmLength.boom + ArmLength.forearm;
  printf("The Length Is Too Long!!        Length==%d\n",Length);
}

AngleShoulderEx = (u16)(asin(ArmCoordination.z*1.0/sqrt(ArmCoordination.x*ArmCoordination.x+ArmCoordination.z*ArmCoordination.z))*180/PI);
AngleBoomEx = (u16)atan(ArmCoordination.y/((float)sqrt(ArmCoordination.z*ArmCoordination.z+ArmCoordination.x*ArmCoordination.x)))*180/PI;

if (ArmCoordination.x||ArmCoordination.z){
  if (ArmCoordination.x>=0) {
   ArmAngle->shoulder = 90+AngleShoulderEx;

ArmAngle->boom = 90+ AngleBoomEx + LLLToAngle(Length,ArmLength.boom,ArmLength.forearm);
} else {

   if (ArmCoordination.y>=0){
    ArmAngle->shoulder = 90 - AngleShoulderEx;

    ArmAngle->boom = 90 + 180 - AngleBoomEx + LLLToAngle(Length,ArmLength.boom,ArmLength.forearm);
   } else {
    if (ArmCoordination.z>=0)
    {
     ArmAngle->shoulder = 90+180-AngleShoulderEx;

     ArmAngle->boom = 90 + AngleBoomEx + LLLToAngle(Length,ArmLength.boom,ArmLength.forearm);
    } else{
     ArmAngle->shoulder = 90-180-AngleShoulderEx;

     ArmAngle->boom = 90 + AngleBoomEx + LLLToAngle(Length,ArmLength.boom,ArmLength.forearm);
    }
   }
  }
} else{
  if (AngleEx){
   ArmAngle->shoulder = AngleEx;
  } else{
   ArmAngle->shoulder = 90;
  }
  if (ArmCoordination.y>=0){
   ArmAngle->boom = 90 +90 + LLLToAngle(Length,ArmLength.boom,ArmLength.forearm);
  } else{
   ArmAngle->boom = LLLToAngle(Length,ArmLength.boom,ArmLength.forearm);
  }
}

if (ArmCoordination.y>=0||ArmCoordination.x>=0) {
  ArmAngle->forearm = LLLToAngle(ArmLength.boom,ArmLength.forearm,Length);
} else {
  ArmAngle->forearm = LLLToAngle(ArmLength.boom,ArmLength.forearm,Length);
}
// printf("ArmAngle:    shoulder = %d boom = %d forearm = %d\n",ArmAngle->shoulder,ArmAngle->boom,ArmAngle->forearm);
}
/********************************************************************
函数名：ArmCoordinationSwitchLeft
参数：输入参数： ArmCoordinationL 左臂要达到的终点
     AngleExL 描述见ArmCoordination中的AngleEx
********************************************************************/
void ArmCoordinationSwitchLeft(ARMCOORDINATIONSTRUCT ArmCoordinationL,unsigned char AngleExL)
{
ARMCOORDINATIONSTRUCT ArmCoordinationEx = ArmCoordinationL;
ARMCOORDINATIONSTRUCT ArmShoulderCoordinationEx = ArmShoulderCoordinationLeft;

ArmCoordinationEx.y = - ArmCoordinationL.y;
ArmShoulderCoordinationEx.y = - ArmShoulderCoordinationLeft.y;

ArmCoordination(&ArmAngleLeft,ArmCoordinationEx,ArmShoulderCoordinationEx,ArmLengthLeft,AngleExL);
}
/********************************************************************
函数名：ArmCoordinationSwitchRight
参数：输入参数： ArmCoordinationR 左臂要达到的终点
AngleExR 描述见ArmCoordination中的AngleEx
********************************************************************/
void ArmCoordinationSwitchRight(ARMCOORDINATIONSTRUCT ArmCoordinationR,unsigned char AngleExR)
{
ArmCoordination(&ArmAngleRight,ArmCoordinationR,ArmShoulderCoordinationRight,ArmLengthRight,AngleExR);
}

void main(void)
{
// char i;
while (1)
{
//  printf("Please Select The Coordination To Reach!\n\n");
//   scanf("%d",&i);
//   switch(i){
//    case 1 : ArmCoordinationLeft.x = 200;ArmCoordinationLeft.y= 300;ArmCoordinationLeft.z= 200;break;
//    case 2 : ArmCoordinationLeft.x = 200;ArmCoordinationLeft.y=-300;ArmCoordinationLeft.z= 200;break;
//    case 3 : ArmCoordinationLeft.x =-200;ArmCoordinationLeft.y=-300;ArmCoordinationLeft.z= 200;break;
//    case 4 : ArmCoordinationLeft.x =-200;ArmCoordinationLeft.y= 300;ArmCoordinationLeft.z= 200;break;
//    case 5 : ArmCoordinationLeft.x = 200;ArmCoordinationLeft.y= 300;ArmCoordinationLeft.z=-200;break;
//    case 6 : ArmCoordinationLeft.x = 200;ArmCoordinationLeft.y=-300;ArmCoordinationLeft.z=-200;break;
//    case 7 : ArmCoordinationLeft.x =-200;ArmCoordinationLeft.y=-300;ArmCoordinationLeft.z=-200;break;
//    case 8 : ArmCoordinationLeft.x =-200;ArmCoordinationLeft.y= 300;ArmCoordinationLeft.z=-200;break;
//    default:
//     break;
//   }
  printf("Please Select The Coordination Of Left Arm To Reach!    ");
  scanf("%d %d %d",&ArmCoordinationLeft.x,&ArmCoordinationLeft.y,&ArmCoordinationLeft.z);
  ArmCoordinationSwitchLeft(ArmCoordinationLeft,0);
  printf("ArmAngleLeft:    shoulder = %d boom = %d forearm = %d\n\n",ArmAngleLeft.shoulder,ArmAngleLeft.boom,ArmAngleLeft.forearm);

  printf("Please Select The Coordination Of Right Arm To Reach!    ");
  scanf("%d %d %d",&ArmCoordinationRight.x,&ArmCoordinationRight.y,&ArmCoordinationRight.z);
  ArmCoordinationSwitchRight(ArmCoordinationRight,0);
  printf("ArmAngleRight:    shoulder = %d boom = %d forearm = %d\n\n",ArmAngleRight.shoulder,ArmAngleRight.boom,ArmAngleRight.forearm);

  printf("*********Please Again***********\n\n");
}