/*================================================================ 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; } }