math.c

2005年电子设计竞赛悬挂运动控制系统源码

//**********************************//
//	源 文 件:math.c					//
//	描    述:数学计算				//
//	主要功能:点到点的距离计算		//
//	编写作者:by Gholt(林青)			//
//  编写日期:2005-09-07				//
//  整理日期:2005-10-30				//
//**********************************//

#include "math.h"
#include "SPCE061V004.H"
//线与定滑轮的触点坐标 左:(Lx,Ly) 右:(Rx,Ry)
double Lx=-13.5,Ly=116.0,Rx=93.5,Ry=116.0;

//======================================================
//函数:void L12(double x,double y,double *s1,double *s2)
//语法:void L12(double x,double y,double *s1,double *s2)
//描述:计算点(x,y)到两定滑轮的距离s1、s2
//参数:坐标点(x,y)，指向存放点到滑轮距离的指针s1、s2
//返回:无
//======================================================
void L12(double x,double y,double *s1,double *s2)
{
	double L1,L2;
	double Dx,Dy;
	Dx = x - Lx;
	Dy = y - Ly;
	Dx *= Dx;
	Dy *= Dy;
	L1 = sqrt(Dx+Dy);
	*s1=L1;
	*P_Watchdog_Clear = 1;
	Dx = Rx - x;
	Dy = Ry - y;
	Dx *= Dx;
	Dy *= Dy;
	L2 = sqrt(Dx+Dy);
	*s2=L2;
}