594 lines
17 KiB
C
594 lines
17 KiB
C
/*================================================================
|
||
File: Dynamic.h
|
||
Prupose: Dynamic parameters of a connection
|
||
Author: Yann Le Tensorer
|
||
Creation Date: 29 january 1997
|
||
------------------------------------------------------------------
|
||
Revisions: 1.01 Yann Le Tensorer
|
||
4 february 1997
|
||
Added function fn_vDynamicObject_GetParams
|
||
=================================================================*/
|
||
#ifdef _AI_LIB_
|
||
#include "AIUseCPA.h"
|
||
#include "dynamic.h"
|
||
#include "IAOption.h"
|
||
#include "IAMacros.h"
|
||
/* AI memory and error management */
|
||
#include "MemIA.h"
|
||
#include "ErrIA.h"
|
||
#else /*_AI_LIB_*/
|
||
#include "acp_base.h"
|
||
#include "dynamic.h"
|
||
#include "malloc.h"
|
||
#endif /*_AI_LIB_*/
|
||
|
||
/*============================================================
|
||
Memory management macros, to be replaced with correct memory management
|
||
============================================================*/
|
||
#ifdef _AI_LIB_
|
||
#define M_malloc(pointer,cast,size) M_IAAlloc(pointer,cast,size)
|
||
#define M_free(pointer) M_IAFree(pointer)
|
||
#else /*_AI_LIB_*/
|
||
#define M_malloc(pointer,cast,size) (pointer=(cast)malloc(size))
|
||
#define M_free(pointer) (free(pointer))
|
||
#endif /*_AI_LIB_*/
|
||
|
||
/*==========================================================
|
||
Function name: fn_vDynamicObject_Calculate
|
||
Description: Caculates the speeds at sampled points, according to dynamic parameters
|
||
Input: p_stDynaParam: pointer to a pre-created tdstDynaParam structure
|
||
Output: none
|
||
Author: Yann Le Tensorer
|
||
Date: 30 january 1997
|
||
Revision:
|
||
==========================================================*/
|
||
void fn_vDynamicObject_Calculate(ACP_tdstDynaParam* p_stDynaParam)
|
||
{
|
||
MTH_tdxReal *SpeedList;
|
||
MTH_tdxReal *SpeedListEnd;
|
||
MTH_tdxReal CurSpeed;
|
||
MTH_tdxReal inc;
|
||
MTH_tdxReal MaxSpeed;
|
||
MTH_tdxReal CurAngle;
|
||
|
||
if (p_stDynaParam==0) return; /* avoid unexepected crash */
|
||
|
||
switch (p_stDynaParam->ucDynamicType)
|
||
{
|
||
case C_ucNone:
|
||
case C_ucConst:
|
||
break;
|
||
case C_ucLinear:
|
||
inc=(p_stDynaParam->xEndSpeed-p_stDynaParam->xStartSpeed)/p_stDynaParam->ucSamplingRate;
|
||
SpeedList = p_stDynaParam->d_stListOfSpeeds; /* start adress */
|
||
if (SpeedList==0) break; /* avoid unexpected crash */
|
||
SpeedListEnd= p_stDynaParam->d_stListOfSpeeds+p_stDynaParam->ucSamplingRate; /* end adress */
|
||
CurSpeed = p_stDynaParam->xStartSpeed;
|
||
|
||
for (;SpeedList<=SpeedListEnd;SpeedList++)
|
||
{
|
||
*SpeedList=CurSpeed;
|
||
CurSpeed+=inc;
|
||
}
|
||
break;
|
||
case C_ucSinus:
|
||
MaxSpeed=(p_stDynaParam->xMaxSpeed)/2;
|
||
inc=(p_stDynaParam->xEndAngle-p_stDynaParam->xStartAngle)/p_stDynaParam->ucSamplingRate;
|
||
SpeedList = p_stDynaParam->d_stListOfSpeeds; /* start adress */
|
||
if (SpeedList==0) break; /* avoid unexpected crash */
|
||
SpeedListEnd= p_stDynaParam->d_stListOfSpeeds+p_stDynaParam->ucSamplingRate; /* end adress */
|
||
CurAngle = p_stDynaParam->xStartAngle;
|
||
|
||
for (;SpeedList<=SpeedListEnd;SpeedList++)
|
||
{
|
||
*SpeedList=(MTH_M_xSin(CurAngle)+1)*MaxSpeed;
|
||
CurAngle+=inc;
|
||
}
|
||
break;
|
||
|
||
|
||
}
|
||
}
|
||
|
||
|
||
/*==========================================================
|
||
Function name: fn_vDynamicObject_Create
|
||
Description: Creates a dynamic parameters object
|
||
Input: p_stDynaParam: pointer to a preallocated tdstDynaParam structure
|
||
ucSamplingRate: sampling rate
|
||
ucDynamicType: dynamic type (C_NONE,C_CONST,C_LINEAR,C_SINUS)
|
||
xFirst,xSecond,xThird: See table below
|
||
|
||
Type: | C_NONE | C_CONST | C_LINEAR | C_SINUS
|
||
===========================================================
|
||
xFirst | - | speed | start speed | start angle
|
||
| | | |
|
||
xSecond | - | - | end speed | end angle
|
||
| | | |
|
||
xThird | - | - | - | max speed
|
||
|
||
|
||
Output: none
|
||
Author: Yann Le Tensorer
|
||
Date: 30 january 1997
|
||
Revision: 5 february 1997 : corrected minor bugs
|
||
==========================================================*/
|
||
|
||
void fn_vDynamicObject_Create( ACP_tdstDynaParam* p_stDynaParam,
|
||
unsigned char ucSamplingRate,
|
||
unsigned char ucDynamicType,
|
||
MTH_tdxReal xFirst,
|
||
MTH_tdxReal xSecond,
|
||
MTH_tdxReal xThird)
|
||
{
|
||
if (p_stDynaParam==0) return;
|
||
|
||
p_stDynaParam->ucSamplingRate=ucSamplingRate;
|
||
if (p_stDynaParam->ucSamplingRate<2) p_stDynaParam->ucSamplingRate=2;
|
||
|
||
p_stDynaParam->ucDynamicType=ucDynamicType;
|
||
switch (ucDynamicType)
|
||
{
|
||
case C_ucNone:
|
||
p_stDynaParam->xSpeed=0;
|
||
p_stDynaParam->xEndSpeed=0;
|
||
p_stDynaParam->xMaxSpeed=0;
|
||
p_stDynaParam->d_stListOfSpeeds=0;
|
||
break;
|
||
case C_ucConst:
|
||
p_stDynaParam->xSpeed=xFirst;
|
||
p_stDynaParam->xEndSpeed=0;
|
||
p_stDynaParam->xMaxSpeed=0;
|
||
p_stDynaParam->d_stListOfSpeeds=0;
|
||
break;
|
||
case C_ucLinear:
|
||
p_stDynaParam->xStartSpeed=xFirst;
|
||
p_stDynaParam->xEndSpeed=xSecond;
|
||
p_stDynaParam->xMaxSpeed=0;
|
||
M_malloc(p_stDynaParam->d_stListOfSpeeds,MTH_tdxReal*,sizeof(MTH_tdxReal)*(p_stDynaParam->ucSamplingRate+1));
|
||
break;
|
||
case C_ucSinus:
|
||
p_stDynaParam->xStartAngle=xFirst;
|
||
p_stDynaParam->xEndAngle=xSecond;
|
||
p_stDynaParam->xMaxSpeed=xThird;
|
||
M_malloc(p_stDynaParam->d_stListOfSpeeds,MTH_tdxReal*,sizeof(MTH_tdxReal)*(p_stDynaParam->ucSamplingRate+1));
|
||
break;
|
||
|
||
}
|
||
fn_vDynamicObject_Calculate(p_stDynaParam);
|
||
}
|
||
|
||
/*==========================================================
|
||
Function name: fn_vDynamicObject_ChangeSamplingRate
|
||
Description: Changes the sampling rate of a dynamic object
|
||
Input: p_stDynaParam: pointer to a preallocated tdstDynaParam structure
|
||
ucSamplingRate: sampling rate
|
||
Output: none
|
||
Author: Yann Le Tensorer
|
||
Date: 5 february 1997
|
||
Revision:
|
||
==========================================================*/
|
||
|
||
void fn_vDynamicObject_ChangeSamplingRate( ACP_tdstDynaParam* p_stDynaParams,
|
||
unsigned char ucSamplingRate)
|
||
|
||
{
|
||
if (p_stDynaParams==0) return;
|
||
|
||
p_stDynaParams->ucSamplingRate=ucSamplingRate;
|
||
if (p_stDynaParams->ucSamplingRate<2) p_stDynaParams->ucSamplingRate=2;
|
||
|
||
switch (p_stDynaParams->ucDynamicType)
|
||
{
|
||
case C_ucNone:
|
||
p_stDynaParams->d_stListOfSpeeds=0;
|
||
break;
|
||
case C_ucConst:
|
||
p_stDynaParams->d_stListOfSpeeds=0;
|
||
break;
|
||
case C_ucLinear:
|
||
if (p_stDynaParams->d_stListOfSpeeds!=0)
|
||
M_free(p_stDynaParams->d_stListOfSpeeds);
|
||
M_malloc(p_stDynaParams->d_stListOfSpeeds,MTH_tdxReal*,sizeof(MTH_tdxReal)*(p_stDynaParams->ucSamplingRate+1));
|
||
break;
|
||
case C_ucSinus:
|
||
if (p_stDynaParams->d_stListOfSpeeds!=0)
|
||
M_free(p_stDynaParams->d_stListOfSpeeds);
|
||
M_malloc(p_stDynaParams->d_stListOfSpeeds,MTH_tdxReal*,sizeof(MTH_tdxReal)*(p_stDynaParams->ucSamplingRate+1));
|
||
break;
|
||
|
||
}
|
||
fn_vDynamicObject_Calculate(p_stDynaParams);
|
||
}
|
||
|
||
|
||
/*==========================================================
|
||
Function name: fn_vDynaParams_Free
|
||
Description: removes the allocated ram for DynaParams
|
||
Input: p_stDynaParam: pointer to a pre-created tdstDynaParam structure
|
||
Output: none
|
||
Author: Yann Le Tensorer
|
||
Date: 30 january 1997
|
||
Revision:
|
||
==========================================================*/
|
||
void fn_vDynamicObject_Free(ACP_tdstDynaParam* p_stDynaParams)
|
||
{
|
||
if (p_stDynaParams!=0)
|
||
if (p_stDynaParams->d_stListOfSpeeds!=0)
|
||
M_free(p_stDynaParams->d_stListOfSpeeds);
|
||
}
|
||
|
||
|
||
/*==========================================================
|
||
Function name: fn_vDynamicObject_ChangeParams
|
||
Description: changes the dynamic parameters (except sampling rate)
|
||
Input: p_stDynaParam: pointer to a preallocated tdstDynaParam structure
|
||
ucDynamicType: dynamic type (C_ucNone,C_ucConst,C_ucLinear,C_ucSinus)
|
||
xFirst,xSecond,xThird: See table below
|
||
|
||
Type: | C_ucNone | C_ucConst | C_ucLinear | C_ucSinus
|
||
===========================================================
|
||
xFirst | - | speed | start speed | start angle
|
||
| | | |
|
||
xSecond | - | - | end speed | end angle
|
||
| | | |
|
||
xThird | - | - | - | max speed
|
||
|
||
|
||
Output: none
|
||
Author: Yann Le Tensorer
|
||
Date: 30 january 1997
|
||
Revision:
|
||
==========================================================*/
|
||
|
||
void fn_vDynamicObject_ChangeParams( ACP_tdstDynaParam* p_stDynaParam,
|
||
unsigned char ucDynamicType,
|
||
MTH_tdxReal xFirst,
|
||
MTH_tdxReal xSecond,
|
||
MTH_tdxReal xThird)
|
||
{
|
||
p_stDynaParam->ucDynamicType=ucDynamicType;
|
||
switch (ucDynamicType)
|
||
{
|
||
case C_ucNone:
|
||
p_stDynaParam->xSpeed=0;
|
||
p_stDynaParam->xEndSpeed=0;
|
||
p_stDynaParam->xMaxSpeed=0;
|
||
if (p_stDynaParam->d_stListOfSpeeds!=0)
|
||
{
|
||
M_free(p_stDynaParam->d_stListOfSpeeds);
|
||
p_stDynaParam->d_stListOfSpeeds=0;
|
||
}
|
||
break;
|
||
case C_ucConst:
|
||
p_stDynaParam->xSpeed=xFirst;
|
||
p_stDynaParam->xEndSpeed=0;
|
||
p_stDynaParam->xMaxSpeed=0;
|
||
if (p_stDynaParam->d_stListOfSpeeds!=0)
|
||
{
|
||
M_free(p_stDynaParam->d_stListOfSpeeds);
|
||
p_stDynaParam->d_stListOfSpeeds=0;
|
||
}
|
||
break;
|
||
case C_ucLinear:
|
||
p_stDynaParam->xStartSpeed=xFirst;
|
||
p_stDynaParam->xEndSpeed=xSecond;
|
||
p_stDynaParam->xMaxSpeed=0;
|
||
if (p_stDynaParam->d_stListOfSpeeds==0)
|
||
M_malloc(p_stDynaParam->d_stListOfSpeeds,MTH_tdxReal*,sizeof(MTH_tdxReal)*(p_stDynaParam->ucSamplingRate+1));
|
||
|
||
break;
|
||
case C_ucSinus:
|
||
p_stDynaParam->xStartAngle=xFirst;
|
||
p_stDynaParam->xEndAngle=xSecond;
|
||
p_stDynaParam->xMaxSpeed=xThird;
|
||
if (p_stDynaParam->d_stListOfSpeeds==0)
|
||
M_malloc(p_stDynaParam->d_stListOfSpeeds,MTH_tdxReal*,sizeof(MTH_tdxReal)*(p_stDynaParam->ucSamplingRate+1));
|
||
break;
|
||
|
||
|
||
}
|
||
fn_vDynamicObject_Calculate(p_stDynaParam);
|
||
|
||
}
|
||
|
||
/*==========================================================
|
||
Function name: fn_vDynamicObject_GetParams
|
||
Description: Gets the dynamic parameters (including sampling rate)
|
||
Input: p_stDynaParam: pointer to a preallocated tdstDynaParam structure
|
||
p_ucDynamicType:pointer to get dynamic type (C_ucNone,C_ucConst,C_ucLinear,C_ucSinus)
|
||
p_SamplingRate: pointer to get sampling rate
|
||
p_xFirst,p_xSecond,p_xThird:pointers to parameters to get See table below
|
||
|
||
Type: | C_ucNone | C_ucConst | C_ucLinear | C_ucSinus
|
||
===========================================================
|
||
xFirst | - | speed | start speed | start angle
|
||
| | | |
|
||
xSecond | - | - | end speed | end angle
|
||
| | | |
|
||
xThird | - | - | - | max speed
|
||
|
||
|
||
Output: none
|
||
Author: Yann Le Tensorer
|
||
Date: 04 february 1997
|
||
Revision:
|
||
|
||
Note: Programmer may call the function with null pointers.
|
||
Only non-null pointers will be affected...
|
||
==========================================================*/
|
||
|
||
void fn_vDynamicObject_GetParams( ACP_tdstDynaParam* p_stDynaParam,
|
||
unsigned char* p_ucDynamicType,
|
||
unsigned char* p_ucSamplingRate,
|
||
MTH_tdxReal* p_xFirst,
|
||
MTH_tdxReal* p_xSecond,
|
||
MTH_tdxReal* p_xThird)
|
||
{
|
||
if (p_stDynaParam==0) return;
|
||
|
||
if (p_ucDynamicType!=0) *p_ucDynamicType=p_stDynaParam->ucDynamicType;
|
||
if (p_ucSamplingRate!=0) *p_ucSamplingRate=p_stDynaParam->ucSamplingRate;
|
||
switch (p_stDynaParam->ucDynamicType)
|
||
{
|
||
case C_ucNone:
|
||
if (p_xFirst!=0) *p_xFirst =0;
|
||
if (p_xSecond!=0) *p_xSecond =0;
|
||
if (p_xThird!=0) *p_xThird =0;
|
||
break;
|
||
case C_ucConst:
|
||
if (p_xFirst!=0) *p_xFirst =p_stDynaParam->xSpeed;
|
||
if (p_xSecond!=0) *p_xSecond =0;
|
||
if (p_xThird!=0) *p_xThird =0;
|
||
break;
|
||
case C_ucLinear:
|
||
if (p_xFirst!=0) *p_xFirst =p_stDynaParam->xStartSpeed;
|
||
if (p_xSecond!=0) *p_xSecond =p_stDynaParam->xEndSpeed;
|
||
if (p_xThird!=0) *p_xThird =0;
|
||
break;
|
||
case C_ucSinus:
|
||
if (p_xFirst!=0) *p_xFirst =p_stDynaParam->xStartAngle;
|
||
if (p_xSecond!=0) *p_xSecond =p_stDynaParam->xEndAngle;
|
||
if (p_xThird!=0) *p_xThird =p_stDynaParam->xMaxSpeed;
|
||
break;
|
||
default:
|
||
break;
|
||
|
||
|
||
}
|
||
|
||
}
|
||
|
||
/*=========================================================
|
||
Function name: fn_xDynamicObject_GetSpeed
|
||
Description: returns the speed at a given sample (n<> of the point)
|
||
Input: p_stDynaParam: pointer to a pre-created tdstDynaParam structure
|
||
iSampleNo: n<> of the point to get the speed from.
|
||
Output: return value is the speed.
|
||
Author: Yann Le Tensorer
|
||
Date: 31 january 1997
|
||
==========================================================*/
|
||
MTH_tdxReal fn_xDynamicObject_GetSpeed(ACP_tdstDynaParam* p_stDynaParams,unsigned char ucSampleNo)
|
||
{
|
||
|
||
MTH_tdxReal xReturn;
|
||
|
||
if (p_stDynaParams==0) return 0;
|
||
|
||
switch (p_stDynaParams->ucDynamicType)
|
||
{
|
||
case C_ucNone:
|
||
xReturn=0;
|
||
break;
|
||
case C_ucConst:
|
||
xReturn=p_stDynaParams->xSpeed;
|
||
break;
|
||
case C_ucLinear:
|
||
case C_ucSinus:
|
||
if (ucSampleNo<=p_stDynaParams->ucSamplingRate && p_stDynaParams->d_stListOfSpeeds!=0)
|
||
xReturn=p_stDynaParams->d_stListOfSpeeds[ucSampleNo];
|
||
else
|
||
xReturn=0;
|
||
|
||
break;
|
||
default: xReturn=0;
|
||
}
|
||
|
||
return xReturn;
|
||
|
||
|
||
}
|
||
|
||
/*==========================================================
|
||
Function name: fn_vDynamicObject_CreateNotSampled
|
||
Description: Creates a dynamic parameters object for not sampled type
|
||
Input: p_stDynaParam: pointer to a preallocated tdstDynaParam structure
|
||
ucDynamicType: dynamic type (C_NONE,C_CONST,C_LINEAR,C_SINUS)
|
||
xFirst,xSecond,xThird: See table below
|
||
|
||
Type: | C_NONE | C_CONST | C_LINEAR | C_SINUS
|
||
===========================================================
|
||
xFirst | - | speed | start speed | start angle
|
||
| | | |
|
||
xSecond| - | - | end speed | end angle
|
||
| | | |
|
||
xThird | - | - | - | max speed
|
||
|
||
|
||
Output: none
|
||
Author: Albert Pais
|
||
Date: February 04, 1997
|
||
Revision:
|
||
==========================================================*/
|
||
void fn_vDynamicObject_CreateNotSampled
|
||
(
|
||
ACP_tdstDynaParam* p_stDynaParam,
|
||
unsigned char ucDynamicType,
|
||
MTH_tdxReal xFirst,
|
||
MTH_tdxReal xSecond,
|
||
MTH_tdxReal xThird
|
||
)
|
||
{
|
||
p_stDynaParam->ucDynamicType=ucDynamicType;
|
||
switch (ucDynamicType)
|
||
{
|
||
case C_ucNone:
|
||
p_stDynaParam->xSpeed=0;
|
||
p_stDynaParam->xEndSpeed=0;
|
||
p_stDynaParam->xMaxSpeed=0;
|
||
p_stDynaParam->d_stListOfSpeeds=0;
|
||
break;
|
||
case C_ucConst:
|
||
p_stDynaParam->xSpeed=xFirst;
|
||
p_stDynaParam->xEndSpeed=0;
|
||
p_stDynaParam->xMaxSpeed=0;
|
||
p_stDynaParam->d_stListOfSpeeds=0;
|
||
break;
|
||
case C_ucLinear:
|
||
p_stDynaParam->xStartSpeed=xFirst;
|
||
p_stDynaParam->xEndSpeed=xSecond;
|
||
p_stDynaParam->xMaxSpeed=0;
|
||
break;
|
||
case C_ucSinus:
|
||
p_stDynaParam->xStartAngle=xFirst;
|
||
p_stDynaParam->xEndAngle=xSecond;
|
||
p_stDynaParam->xMaxSpeed=xThird;
|
||
break;
|
||
}
|
||
}
|
||
|
||
/*==========================================================
|
||
Function name: fn_xDynamicObject_GetSpeedNotSampled
|
||
Description: returns the speed at a given position when it is not sampled
|
||
Input:
|
||
p_stDynaParam: pointer to a pre-created tdstDynaParam structure
|
||
_xDist : the dist beetween the position where speed is required and the point to reach
|
||
Output: return value is the speed.
|
||
Author: Albert Pais
|
||
Date: February 4,1997
|
||
Revision:
|
||
==========================================================*/
|
||
MTH_tdxReal fn_xDynamicObject_GetSpeedNotSampled
|
||
(
|
||
ACP_tdstDynaParam* _p_stDynaParams,
|
||
MTH_tdxReal _xDist
|
||
)
|
||
{
|
||
MTH_tdxReal xReturn;
|
||
|
||
switch (_p_stDynaParams->ucDynamicType)
|
||
{
|
||
case C_ucNone:
|
||
xReturn=0;
|
||
break;
|
||
case C_ucConst:
|
||
xReturn=_p_stDynaParams->xSpeed;
|
||
break;
|
||
case C_ucLinear:
|
||
/* Speed = InitialSpeed + CurrentDist x (FinalSpeed-InitialSpeed) / InitialDist */
|
||
/* tip : (FinalSpeed-InitialSpeed) / InitialDist is stored inside xSpeed field */
|
||
xReturn = MTH_M_xAdd
|
||
(
|
||
_p_stDynaParams->xStartSpeed,
|
||
MTH_M_xMul(_xDist,_p_stDynaParams->xSpeed )
|
||
);
|
||
break;
|
||
case C_ucSinus:
|
||
/* Speed = MaxSpeed x
|
||
(1+sin( (FinalAngle - InitialAngle)/InitialDist * CurrentDist + InitialAngle )) */
|
||
/* tip : (FinalAngle - InitialAngle)/InitialDist is stored inside xSpeed field */
|
||
xReturn = MTH_M_xMul
|
||
(
|
||
(_p_stDynaParams->xMaxSpeed),
|
||
MTH_M_xAdd
|
||
(
|
||
MTH_C_ONE,
|
||
MTH_M_xSin
|
||
(
|
||
MTH_M_xAdd
|
||
(
|
||
MTH_M_xMul
|
||
(
|
||
_p_stDynaParams->xSpeed,
|
||
_xDist
|
||
),
|
||
_p_stDynaParams->xStartAngle
|
||
)
|
||
)
|
||
)
|
||
);
|
||
break;
|
||
default: xReturn=0;
|
||
}
|
||
|
||
return xReturn;
|
||
}
|
||
|
||
/*==========================================================
|
||
Function name: fn_vDynamicObject_CalculateNotSampled
|
||
Description: Caculates specific coef when speed is not sampled
|
||
Input:
|
||
p_stDynaParam: pointer to a pre-created tdstDynaParam structure
|
||
the initial dist beetween the initial point and the final point
|
||
Output: none
|
||
Remark :
|
||
The xSpeed field of the structure is used to store some
|
||
parameters used at execution
|
||
Author: Albert Pais
|
||
Date: Frebruary 4,1997
|
||
Revision:
|
||
==========================================================*/
|
||
void fn_vDynamicObject_CalculateNotSampled
|
||
(
|
||
ACP_tdstDynaParam* _p_stDynaParam,
|
||
MTH_tdxReal _xInitialDist
|
||
)
|
||
{
|
||
switch (_p_stDynaParam->ucDynamicType)
|
||
{
|
||
case C_ucNone:
|
||
case C_ucConst:
|
||
/* nothing to compute */
|
||
break;
|
||
case C_ucLinear:
|
||
if(_xInitialDist)
|
||
{/* xSpeed is used to store a specific coef :*/
|
||
_p_stDynaParam->xSpeed =
|
||
MTH_M_xDiv
|
||
(
|
||
MTH_M_xSub
|
||
(
|
||
(_p_stDynaParam->xEndSpeed),
|
||
(_p_stDynaParam->xStartSpeed)
|
||
),
|
||
_xInitialDist
|
||
);
|
||
}
|
||
else
|
||
{
|
||
_p_stDynaParam->xSpeed = MTH_C_ZERO;
|
||
}
|
||
break;
|
||
case C_ucSinus:
|
||
if(_xInitialDist)
|
||
{/* xSpeed is used to store a specific coef :*/
|
||
_p_stDynaParam->xSpeed =
|
||
MTH_M_xDiv
|
||
(
|
||
MTH_M_xSub
|
||
(
|
||
(_p_stDynaParam->xEndAngle),
|
||
(_p_stDynaParam->xStartAngle)
|
||
),
|
||
_xInitialDist
|
||
);
|
||
}
|
||
else
|
||
{
|
||
_p_stDynaParam->xSpeed = MTH_C_ZERO;
|
||
}
|
||
break;
|
||
}
|
||
}
|
||
|