From anonymous, 10 Months ago, written in C.
Embed
  1. void CO_FF_Run(co_dev_t *drv)
  2. {
  3.     ASSERT_PTR(drv);
  4.     ASSERT(drv->dev_nr < CO_DEV_COUNT);
  5.     ret_t ret_val = RET_OK;
  6. #ifdef FF_Mode
  7.     int32_t  iStep1 = 0;
  8.     int32_t  iStep2 = 0;
  9.     int32_t  i32StepActual = 0;
  10.     static int32_t i32TempCalc[2] = {0, 0};
  11.     static int32_t i32TempCalcSoll[2] = {0, 0};
  12.     int32_t  i32pFactorMoveBack = 50;
  13.     int32_t iDiff = 0;
  14.     int16_t   iTurbulenceForce[2] = {0, 0};
  15.     int16_t   iTurbulenceForce2[2] = {0, 0};
  16.  
  17.     int32_t iTorqueValue = 0;
  18.     int32_t i32TorqueValue1 = 0;
  19.     int32_t i32TorqueValue2 = 0;
  20.     int32_t i32MaxSpeed = 0;
  21.  
  22.     int32_t i32TrimCalc = 0;
  23.     static int32_t i32MaxSpeedSoll[2] = {0, 0};
  24.     //static int16_t   i16FCounter[2]={0,0};
  25.     static int16_t iOffset[2] = {200, 200};
  26.     static int16_t iOffsetShadow[2] = {200, 200};
  27.     static int16_t iTurbulenceTime[2] = {0, 0};
  28.     static int16_t iTurbulenceTime2[2] = {0, 0};
  29.     static int32_t iMaxMotorSpeed[2] = {1000, 1000};
  30.     int32_t  i32SoftEndPosDelta = 0;
  31.     int32_t  position_window_half = 0;
  32.     int16_t iTargetTorqueShadow = 0;
  33.     //static int32_t iForceTot[2]={0,0};
  34.     static int32_t i32AbsForce[2] = {0, 0};
  35.  
  36.     int32_t  i32absffPosition = 0;
  37.     static int32_t i32FFPosWaitCounter[2] = {0, 0};
  38.     static int32_t i32OffsetCounter[2] = {-1500, -1500};
  39.     static int32_t i32FFEndPosCounter[2] = {0, 0};
  40.     static int32_t i32AninSum1[2] = {0, 0};
  41.     static int32_t i32AninSum2[2] = {0, 0};
  42.     static int32_t i32LastTrimPosition[2] = {0, 0};
  43.     static int32_t i32SollTrimPosition[2] = {0, 0};
  44.     static int16_t i16TrimCounter[2] = {0, 0};
  45.  
  46.  
  47.     static int16_t i16ScaleFactorCounter[2] = {0, 0};
  48.  
  49.     static int32_t i32FrictionValue[2] = {0, 0};
  50.     static int32_t i32FrictionCorCounter[2] = {0, 0};
  51.     static int16_t i16FrictionCor[2] = {0, 0};
  52.     static int16_t i16FrictionZeroCounter[2] = {0, 0};
  53.     static int16_t i16OffsetVib[2] = {0, 0};
  54.     static int16_t i16ManualOffsetForce[2] = {0, 0};
  55.     static int16_t i16ManualOffsetForceSoll[2] = {0, 0};
  56.     static int16_t i16ManualOffsetForceCalc[2] = {0, 0};
  57.     static int16_t i16ManualOffsetChangeCounter[2] = {0, 0};
  58.     static uint16_t ui16OffsetTimeOut[2] = {0, 0};
  59.  
  60.  
  61.     static int16_t i16DynamicOffset[2] = {0, 0};
  62.  
  63.     static int32_t i32FrictionRegulatorSollValue[2] = {0, 0};
  64.  
  65.     static int32_t i32ForceMaxCounter[2] = {0, 0};
  66.     static int16_t iAxisForceSlave[2] = {0, 0};
  67.     static int32_t iOverheatProtectCounter[2] = {0, 0};
  68.     int16_t  i16ff_manual_offset_force = 0;
  69.     static int16_t  i16ff_manual_offset_forceLast[2] = {0, 0};
  70.  
  71.     static int16_t i16WaitManuelOffsetTimer[2] = {0, 0};
  72.     static int16_t i16SpeedLimitOff[2] = {0, 0};
  73.     uint16_t ui16DynTrimStep = 0;
  74.  
  75.     //static int16_t iValueVelocity[2]={0,0};
  76.     int32_t i32ActualTorque = 0;
  77.     int32_t i32temp = 0;
  78.     //int32_t i32temp2=0;
  79.  
  80.     uint32_t ui32MaxVelocity = 0;
  81.     int16_t  i16InverseSensorControl = 0;
  82.  
  83.     int32_t i32tempNegativePosLimit = 0;
  84.     int16_t  i16OverForce = 0;
  85.     static int32_t i32OverTime[2] = {0, 0};
  86.  
  87.     static int16_t i16SyncAverageCount[2] = {0, 0};
  88.     static int32_t i32SyncForceIst[2] = {0, 0};
  89.     static int32_t i32SyncPosIst[2] = {0, 0};
  90.  
  91.     static int16_t iStartequalTimer = 0;
  92.     static int32_t sync_torqu_ist_back = 0;
  93.     static int16_t iForceErrorCounter[2] = {0, 0};
  94.  
  95.     static int32_t  i32PositionLast[2] = {0, 0};
  96.     static int32_t  i32PositionWindowShadow[2] = {0, 0};
  97.     int32_t  i32PositionLimitMax[2] = {0, 0};
  98.  
  99.     static int16_t iVelZeroCounter[2] = {0, 0};
  100.     static int16_t iSyncAverage[2] = {0, 0};
  101.     static int32_t iAutopilotSpeed[2] = {0, 0};
  102.     int32_t  i32SmoothPos = 0;
  103.  
  104.     static int16_t i32DynamicCenter[2] = {0, 0};
  105.     //static int16_t i16DynamicCounter[2]={0,0};
  106.     //static int32_t i32SensorZeroCounter[2]={0,0};
  107.     static int16_t i16ForceUnfiltered[2] = {0, 0};
  108.     static int32_t i32ForceCalc[2] = {0, 0};
  109.     static int32_t i32ForceFilterCounter[2] = {0, 0};
  110.  
  111.     //static int32_t i32RollingAverage[100];
  112.     static int32_t iMaxSynCorr[2] = {0, 0};
  113.     static int32_t iSetOverheat[2] = {0, 0};
  114.     static int32_t i32SwingSoll = 0;
  115.     int16_t i = 0;
  116.  
  117.  
  118.     /*static int32_t iCountSwing=10;
  119.     static int32_t iSwingSpeed=500;
  120.     static int32_t iSwingCMD=0;
  121.     static int32_t iLastVel=0;
  122.     */
  123.     uint16_t  u16ScalfactorStepSoll[2] = {0, 0};
  124.  
  125.  
  126.  
  127.  
  128. #endif
  129.  
  130.  
  131. #ifdef FF_Mode
  132.     if (drv->iSyncLostCounter > 2000)
  133.     {
  134.         drv->FFsyncLost = true;
  135.     }
  136.     drv->iSyncLostCounter++;
  137.  
  138.     if (drv->od_vars->od_6067_p402_position_window < 100)
  139.     {
  140.         drv->od_vars->od_6067_p402_position_window = 100;
  141.     }
  142.  
  143.     //Force Feedback Mode Enabled
  144.     if ((drv->od_vars->od_6060_p402_modes_of_operation == DS402_OP_MODE_FF) && (bPowerUpOk == true))
  145.     {
  146.         if (drv->ForceFeedbackMode.FFMode_Reg.bit.InvertedPosition == 1)
  147.         {
  148.             drv->ForceFeedbackMode.FFMode_Reg.bit.InvertedPosition = 0;
  149.  
  150.         }
  151.         if (i32PositionLast[drv->dev_nr] != drv->od_vars->od_6067_p402_position_window )
  152.         {
  153.             i32PositionLimitMax[drv->dev_nr] = (drv->od_vars->od_2119_msp_ff_soft_limit / 20);
  154.             if (i32PositionLimitMax[drv->dev_nr] < 10)
  155.             {
  156.                 i32PositionLimitMax[drv->dev_nr] = 10;
  157.             }
  158.             i32PositionWindowShadow[drv->dev_nr] = drv->od_vars->od_6067_p402_position_window ;
  159.             if (i32PositionWindowShadow[drv->dev_nr] > i32PositionLimitMax[drv->dev_nr])
  160.             {
  161.                 i32PositionWindowShadow[drv->dev_nr] = i32PositionLimitMax[drv->dev_nr];
  162.                 drv->od_vars->od_6067_p402_position_window = i32PositionLimitMax[drv->dev_nr];
  163.             }
  164.             i32PositionLast[drv->dev_nr] = drv->od_vars->od_6067_p402_position_window ;
  165.  
  166.         }
  167.  
  168.  
  169.         drv->od_vars->od_606C_p402_velocity_actual_value = drv->od_vars->od_2045_msp_actual_motor_speed;
  170.         //KickDog();
  171.         if (drv->od_vars->od_2100_msp_ff_regulator.profileMode == 0)
  172.         {
  173.             i32tempNegativePosLimit = (drv->od_vars->od_2119_msp_ff_soft_limit * -1);
  174.         }
  175.         else
  176.         {
  177.             if (drv->od_vars->od_209C_msp_neg_software_limit > -300)
  178.             {
  179.                 i32tempNegativePosLimit = -300;
  180.             }
  181.             else
  182.             {
  183.                 i32tempNegativePosLimit = drv->od_vars->od_209C_msp_neg_software_limit;
  184.             }
  185.         }
  186.         if ((drv->ForceFeedbackMode.FFMode_Reg.bit.Force_Linear_Up == 0) && (drv->ForceFeedbackMode.FFMode_Reg.bit.EXT_Control == 1))
  187.         {
  188.             drv->od_vars->od_2114_msp_ff_scale_factor = 100;
  189.             if (drv->od_vars->od_2102_msp_ff_max_move_back_speed != 200)
  190.             {
  191.                 drv->od_vars->od_2102_msp_ff_max_move_back_speed = 200;
  192.             }
  193.         }
  194.         drv->ForceFeedbackSyncStateSoll.FFSyncState_Reg.all =  drv->od_vars->od_2111_msp_ff_state_soll;
  195.  
  196.  
  197.  
  198.         if (drv->ForceFeedbackMode.FFMode_Reg.bit.InvertedPosition == 1)
  199.         {
  200.             if (drv->ForceFeedbackMode.FFMode_Reg.bit.InversForce == 1)
  201.             {
  202.                 i16InverseSensorControl = 0;
  203.             }
  204.             else
  205.             {
  206.                 i16InverseSensorControl = 1;
  207.             }
  208.         }
  209.         else
  210.         {
  211.             i16InverseSensorControl = drv->ForceFeedbackMode.FFMode_Reg.bit.InversForce;
  212.         }
  213.  
  214.  
  215.         if (drv->od_vars->od_2114_msp_ff_scale_factor > 99)
  216.         {
  217.             i32FrictionRegulatorSollValue[drv->dev_nr] = drv->od_vars->od_211D_msp_friction_regulator[2];
  218.         }
  219.         else
  220.         {
  221.             i32FrictionRegulatorSollValue[drv->dev_nr] = (drv->od_vars->od_211D_msp_friction_regulator[2] * drv->od_vars->od_2114_msp_ff_scale_factor) / 100;
  222.         }
  223.  
  224.  
  225.         if ( drv->od_vars->od_2116_msp_ff_trim_position > ((i32tempNegativePosLimit * -1) - 20))
  226.         {
  227.             drv->od_vars->od_2116_msp_ff_trim_position -= 20;
  228.         }
  229.         else
  230.         {
  231.             if ( drv->od_vars->od_2116_msp_ff_trim_position < ((drv->od_vars->od_2119_msp_ff_soft_limit - 20) * -1))
  232.             {
  233.                 drv->od_vars->od_2116_msp_ff_trim_position += 20;
  234.             }
  235.         }
  236.  
  237.  
  238.         i32TrimCalc = drv->od_vars->od_2115_msp_ff_trim_offset + drv->od_vars->od_2116_msp_ff_trim_position;
  239.  
  240.  
  241.  
  242.         if (drv->od_vars->od_2115_msp_ff_trim_offset > drv->od_vars->od_2119_msp_ff_soft_limit)
  243.         {
  244.             drv->od_vars->od_2115_msp_ff_trim_offset = drv->od_vars->od_2119_msp_ff_soft_limit;
  245.         }
  246.         else
  247.         {
  248.             if (drv->od_vars->od_2115_msp_ff_trim_offset < i32tempNegativePosLimit)
  249.             {
  250.                 drv->od_vars->od_2115_msp_ff_trim_offset = i32tempNegativePosLimit;
  251.             }
  252.         }
  253.         if (i32TrimCalc < (drv->od_vars->od_2119_msp_ff_soft_limit * -1) )
  254.         {
  255.             i32TrimCalc = drv->od_vars->od_2119_msp_ff_soft_limit * -1;
  256.             drv->od_vars->od_2116_msp_ff_trim_position = (drv->od_vars->od_2119_msp_ff_soft_limit * -1) - drv->od_vars->od_2115_msp_ff_trim_offset;
  257.         }
  258.         else
  259.         {
  260.             if (i32TrimCalc > (i32tempNegativePosLimit * -1))
  261.             {
  262.                 i32TrimCalc = i32tempNegativePosLimit * -1;
  263.                 drv->od_vars->od_2116_msp_ff_trim_position = (i32tempNegativePosLimit * -1) - drv->od_vars->od_2115_msp_ff_trim_offset;
  264.  
  265.             }
  266.         }
  267.  
  268.  
  269.         //Trim Step
  270.         if (drv->od_vars->od_211C_msp_ff_Trim_Step > 0)
  271.         {
  272.             if (i32LastTrimPosition[drv->dev_nr] != i32TrimCalc)
  273.             {
  274.                 //i16FrictionRegulatorSollValue=(msp_friction_regulator[2]*4)/3;
  275.                 i16FrictionZeroCounter[drv->dev_nr] = 0;
  276.                 if (i16TrimCounter[drv->dev_nr] > 100)
  277.                 {
  278.                     i16TrimCounter[drv->dev_nr] = 0;
  279.  
  280.                     ui16DynTrimStep = labs(i32SollTrimPosition[drv->dev_nr] - i32TrimCalc) >> drv->od_vars->od_2100_msp_ff_regulator.trimRamp;
  281.  
  282.                     if (ui16DynTrimStep > drv->od_vars->od_211C_msp_ff_Trim_Step)
  283.                     {
  284.                         ui16DynTrimStep = drv->od_vars->od_211C_msp_ff_Trim_Step;
  285.                     }
  286.                     if (ui16DynTrimStep < 1)
  287.                     {
  288.                         ui16DynTrimStep = 1;
  289.                     }
  290.  
  291.                     if (i32SollTrimPosition[drv->dev_nr] < i32TrimCalc)
  292.                     {
  293.                         if ((i32SollTrimPosition[drv->dev_nr] + ui16DynTrimStep) < i32TrimCalc)
  294.                         {
  295.                             i32SollTrimPosition[drv->dev_nr] = i32SollTrimPosition[drv->dev_nr] + ui16DynTrimStep;
  296.                         }
  297.                         else
  298.                         {
  299.                             i32SollTrimPosition[drv->dev_nr] = i32TrimCalc;
  300.                         }
  301.  
  302.                     }
  303.                     else
  304.                     {
  305.                         if ((i32SollTrimPosition[drv->dev_nr] - ui16DynTrimStep) > i32TrimCalc)
  306.                         {
  307.                             i32SollTrimPosition[drv->dev_nr] = i32SollTrimPosition[drv->dev_nr] - ui16DynTrimStep;
  308.                         }
  309.                         else
  310.                         {
  311.                             i32SollTrimPosition[drv->dev_nr] = i32TrimCalc;
  312.                         }
  313.                     }
  314.                 }
  315.                 else
  316.                 {
  317.                     i16TrimCounter[drv->dev_nr] += 2;
  318.                 }
  319.                 i32LastTrimPosition[drv->dev_nr] = i32SollTrimPosition[drv->dev_nr];
  320.             }
  321.             else
  322.             {
  323.                 i16TrimCounter[drv->dev_nr] = 0;
  324.  
  325.                 i32LastTrimPosition[drv->dev_nr] = i32TrimCalc;
  326.  
  327.                 i32SollTrimPosition[drv->dev_nr] = i32TrimCalc;
  328.             }
  329.         }
  330.         else
  331.         {
  332.             i32SollTrimPosition[drv->dev_nr] = i32TrimCalc;
  333.         }
  334.         //drv->od_vars->od_2033_msp_debug_monitor[1] = i32SollTrimPosition[drv->dev_nr];
  335.  
  336.         //Scale Factor step
  337.         if (drv->od_vars->od_211E_msp_ff_scalefactor_step > 0)
  338.         {
  339.             if (i32SollScaleFactor[drv->dev_nr] <= drv->od_vars->od_211F_msp_manual_offset_off_pos)
  340.             {
  341.                 u16ScalfactorStepSoll[drv->dev_nr] = 1;
  342.             }
  343.             else
  344.             {
  345.                 u16ScalfactorStepSoll[drv->dev_nr] = drv->od_vars->od_211E_msp_ff_scalefactor_step;
  346.             }
  347.  
  348.  
  349.             if (i32LastScaleFactor[drv->dev_nr] != drv->od_vars->od_2114_msp_ff_scale_factor)
  350.             {
  351.                 if (i16ScaleFactorCounter[drv->dev_nr] > 100)
  352.                 {
  353.                     i16ScaleFactorCounter[drv->dev_nr] = 0;
  354.                     if (i32SollScaleFactor[drv->dev_nr] < drv->od_vars->od_2114_msp_ff_scale_factor)
  355.                     {
  356.  
  357.                         if ((i32SollScaleFactor[drv->dev_nr] + u16ScalfactorStepSoll[drv->dev_nr]) < drv->od_vars->od_2114_msp_ff_scale_factor)
  358.                         {
  359.                             i32SollScaleFactor[drv->dev_nr] = i32SollScaleFactor[drv->dev_nr] + u16ScalfactorStepSoll[drv->dev_nr];
  360.                         }
  361.                         else
  362.                         {
  363.                             i32SollScaleFactor[drv->dev_nr] = drv->od_vars->od_2114_msp_ff_scale_factor;
  364.                         }
  365.  
  366.                     }
  367.                     else
  368.                     {
  369.                         if ((i32SollScaleFactor[drv->dev_nr] - u16ScalfactorStepSoll[drv->dev_nr]) > drv->od_vars->od_2114_msp_ff_scale_factor)
  370.                         {
  371.                             i32SollScaleFactor[drv->dev_nr] = i32SollScaleFactor[drv->dev_nr] - u16ScalfactorStepSoll[drv->dev_nr];
  372.                         }
  373.                         else
  374.                         {
  375.                             i32SollScaleFactor[drv->dev_nr] = drv->od_vars->od_2114_msp_ff_scale_factor;
  376.                         }
  377.                     }
  378.                 }
  379.                 else
  380.                 {
  381.                     i16ScaleFactorCounter[drv->dev_nr] += 2;
  382.                 }
  383.                 i32LastScaleFactor[drv->dev_nr] = i32SollScaleFactor[drv->dev_nr];
  384.             }
  385.             else
  386.             {
  387.                 i16ScaleFactorCounter[drv->dev_nr] = 0;
  388.  
  389.                 i32LastScaleFactor[drv->dev_nr] = drv->od_vars->od_2114_msp_ff_scale_factor;
  390.  
  391.                 i32SollScaleFactor[drv->dev_nr] = drv->od_vars->od_2114_msp_ff_scale_factor;
  392.             }
  393.         }
  394.         else
  395.         {
  396.             i32LastScaleFactor[drv->dev_nr] = drv->od_vars->od_2114_msp_ff_scale_factor;
  397.             i32SollScaleFactor[drv->dev_nr] = drv->od_vars->od_2114_msp_ff_scale_factor;
  398.         }
  399.  
  400.         if (drv->od_vars->od_2100_msp_ff_regulator.ProfUnsym != 0)
  401.         {
  402.             if (drv->od_vars->od_2100_msp_ff_regulator.ProfUnsym > 0)
  403.             {
  404.                 if (i32ffPosition[drv->dev_nr] > 0)
  405.                 {
  406.                     i32SollScaleFactor[drv->dev_nr] *= drv->od_vars->od_2100_msp_ff_regulator.ProfUnsym;
  407.                 }
  408.             }
  409.             else
  410.             {
  411.                 if (i32ffPosition[drv->dev_nr] < 0)
  412.                 {
  413.                     i32SollScaleFactor[drv->dev_nr] *= abs(drv->od_vars->od_2100_msp_ff_regulator.ProfUnsym);
  414.  
  415.                 }
  416.             }
  417.         }
  418.         if (i32SollScaleFactor[drv->dev_nr] > 999)
  419.         {
  420.             i32SollScaleFactor[drv->dev_nr] = 999;
  421.         }
  422.  
  423.  
  424.  
  425.  
  426.  
  427.  
  428.  
  429.  
  430.  
  431.         if (drv->od_vars->od_2101_msp_ff_settings.autopilotMode == 0)
  432.         {
  433.             i32ffPosition[drv->dev_nr] = drv->od_vars->od_6063_p402_position_actual_value + i32SollTrimPosition[drv->dev_nr] + i32DynamicCenter[drv->dev_nr];
  434.         }
  435.         else
  436.         {
  437.             //Autopilot
  438.             i32ffPosition[drv->dev_nr] = drv->od_vars->od_6063_p402_position_actual_value;
  439.         }
  440.  
  441.  
  442.         i32absffPosition = labs(i32ffPosition[drv->dev_nr]);
  443.  
  444.         position_window_half = i32PositionWindowShadow[drv->dev_nr] >> 1;
  445.  
  446.  
  447.         if (i32absffPosition <  i32PositionWindowShadow[drv->dev_nr])
  448.         {
  449.             if (i32PositionWindowShadow[drv->dev_nr] >  (drv->od_vars->od_2119_msp_ff_soft_limit / 250))
  450.             {
  451.                 i32FrictionRegulatorSollValue[drv->dev_nr] = 0;
  452.             }
  453.         }
  454.  
  455.  
  456.  
  457.  
  458.         if (drv->od_vars->od_2107_msp_torque_sensor == 1)
  459.         {
  460.             i16ForceUnfiltered[drv->dev_nr] = drv->od_vars->od_6077_p402_torque_actual_value;
  461.  
  462.  
  463.         }
  464.         else
  465.         {
  466.             if (drv->bEnableExtForceSensor == true)
  467.             {
  468.                 //force_sensor_ext_act = i16ForceUnfiltered[drv->dev_nr]=drv->od_vars->od_209E_msp_force_sensor_ext_sync- co_cmn_od_vars.od_3001_msp_input_voltage_offset[3]; //TODo Offset
  469.  
  470.  
  471.                 i16ForceUnfiltered[drv->dev_nr] = drv->force_sensor_ext_act - co_cmn_od_vars.od_3001_msp_input_voltage_offset[3]; //TODo Offset
  472.  
  473.                 drv->od_vars->od_2033_msp_debug_monitor[8] = i16ForceUnfiltered[drv->dev_nr];
  474.                 if (drv->i16ExtForceSensorTimeOut > 5000)
  475.                 {
  476.                     drv->FFState = 0;
  477.                     drv->FFSensorError = 1;
  478.                     drv->i16ExtForceSensorTimeOut = 0;
  479.  
  480.  
  481.                 }
  482.                 else
  483.                 {
  484.                     drv->i16ExtForceSensorTimeOut += 2;
  485.                 }
  486.             }
  487.             else
  488.             {
  489.                 i16ForceUnfiltered[drv->dev_nr] = 0;
  490.             }
  491.  
  492.  
  493.  
  494.  
  495.  
  496.  
  497.             /*if ((drv->od_vars->od_2201_msp_node_id % 2)==0)
  498.             {
  499.                 if ((drv->od_vars->od_209E_msp_force_sensor_ext_sync!=0)||(i32SensorZeroCounter[drv->dev_nr]==50000))
  500.                 {
  501.  
  502.                     i32SensorZeroCounter[drv->dev_nr]=50000;
  503.                 }
  504.                 else
  505.                 {
  506.                     i16ForceUnfiltered[drv->dev_nr]=0;
  507.                     i32SensorZeroCounter[drv->dev_nr]++;
  508.                     if (i32SensorZeroCounter[drv->dev_nr]>30000)
  509.                     {
  510.                         drv->FFSensorError=1;
  511.                     }
  512.                 }
  513.  
  514.             }
  515.             else
  516.             {
  517.                 if ((drv->od_vars->od_209F_msp_force_sensor_ext!=0)||(i32SensorZeroCounter[drv->dev_nr]==50000))
  518.                 {
  519.                     i16ForceUnfiltered[drv->dev_nr]=drv->od_vars->od_209F_msp_force_sensor_ext- co_cmn_od_vars.od_3001_msp_input_voltage_offset[3]; //TODo Offset
  520.                     i32SensorZeroCounter[drv->dev_nr]=50000;
  521.                 }
  522.                 else
  523.                 {
  524.                     i16ForceUnfiltered[drv->dev_nr]=0;
  525.                     i32SensorZeroCounter[drv->dev_nr]++;
  526.                     if (i32SensorZeroCounter[drv->dev_nr]>30000)
  527.                     {
  528.                         drv->FFSensorError=1;
  529.                     }
  530.                 }
  531.             }*/
  532.         }
  533.         //drv->od_vars->od_2033_msp_debug_monitor[4]=i16ForceUnfiltered[drv->dev_nr];
  534.         //TODo Filter
  535.         /*if (drv->od_vars->od_2033_msp_debug_monitor[1]<1)
  536.         {
  537.             drv->od_vars->od_2033_msp_debug_monitor[1]=1;
  538.         }*/
  539.  
  540.         /*static int StartRolling=0;
  541.         if (drv->od_vars->od_2803_msp_ff_Sensor_Filter>100)
  542.         {
  543.             drv->od_vars->od_2803_msp_ff_Sensor_Filter=100;
  544.         }
  545.  
  546.         i32RollingAverage[i32ForceFilterCounter[drv->dev_nr]]=i16ForceUnfiltered[drv->dev_nr];
  547.         i32ForceCalc[drv->dev_nr] = i32ForceCalc[drv->dev_nr]+i32RollingAverage[i32ForceFilterCounter[drv->dev_nr]];
  548.  
  549.         i32ForceFilterCounter[drv->dev_nr]++;
  550.  
  551.         if (i32ForceFilterCounter[drv->dev_nr]>=drv->od_vars->od_2803_msp_ff_Sensor_Filter)
  552.         {
  553.             StartRolling=1;
  554.             i32ForceFilterCounter[drv->dev_nr]=0;
  555.  
  556.         }*/
  557.         /*if (StartRolling==1)
  558.         {
  559.            i32ForceCalc[drv->dev_nr] = i32ForceCalc[drv->dev_nr] - i32RollingAverage[i32ForceFilterCounter[drv->dev_nr] +1];
  560.            iForceDel[drv->dev_nr]=  i32ForceCalc[drv->dev_nr] / drv->od_vars->od_2803_msp_ff_Sensor_Filter;
  561.         }
  562.         drv->od_vars->od_2033_msp_debug_monitor[1]=iForceDel[drv->dev_nr];
  563.                                                              drv->od_vars->od_2033_msp_debug_monitor[2]=i32ForceCalc[drv->dev_nr] ;
  564.         */
  565.         //drv->od_vars->od_2033_msp_debug_monitor[1]=i32RollingAverage[i32ForceFilterCounter[drv->dev_nr]];
  566.         if (i32ForceFilterCounter[drv->dev_nr] < drv->od_vars->od_2803_msp_ff_Sensor_Filter)
  567.         {
  568.             i32ForceCalc[drv->dev_nr] = i32ForceCalc[drv->dev_nr] + i16ForceUnfiltered[drv->dev_nr];
  569.  
  570.  
  571.  
  572.  
  573.  
  574.  
  575.  
  576.  
  577.  
  578.             /*  if (i16ForceUnfiltered[drv->dev_nr]>iForceDel[drv->dev_nr])
  579.               {
  580.                   drv->od_vars->od_2033_msp_debug_monitor[2]=labs(i16ForceUnfiltered[drv->dev_nr]-iForceDel[drv->dev_nr]);
  581.               }
  582.               else
  583.               {
  584.                   drv->od_vars->od_2033_msp_debug_monitor[2]=labs(iForceDel[drv->dev_nr]-i16ForceUnfiltered[drv->dev_nr]);
  585.               }*/
  586.  
  587.             i32ForceFilterCounter[drv->dev_nr]++;
  588.         }
  589.         else
  590.         {
  591.             //  drv->od_vars->od_2033_msp_debug_monitor[5]=i32ForceCalc[drv->dev_nr];
  592.             iForceDel[drv->dev_nr] =  i32ForceCalc[drv->dev_nr] / i32ForceFilterCounter[drv->dev_nr];
  593.  
  594.             //              drv->od_vars->od_2033_msp_debug_monitor[6]=iForceDel[drv->dev_nr];
  595.  
  596.             i32ForceCalc[drv->dev_nr] = i16ForceUnfiltered[drv->dev_nr];
  597.             i32ForceFilterCounter[drv->dev_nr] = 1;
  598.  
  599.         }
  600.  
  601.  
  602.  
  603.         // drv->od_vars->od_2033_msp_debug_monitor[7]=i32Force[drv->dev_nr];
  604.         i32Force[drv->dev_nr] = (iForceDel[drv->dev_nr] * (int32_t)drv->od_vars->od_2100_msp_ff_regulator.forceScaleNum) / drv->od_vars->od_2100_msp_ff_regulator.forceScaleDenum;
  605.         i32AbsForce[drv->dev_nr] = labs(i32Force[drv->dev_nr]);
  606.  
  607.         if (drv->od_vars->od_2107_msp_torque_sensor == 1)
  608.         {
  609.             drv->od_vars->od_211B_msp_ff_Sensor_Error = drv->od_vars->od_6072_p402_max_torque;
  610.  
  611.  
  612.  
  613.  
  614.  
  615.             if (i32AbsForce[drv->dev_nr] > (drv->od_vars->od_211B_msp_ff_Sensor_Error >> 1))
  616.             {
  617.                 //After 1.5 Minutes
  618.                 if (iOverheatProtectCounter[drv->dev_nr] > 450000)
  619.                 {
  620.                     /*drv->FFState=0;
  621.                     drv->FFSensorError=1;
  622.                     drv->bInitFirstInit=false;
  623.                     drv->AutoFaultReset = 0;
  624.                      */
  625.                     iSetOverheat[drv->dev_nr] = 1;
  626.  
  627.                 }
  628.                 else
  629.                 {
  630.                     iOverheatProtectCounter[drv->dev_nr]++;
  631.                 }
  632.             }
  633.             else
  634.             {
  635.                 if (iOverheatProtectCounter[drv->dev_nr] > 0)
  636.                 {
  637.                     iOverheatProtectCounter[drv->dev_nr]--;
  638.                 }
  639.                 else
  640.                 {
  641.                     iSetOverheat[drv->dev_nr] = 0;
  642.                 }
  643.             }
  644.  
  645.  
  646.             if (i32AbsForce[drv->dev_nr] > (drv->od_vars->od_211B_msp_ff_Sensor_Error + (drv->od_vars->od_211B_msp_ff_Sensor_Error >> 1)) )
  647.             {
  648.                 if (i32ForceMaxCounter[drv->dev_nr] < 2000)
  649.                 {
  650.                     i32ForceMaxCounter[drv->dev_nr]++;
  651.                 }
  652.                 else
  653.                 {
  654.                     drv->FFState = 0;
  655.                     drv->FFSensorError = 1;
  656.                     drv->bInitFirstInit = false;
  657.                     i32ForceMaxCounter[drv->dev_nr] = 0;
  658.                     drv->AutoFaultReset = 0;
  659.  
  660.                 }
  661.             }
  662.             else
  663.             {
  664.                 i32ForceMaxCounter[drv->dev_nr] = 0;
  665.             }
  666.         }
  667.         else
  668.         {
  669.             if (i32AbsForce[drv->dev_nr] > drv->od_vars->od_211B_msp_ff_Sensor_Error)
  670.             {
  671.                 if (i32ForceMaxCounter[drv->dev_nr] < 500)
  672.                 {
  673.                     i32ForceMaxCounter[drv->dev_nr]++;
  674.                 }
  675.                 else
  676.                 {
  677.                     drv->FFState = 0;
  678.                     drv->FFSensorError = 1;
  679.                     drv->bInitFirstInit = false;
  680.                     i32ForceMaxCounter[drv->dev_nr] = 0;
  681.                     drv->AutoFaultReset = 0;
  682.  
  683.                 }
  684.             }
  685.             else
  686.             {
  687.                 i32ForceMaxCounter[drv->dev_nr] = 0;
  688.             }
  689.         }
  690.  
  691.  
  692.  
  693.         if (i32Force[drv->dev_nr] > 0)
  694.         {
  695.             i32Force[drv->dev_nr] = i32Force[drv->dev_nr] * drv->od_vars->od_2100_msp_ff_regulator.offsetDmsCorr / 100;
  696.         }
  697.  
  698.  
  699.  
  700.  
  701.  
  702.         //Force > Friction
  703.         if (labs(i32Force[drv->dev_nr]) > (drv->od_vars->od_211D_msp_friction_regulator[2] << 1))
  704.         {
  705.             i32FrictionRegulatorSollValue[drv->dev_nr] = i32FrictionRegulatorSollValue[drv->dev_nr] >> 1;
  706.         }
  707.         //Soll Position < 100
  708.         if (i32absffPosition < 100)
  709.         {
  710.             i32FrictionRegulatorSollValue[drv->dev_nr] = (i32FrictionRegulatorSollValue[drv->dev_nr] * i32absffPosition) / 100;
  711.         }
  712.  
  713.         if (abs(drv->od_vars->od_606C_p402_velocity_actual_value) > 1)
  714.         {
  715.             i32FrictionRegulatorSollValue[drv->dev_nr] = i32FrictionRegulatorSollValue[drv->dev_nr] / abs(drv->od_vars->od_606C_p402_velocity_actual_value / 2);
  716.  
  717.         }
  718.         //Reduce friction after 200ms non moving
  719.         if (abs(drv->od_vars->od_606C_p402_velocity_actual_value) == 0)
  720.         {
  721.  
  722.             if (iVelZeroCounter[drv->dev_nr] > 1000)
  723.             {
  724.                 i32FrictionRegulatorSollValue[drv->dev_nr] = i32FrictionRegulatorSollValue[drv->dev_nr] >> 1;
  725.             }
  726.             else
  727.             {
  728.                 iVelZeroCounter[drv->dev_nr]++;
  729.             }
  730.  
  731.         }
  732.         else
  733.         {
  734.             if (iVelZeroCounter[drv->dev_nr] > 0)
  735.             {
  736.                 iVelZeroCounter[drv->dev_nr]--;
  737.             }
  738.         }
  739.         if (i32FrictionRegulatorSollValue[drv->dev_nr] > drv->od_vars->od_211D_msp_friction_regulator[2])
  740.         {
  741.             i32FrictionRegulatorSollValue[drv->dev_nr] = drv->od_vars->od_211D_msp_friction_regulator[2];
  742.         }
  743.  
  744.  
  745.  
  746.  
  747.  
  748.  
  749.  
  750.         if ((drv->od_vars->od_211D_msp_friction_regulator[1] == 0) ||
  751.                 (drv->od_vars->od_1002_p301_mnf_status_reg.bit.Neg_soft_limit == 1) ||
  752.                 (drv->od_vars->od_1002_p301_mnf_status_reg.bit.Pos_soft_limit == 1))
  753.         {
  754.             i16FrictionCor[drv->dev_nr] = 0;
  755.         }
  756.  
  757.  
  758.         // when offset Sensor switch off friction
  759.         if (i32OffsetCounter[drv->dev_nr] == -1500)
  760.         {
  761.  
  762.             i32Force[drv->dev_nr] += i16FrictionCor[drv->dev_nr];
  763.  
  764.             //Friction Regulator
  765.             if (i32FrictionCorCounter[drv->dev_nr] < drv->od_vars->od_211D_msp_friction_regulator[5])
  766.             {
  767.                 i32FrictionCorCounter[drv->dev_nr] += 2;
  768.  
  769.             }
  770.             else
  771.             {
  772.                 if (i16FrictionZeroCounter[drv->dev_nr] < 500)
  773.                 {
  774.                     if (i32Force[drv->dev_nr] < 0)
  775.                     {
  776.                         if (i16FrictionCor[drv->dev_nr] < i32FrictionRegulatorSollValue[drv->dev_nr])
  777.                         {
  778.                             i16FrictionCor[drv->dev_nr] += 2;
  779.                         }
  780.                         else
  781.                         {
  782.                             i16FrictionCor[drv->dev_nr] -= 2;
  783.                         }
  784.                     }
  785.                     else
  786.                     {
  787.                         if (i16FrictionCor[drv->dev_nr] > (i32FrictionRegulatorSollValue[drv->dev_nr] * -1))
  788.                         {
  789.                             i16FrictionCor[drv->dev_nr] -= 2;
  790.                         }
  791.                         else
  792.                         {
  793.                             i16FrictionCor[drv->dev_nr] += 2;
  794.                         }
  795.                     }
  796.                 }
  797.                 else
  798.                 {
  799.  
  800.                     if (i16FrictionCor[drv->dev_nr] < i32FrictionRegulatorSollValue[drv->dev_nr])
  801.                     {
  802.                         i16FrictionCor[drv->dev_nr] += 2;
  803.                     }
  804.                     else
  805.                     {
  806.                         i16FrictionCor[drv->dev_nr] -= 2;
  807.                     }
  808.                 }
  809.                 i32FrictionCorCounter[drv->dev_nr] = 0;
  810.  
  811.             }
  812.  
  813.  
  814.  
  815.         }
  816.  
  817.         //I_Regler.i_vect(&I_Regler);
  818.         i32ForceSensor[drv->dev_nr] = i32Force[drv->dev_nr];
  819.  
  820.  
  821.         //If init start force feedback regulator
  822.         if ((drv->FFState == FFSTATERUN) || (drv->FFState == FFSTATEZEROPOS))
  823.         {
  824.  
  825.             switch (drv->od_vars->od_2101_msp_ff_settings.configOffset)
  826.             {
  827.                 case 0:
  828.                 {
  829.                     //2Min Offset Current every 20 sec (Offset drift away most in the first minute of operation enable)
  830.  
  831.                     //do nothing
  832.                     break;
  833.                 }
  834.                 case 1:
  835.                 {
  836.                     //Config offset
  837.                     drv->od_vars->od_607C_p402_home_offset = drv->od_vars->od_607C_p402_home_offset - drv->od_vars->od_6063_p402_position_actual_value;
  838.                     drv->Int_Task.l_user_pos = 0;
  839.                     drv->od_vars->od_2101_msp_ff_settings.configOffset = 2;
  840.                     break;
  841.                 }
  842.                 case 2:
  843.                 {
  844.                     //Set Move negative limit
  845.                     //p402_home_offset=p402_home_offset-drv->od_vars->od_6063_p402_position_actual_value;
  846.                     drv->od_vars->od_2101_msp_ff_settings.autopilotMode = 1;
  847.                     drv->od_vars->od_2117_msp_ff_move_to_position = i32tempNegativePosLimit;
  848.                     drv->od_vars->od_2101_msp_ff_settings.configOffset = 3;
  849.                     drv->od_vars->od_2114_msp_ff_scale_factor = 100;
  850.                     break;
  851.                 }
  852.                 case 3:
  853.                 {
  854.                     //Move negative limit
  855.  
  856.  
  857.  
  858.  
  859.                     if (drv->od_vars->od_6078_p402_current_actual_value >= 3000)
  860.                     {
  861.                         drv->od_vars->od_2101_msp_ff_settings.configOffset = 0;
  862.                         drv->od_vars->od_2101_msp_ff_settings.autopilotMode = 0;
  863.                     }
  864.  
  865.                     if (drv->od_vars->od_6063_p402_position_actual_value < (i32tempNegativePosLimit + 150) )
  866.                     {
  867.                         drv->od_vars->od_2117_msp_ff_move_to_position = drv->od_vars->od_2119_msp_ff_soft_limit;
  868.                         drv->od_vars->od_2101_msp_ff_settings.configOffset = 4;
  869.                     }
  870.  
  871.  
  872.                     break;
  873.                 }
  874.                 case 4:
  875.                 {
  876.  
  877.                     //3A
  878.                     //Error
  879.                     if (drv->od_vars->od_6078_p402_current_actual_value >= 3000)
  880.                     {
  881.                         drv->od_vars->od_2101_msp_ff_settings.configOffset = 0;
  882.                         drv->od_vars->od_2101_msp_ff_settings.autopilotMode = 0;
  883.                     }
  884.                     if (drv->od_vars->od_6063_p402_position_actual_value > (drv->od_vars->od_2119_msp_ff_soft_limit - 150))
  885.                     {
  886.                         drv->od_vars->od_2101_msp_ff_settings.configOffset = 0;
  887.                         drv->od_vars->od_2101_msp_ff_settings.autopilotMode = 0;
  888.                         eeprom_al_store_od_entry_to_eeprom(drv, 0x607C, 0);
  889.                     }
  890.                     break;
  891.                 }
  892.             }
  893.             //Offset sensor function
  894.  
  895.             if ((drv->ForceFeedbackMode.FFMode_Reg.bit.OffsetFFSensor == 1) && (drv->bOffsetAnin0 == false))
  896.             {
  897.                 //Set Master
  898.  
  899.                 drv->ForceFeedbackSyncStateIst.FFSyncState_Reg.bit.State = 1;
  900.                 drv->ForceFeedbackSyncStateIst.FFSyncState_Reg.bit.SetMasterOffset = 0;
  901.                 i16UnitForceOff[drv->dev_nr] = 0;
  902.                 drv->od_vars->od_2097_msp_ff_master_slave_parameter_soll = 0;
  903.  
  904.                 if ( drv->od_vars->od_2107_msp_torque_sensor == 1)
  905.                 {
  906.                     drv->iOffsetCurrentSensors = 3;
  907.                     drv->bOffsetAnin0 = true;
  908.                 }
  909.                 else
  910.                 {
  911.                     if (labs(i32Force[drv->dev_nr]) > 6000)
  912.                     {
  913.                         drv->od_vars->od_6041_p402_statusword = drv->od_vars->od_6041_p402_statusword | 0x1000;
  914.                         drv->bOffsetAnin0 = true;
  915.                     }
  916.                     if (i32OffsetCounter[drv->dev_nr] >= 0)
  917.                     {
  918.  
  919.  
  920.                         if (i32OffsetCounter[drv->dev_nr] < 50)
  921.                         {
  922.  
  923.                             if (drv->ui16WaitOffsetTimer >= drv->od_vars->od_211D_msp_friction_regulator[10])
  924.                             {
  925.                                 //Init Bit off
  926.                                 if ((drv->od_vars->od_6041_p402_statusword & 0x1000) > 0)
  927.                                 {
  928.                                     drv->od_vars->od_6041_p402_statusword = drv->od_vars->od_6041_p402_statusword ^ 0x1000;
  929.                                 }
  930.  
  931.                                 if (i32OffsetCounter[drv->dev_nr] < 1)
  932.                                 {
  933.                                     drv->DisableSync = 1;
  934.                                     drv->ForceFeedbackMode.FFMode_Reg.bit.Turbulence = 0;
  935.                                     i32OffsetCounter[drv->dev_nr]++;
  936.  
  937.                                 }
  938.  
  939.                                 if (abs(drv->od_vars->od_60FF_p402_target_velocity) < 5)
  940.                                 {
  941.  
  942.                                     i32AninSum1[drv->dev_nr] = 0;
  943.                                     i32AninSum2[drv->dev_nr] = 0;
  944.                                     i32OffsetCounter[drv->dev_nr]++;
  945.                                 }
  946.                                 if (drv->od_vars->od_211D_msp_friction_regulator[3] > 1)
  947.                                 {
  948.                                     //Move to zero for offset
  949.                                     if (drv->od_vars->od_6063_p402_position_actual_value > (position_window_half * 2))
  950.                                     {
  951.  
  952.                                         if (i16InverseSensorControl == 1)
  953.                                         {
  954.                                             i32Force[drv->dev_nr] = drv->od_vars->od_211D_msp_friction_regulator[6] * -1;
  955.                                         }
  956.                                         else
  957.                                         {
  958.                                             i32Force[drv->dev_nr] = drv->od_vars->od_211D_msp_friction_regulator[6];
  959.                                         }
  960.                                         i32OffsetCounter[drv->dev_nr] = 1;
  961.  
  962.  
  963.  
  964.                                     }
  965.                                     else
  966.                                     {
  967.                                         if (drv->od_vars->od_6063_p402_position_actual_value < (position_window_half * -2))
  968.                                         {
  969.                                             if (i16InverseSensorControl == 1)
  970.                                             {
  971.                                                 i32Force[drv->dev_nr] = drv->od_vars->od_211D_msp_friction_regulator[6];
  972.                                             }
  973.                                             else
  974.                                             {
  975.                                                 i32Force[drv->dev_nr] = drv->od_vars->od_211D_msp_friction_regulator[6] * -1;
  976.                                             }
  977.  
  978.                                             i32OffsetCounter[drv->dev_nr] = 1;
  979.  
  980.                                         }
  981.                                         else
  982.                                         {
  983.                                             i32Force[drv->dev_nr] = 0;
  984.                                             i32OffsetCounter[drv->dev_nr] = 50;
  985.                                         }
  986.                                     }
  987.  
  988.                                     ui16OffsetTimeOut[drv->dev_nr]++;
  989.                                     if (ui16OffsetTimeOut[drv->dev_nr] > 30000)
  990.                                     {
  991.                                         ui16OffsetTimeOut[drv->dev_nr] = 0;
  992.                                         //TimeOut
  993.                                         i32OffsetCounter[drv->dev_nr] = 50;
  994.                                     }
  995.                                 }
  996.                             }
  997.                         }
  998.                         else
  999.                         {
  1000.  
  1001.                             if (i32OffsetCounter[drv->dev_nr] < 7500)
  1002.                             {
  1003.                                 if (i32OffsetCounter[drv->dev_nr] > 4999)
  1004.                                 {
  1005.  
  1006.                                     i32AninSum1[drv->dev_nr] = i32AninSum1[drv->dev_nr] + (int32_t)drv->force_sensor_ext_act;
  1007.  
  1008.                                 }
  1009.                                 if (i32OffsetCounter[drv->dev_nr] == 7499)
  1010.                                 {
  1011.                                     i32AninSum1[drv->dev_nr] /= 2500;
  1012.  
  1013.                                 }
  1014.  
  1015.                                 if (i32OffsetCounter[drv->dev_nr] < 3500) //&&(drv->Dic.msp_friction_regulator[3]<4))
  1016.                                 {
  1017.                                     if (drv->od_vars->od_211D_msp_friction_regulator[3] < 5)
  1018.                                     {
  1019.                                         if (i32OffsetCounter[drv->dev_nr] < 1500)
  1020.                                         {
  1021.                                             if ((i32OffsetCounter[drv->dev_nr] % 250) == 0)
  1022.                                             {
  1023.                                                 if ( i16OffsetVib[drv->dev_nr] == 0)
  1024.                                                 {
  1025.                                                     i16OffsetVib[drv->dev_nr] = 1;
  1026.  
  1027.                                                 }
  1028.                                                 else
  1029.                                                 {
  1030.                                                     i16OffsetVib[drv->dev_nr] = 0;
  1031.  
  1032.                                                 }
  1033.                                             }
  1034.                                         }
  1035.                                         else
  1036.                                         {
  1037.                                             if ((i32OffsetCounter[drv->dev_nr] % 125) == 0)
  1038.                                             {
  1039.                                                 if ( i16OffsetVib[drv->dev_nr] == 0)
  1040.                                                 {
  1041.                                                     i16OffsetVib[drv->dev_nr] = 1;
  1042.  
  1043.                                                 }
  1044.                                                 else
  1045.                                                 {
  1046.                                                     i16OffsetVib[drv->dev_nr] = 0;
  1047.  
  1048.                                                 }
  1049.                                             }
  1050.                                         }
  1051.                                         if ( i16OffsetVib[drv->dev_nr] == 0)
  1052.                                         {
  1053.  
  1054.                                             i32Force[drv->dev_nr] = (drv->od_vars->od_211D_msp_friction_regulator[6]);
  1055.                                         }
  1056.                                         else
  1057.                                         {
  1058.  
  1059.                                             i32Force[drv->dev_nr] = (drv->od_vars->od_211D_msp_friction_regulator[6] * -1);
  1060.                                         }
  1061.                                     }
  1062.                                     else
  1063.                                     {
  1064.                                         //Dont Vib
  1065.                                         i32OffsetCounter[drv->dev_nr] = 3500;
  1066.                                     }
  1067.  
  1068.                                 }
  1069.                                 else
  1070.                                 {
  1071.                                     if ((drv->od_vars->od_211D_msp_friction_regulator[3] < 4) && (i32OffsetCounter[drv->dev_nr] < 6000))
  1072.                                     {
  1073.                                         i32Force[drv->dev_nr] = drv->od_vars->od_211D_msp_friction_regulator[6];
  1074.                                     }
  1075.                                     else
  1076.                                     {
  1077.                                         i32Force[drv->dev_nr] = 0;
  1078.                                     }
  1079.                                 }
  1080.                             }
  1081.                             else
  1082.                             {
  1083.                                 if (i32OffsetCounter[drv->dev_nr] < 17500)
  1084.                                 {
  1085.                                     if (i32OffsetCounter[drv->dev_nr] > 14999)
  1086.                                     {
  1087.                                         i32AninSum2[drv->dev_nr] = i32AninSum2[drv->dev_nr] + drv->force_sensor_ext_act;
  1088.  
  1089.                                     }
  1090.                                     if ((drv->od_vars->od_211D_msp_friction_regulator[3] < 4) && (i32OffsetCounter[drv->dev_nr] < 16000))
  1091.                                     {
  1092.                                         i32Force[drv->dev_nr] = (drv->od_vars->od_211D_msp_friction_regulator[6] * -1);
  1093.                                     }
  1094.                                     else
  1095.                                     {
  1096.                                         i32Force[drv->dev_nr] = 0;
  1097.                                     }
  1098.                                     if (i32OffsetCounter[drv->dev_nr] == 17499)
  1099.                                     {
  1100.                                         i32AninSum2[drv->dev_nr] /= 2500;
  1101.  
  1102.  
  1103.                                         if (i32AninSum1[drv->dev_nr] < i32AninSum2[drv->dev_nr])
  1104.                                         {
  1105.                                             i32FrictionValue[drv->dev_nr] = ( i32AninSum2[drv->dev_nr] - i32AninSum1[drv->dev_nr]) >> 1;
  1106.  
  1107.  
  1108.                                             if (drv->od_vars->od_211D_msp_friction_regulator[3] == 3)
  1109.                                             {
  1110.                                                 co_cmn_od_vars.od_3001_msp_input_voltage_offset[3] = (int16_t)(i32AninSum1[drv->dev_nr] + (drv->od_vars->od_211D_msp_friction_regulator[2] >> 1));
  1111.                                             }
  1112.                                             else
  1113.                                             {
  1114.                                                 co_cmn_od_vars.od_3001_msp_input_voltage_offset[3] = (int16_t)(i32AninSum1[drv->dev_nr] + i32FrictionValue[drv->dev_nr]);
  1115.                                             }
  1116.  
  1117.                                         }
  1118.                                         else
  1119.                                         {
  1120.                                             i32FrictionValue[drv->dev_nr] = (i32AninSum1[drv->dev_nr] - i32AninSum2[drv->dev_nr]) >> 1;
  1121.                                             if (drv->od_vars->od_211D_msp_friction_regulator[3] == 3)
  1122.                                             {
  1123.                                                 co_cmn_od_vars.od_3001_msp_input_voltage_offset[3] = (int16_t)(i32AninSum2[drv->dev_nr] + (drv->od_vars->od_211D_msp_friction_regulator[2] >> 1));
  1124.                                             }
  1125.                                             else
  1126.                                             {
  1127.                                                 co_cmn_od_vars.od_3001_msp_input_voltage_offset[3] = (int16_t)(i32AninSum2[drv->dev_nr] + i32FrictionValue[drv->dev_nr]);
  1128.                                             }
  1129.                                         }
  1130.                                         if (drv->od_vars->od_211D_msp_friction_regulator[3] == 1)
  1131.                                         {
  1132.                                             drv->od_vars->od_211D_msp_friction_regulator[2] = i32FrictionValue[drv->dev_nr];
  1133.                                             if (drv->od_vars->od_211D_msp_friction_regulator[2] > drv->od_vars->od_211D_msp_friction_regulator[4])
  1134.                                             {
  1135.                                                 drv->od_vars->od_211D_msp_friction_regulator[2] = 0;
  1136.                                             }
  1137.                                         }
  1138.                                         i32AninSum1[drv->dev_nr] = 0;
  1139.                                         i32AninSum2[drv->dev_nr] = 0;
  1140.                                         i32OffsetCounter[drv->dev_nr] = -1500;
  1141.                                         ui16OffsetTimeOut[drv->dev_nr] = 0;
  1142.                                         if (drv->ForceFeedbackMode.FFMode_Reg.bit.PdoAutoInit == 1)
  1143.                                         {
  1144.                                             if (drv->DisableSync == 1)
  1145.                                             {
  1146.                                                 drv->DisableSync = 0;
  1147.                                                 if (drv->od_vars->od_2100_msp_ff_regulator.regulatorType == 2)
  1148.                                                 {
  1149.                                                     i16UnitForceOff[drv->dev_nr] = 1;
  1150.                                                     drv->od_vars->od_2097_msp_ff_master_slave_parameter_soll = 1;
  1151.                                                 }
  1152.                                             }
  1153.                                         }
  1154.  
  1155.                                         //Set init bit
  1156.                                         drv->od_vars->od_6041_p402_statusword = drv->od_vars->od_6041_p402_statusword | 0x1000;
  1157.                                         drv->bOffsetAnin0 = true;
  1158.  
  1159.  
  1160.  
  1161.  
  1162.                                         ret_val |= eeprom_al_store_var_to_eeprom(&co_cmn_od_vars.od_3001_msp_input_voltage_offset[3]);
  1163. #if (CO_DEV_COUNT == 2)
  1164.                                         //ToDo
  1165. #endif
  1166.                                     }
  1167.                                 }
  1168.                             }
  1169.                             if ( drv->bOffsetAnin0 == false)
  1170.                             {
  1171.                                 i32OffsetCounter[drv->dev_nr]++;
  1172.                             }
  1173.                         }
  1174.                     }
  1175.                     else
  1176.                     {
  1177.                         if ( drv->bOffsetAnin0 == false)
  1178.                         {
  1179.                             i32OffsetCounter[drv->dev_nr]++;
  1180.                         }
  1181.                     }
  1182.                 }
  1183.  
  1184.             }
  1185.             else
  1186.             {
  1187.  
  1188.                 drv->ForceFeedbackSyncStateIst.FFSyncState_Reg.bit.State = 0;
  1189.                 if ((i32SollScaleFactor[drv->dev_nr] < drv->od_vars->od_211F_msp_manual_offset_off_pos) && (i16WaitManuelOffsetTimer[drv->dev_nr] > 1000))
  1190.                 {
  1191.                     if (drv->od_vars->od_211A_msp_ff_manual_offset_force > (drv->od_vars->od_211B_msp_ff_Sensor_Error >> 1))
  1192.                     {
  1193.                         drv->od_vars->od_211A_msp_ff_manual_offset_force = (drv->od_vars->od_211B_msp_ff_Sensor_Error >> 1);
  1194.                     }
  1195.  
  1196.  
  1197.  
  1198.                     i32SollTrimPosition[drv->dev_nr] = 0;
  1199.                     //if (drv->od_vars->od_211A_msp_ff_manual_offset_force!=0)
  1200.                     //{
  1201.                     drv->ForceFeedbackSyncStateIst.FFSyncState_Reg.bit.ManualOffset = drv->od_vars->od_211A_msp_ff_manual_offset_force;
  1202.  
  1203.                     //}
  1204.                     //else
  1205.                     //{
  1206.                     //  drv->ForceFeedbackSyncStateIst.FFSyncState_Reg.bit.ManualOffset=drv->ForceFeedbackSyncStateSoll.FFSyncState_Reg.bit.ManualOffset;
  1207.                     //}
  1208.  
  1209.  
  1210.                     if (drv->ForceFeedbackSyncStateSoll.FFSyncState_Reg.bit.SetMasterOffset == false)
  1211.                     {
  1212.                         if (drv->od_vars->od_211A_msp_ff_manual_offset_force != 0)
  1213.                         {
  1214.                             i16ff_manual_offset_force = drv->od_vars->od_211A_msp_ff_manual_offset_force;
  1215.  
  1216.                             if (drv->od_vars->od_2100_msp_ff_regulator.regulatorType == 0)
  1217.                             {
  1218.                                 if (abs(drv->ForceFeedbackSyncStateSoll.FFSyncState_Reg.bit.ManualOffset) > 0)
  1219.                                 {
  1220.                                     i16ff_manual_offset_force = 0;
  1221.                                     drv->ForceFeedbackSyncStateIst.FFSyncState_Reg.bit.ManualOffset = 0;
  1222.  
  1223.  
  1224.                                 }
  1225.                             }
  1226.  
  1227.  
  1228.                         }
  1229.                         else
  1230.                         {
  1231.                             i16ManualOffsetForceSoll[drv->dev_nr] = 0;
  1232.                             i16ff_manual_offset_force = drv->ForceFeedbackSyncStateSoll.FFSyncState_Reg.bit.ManualOffset;
  1233.  
  1234.                         }
  1235.  
  1236.                         i16ManualOffsetForceCalc[drv->dev_nr] = ((int32_t)i16ff_manual_offset_force * (drv->od_vars->od_211F_msp_manual_offset_off_pos - i32SollScaleFactor[drv->dev_nr])) / (drv->od_vars->od_211F_msp_manual_offset_off_pos - 20);
  1237.                         if (i16InverseSensorControl == 0)
  1238.                         {
  1239.                             i16ManualOffsetForceCalc[drv->dev_nr] *= -1;
  1240.                         }
  1241.  
  1242.                         if (i16ff_manual_offset_forceLast[drv->dev_nr] == 0)
  1243.                         {
  1244.                             //Switch from Slave to master
  1245.                             //i16ManualOffsetForceSoll[drv->dev_nr]= i16ManualOffsetForceCalc[drv->dev_nr];
  1246.                             i16ManualOffsetForceSoll[drv->dev_nr] = 0;
  1247.                         }
  1248.                         else
  1249.                         {
  1250.                             if (i16ManualOffsetChangeCounter[drv->dev_nr] > 3)
  1251.                             {
  1252.  
  1253.  
  1254.  
  1255.                                 if (i16ManualOffsetForceSoll[drv->dev_nr] < i16ManualOffsetForceCalc[drv->dev_nr])
  1256.                                 {
  1257.  
  1258.  
  1259.                                     i16ManualOffsetForceSoll[drv->dev_nr]++;
  1260.                                 }
  1261.                                 else
  1262.                                 {
  1263.  
  1264.                                     i16ManualOffsetForceSoll[drv->dev_nr]--;
  1265.  
  1266.                                 }
  1267.                                 i16ManualOffsetChangeCounter[drv->dev_nr] = 0;
  1268.  
  1269.                             }
  1270.                             else
  1271.                             {
  1272.                                 i16ManualOffsetChangeCounter[drv->dev_nr]++;
  1273.                             }
  1274.                         }
  1275.                         i16ff_manual_offset_forceLast[drv->dev_nr] = i16ff_manual_offset_force;
  1276.                     }
  1277.                     else
  1278.                     {
  1279.  
  1280.                         if (drv->ForceFeedbackSyncStateIst.FFSyncState_Reg.bit.SetMasterOffset == true)
  1281.                         {
  1282.                             drv->ForceFeedbackSyncStateIst.FFSyncState_Reg.bit.SetMasterOffset = false;
  1283.                         }
  1284.                         i16ff_manual_offset_force = 0;
  1285.                         i16ff_manual_offset_forceLast[drv->dev_nr] = i16ff_manual_offset_force;
  1286.                         i16ManualOffsetForceSoll[drv->dev_nr] = 0;
  1287.  
  1288.  
  1289.                     }
  1290.  
  1291.  
  1292.  
  1293.  
  1294.                     if (i16ManualOffsetForceSoll[drv->dev_nr] != 0)
  1295.                     {
  1296.                         if (( drv->ForceFeedbackSyncStateSoll.FFSyncState_Reg.bit.SetMasterOffset == false) && (drv->od_vars->od_2100_msp_ff_regulator.regulatorType == 2))
  1297.                         {
  1298.  
  1299.  
  1300.                             drv->ForceFeedbackSyncStateIst.FFSyncState_Reg.bit.SetMasterOffset = true;
  1301.                         }
  1302.                     }
  1303.  
  1304.  
  1305.                     //only elevator
  1306.                     /* if (drv->dev_nr==0)
  1307.                      {
  1308.                          i16ManualOffsetForce[drv->dev_nr]=i16ManualOffsetForceSoll[drv->dev_nr]+((i32Force[drv->dev_nr])>>1);
  1309.                      }
  1310.                      else
  1311.                      {*/
  1312.                     i16ManualOffsetForce[drv->dev_nr] = i16ManualOffsetForceSoll[drv->dev_nr];
  1313.                     //}
  1314.  
  1315.  
  1316.                     if (labs(i16ManualOffsetForceSoll[drv->dev_nr]) > 0)
  1317.                     {
  1318.                         if (i16ManualOffsetForceSoll[drv->dev_nr] > 0)
  1319.                         {
  1320.                             if (i16ManualOffsetForce[drv->dev_nr] < 0)
  1321.                             {
  1322.                                 i16ManualOffsetForce[drv->dev_nr] = 0;
  1323.                             }
  1324.                         }
  1325.                         else
  1326.                         {
  1327.                             if (i16ManualOffsetForce[drv->dev_nr] > 0)
  1328.                             {
  1329.                                 i16ManualOffsetForce[drv->dev_nr] = 0;
  1330.                             }
  1331.                         }
  1332.                     }
  1333.                     else
  1334.                     {
  1335.                         i16ManualOffsetForce[drv->dev_nr] = 0;
  1336.                     }
  1337.  
  1338.  
  1339.                     if (i16UnitForceOff[drv->dev_nr] == 1)
  1340.                     {
  1341.                         drv->ForceFeedbackSyncStateIst.FFSyncState_Reg.bit.SetMasterOffset = false;
  1342.                     }
  1343.                 }
  1344.                 else
  1345.                 {
  1346.                     if (drv->od_vars->od_211A_msp_ff_manual_offset_force == 0)
  1347.                     {
  1348.                         i16ff_manual_offset_force = drv->ForceFeedbackSyncStateSoll.FFSyncState_Reg.bit.ManualOffset;
  1349.                     }
  1350.                     else
  1351.                     {
  1352.                         i16ff_manual_offset_force = drv->od_vars->od_211A_msp_ff_manual_offset_force;
  1353.                     }
  1354.                     i16ManualOffsetForceSoll[drv->dev_nr] = 0;
  1355.                     i16ManualOffsetForce[drv->dev_nr] = 0;
  1356.                     drv->ForceFeedbackSyncStateIst.FFSyncState_Reg.bit.SetMasterOffset = false;
  1357.                     if (i16WaitManuelOffsetTimer[drv->dev_nr] <= 1000)
  1358.                     {
  1359.                         i32SollScaleFactor[drv->dev_nr] = 100;
  1360.                         i16WaitManuelOffsetTimer[drv->dev_nr] += 2;
  1361.                     }
  1362.                 }
  1363.  
  1364.  
  1365.  
  1366.  
  1367.  
  1368.                 i32Force[drv->dev_nr] += i16ManualOffsetForce[drv->dev_nr];
  1369.                 if (drv->DisableSync == 1)
  1370.                 {
  1371.                     //VirtualComSdoNmt(0,0x1,0,0,0);
  1372.                     drv->DisableSync = 0;
  1373.                 }
  1374.                 //Offset Sensor aborted
  1375.                 if (i32OffsetCounter[drv->dev_nr] != -1500)
  1376.                 {
  1377.                     //Set init bit
  1378.                     drv->od_vars->od_6041_p402_statusword = drv->od_vars->od_6041_p402_statusword | 0x1000;
  1379.                     i32OffsetCounter[drv->dev_nr] = -1500;
  1380.                 }
  1381.                 //If set to slave force always 0
  1382.                 if (drv->ForceFeedbackMode.FFMode_Reg.bit.SyncMode != 3)
  1383.                 {
  1384.                     if ((drv->ForceFeedbackMode.FFMode_Reg.bit.SyncMode & 0x1) == true)
  1385.                     {
  1386.                         i32Force[drv->dev_nr] = 0;
  1387.                         drv->od_vars->od_2097_msp_ff_master_slave_parameter_soll = 1;
  1388.                     }
  1389.                     else
  1390.                     {
  1391.                         MasterSlaveSync(i16ManualOffsetForce[drv->dev_nr], drv);
  1392.                     }
  1393.                 }
  1394.  
  1395.             }
  1396.  
  1397.  
  1398.             if (drv->FFState == FFSTATEZEROPOS)
  1399.             {
  1400.                 ui32MaxVelocity = 300;
  1401.                 if (i32FFPosWaitCounter[drv->dev_nr] < 30000)
  1402.                 {
  1403.                     i32FFPosWaitCounter[drv->dev_nr] += 2;
  1404.                 }
  1405.                 else
  1406.                 {
  1407.                     i32FFPosWaitCounter[drv->dev_nr] = 0;
  1408.                     drv->FFState = FFSTATERUN;
  1409.                 }
  1410.             }
  1411.             else
  1412.             {
  1413.                 if (drv->od_vars->od_6080_p402_max_motor_speed > drv->od_vars->od_607F_p402_max_profile_velocity)
  1414.                 {
  1415.                     ui32MaxVelocity = drv->od_vars->od_607F_p402_max_profile_velocity;
  1416.                 }
  1417.                 else
  1418.                 {
  1419.                     ui32MaxVelocity = drv->od_vars->od_6080_p402_max_motor_speed;
  1420.                 }
  1421.             }
  1422.             if (i16InverseSensorControl == 1)
  1423.             {
  1424.                 i32Force[drv->dev_nr] *= -1;
  1425.             }
  1426.  
  1427.  
  1428.             if (drv->ForceFeedbackMode.FFMode_Reg.bit.Force_Linear_Up == 1)
  1429.             {
  1430.                 iStep1 = drv->od_vars->od_2119_msp_ff_soft_limit >> 3;
  1431.                 if (drv->od_vars->od_2100_msp_ff_regulator.profileMode == 2)
  1432.                 {
  1433.                     iStep2 = labs(drv->od_vars->od_209C_msp_neg_software_limit) >> 3;
  1434.                 }
  1435.                 else
  1436.                 {
  1437.                     if (drv->od_vars->od_2100_msp_ff_regulator.profileMode == 1)
  1438.                     {
  1439.                         iStep2 = labs(drv->od_vars->od_209C_msp_neg_software_limit) >> 3;
  1440.                         if (iStep2 > iStep1)
  1441.                         {
  1442.                             iStep1 = iStep2;
  1443.                         }
  1444.                         else
  1445.                         {
  1446.                             iStep2 = iStep1;
  1447.                         }
  1448.                     }
  1449.                     else
  1450.                     {
  1451.                         iStep2 = iStep1;
  1452.                     }
  1453.                 }
  1454.  
  1455.  
  1456.                 if (drv->od_vars->od_20A1_msp_ff_force_profile_position[1] > 0)
  1457.                 {
  1458.                     for (i = 9; i > 0; i--)
  1459.                     {
  1460.                         if (i32absffPosition >= drv->od_vars->od_20A1_msp_ff_force_profile_position[i])
  1461.                         {
  1462.                             iTorqueValue = i + 1;
  1463.                             if (i < 9)
  1464.                             {
  1465.                                 i32StepActual = drv->od_vars->od_20A1_msp_ff_force_profile_position[iTorqueValue] - drv->od_vars->od_20A1_msp_ff_force_profile_position[i];
  1466.                             }
  1467.                             else
  1468.                             {
  1469.                                 i32StepActual = drv->od_vars->od_20A1_msp_ff_force_profile_position[i] - drv->od_vars->od_20A1_msp_ff_force_profile_position[i - 1];
  1470.                             }
  1471.                             i = 0;
  1472.                             /*i32StepActual=iStep1;
  1473.                             iStep2= iStep1;
  1474.                             i=0;*/
  1475.                         }
  1476.                         else
  1477.                         {
  1478.                             iTorqueValue = 1;
  1479.                             i32StepActual = drv->od_vars->od_20A1_msp_ff_force_profile_position[1];
  1480.                             /* iStep1 = drv->od_vars->od_20A1_msp_ff_force_profile_position[1];
  1481.                              i32StepActual=iStep1;
  1482.                              iStep2= iStep1;*/
  1483.                         }
  1484.                     }
  1485.                 }
  1486.                 else
  1487.                 {
  1488.                     if (i32ffPosition[drv->dev_nr] >= 0)
  1489.                     {
  1490.                         i32StepActual = iStep1;
  1491.                     }
  1492.                     else
  1493.                     {
  1494.                         i32StepActual = iStep2;
  1495.                     }
  1496.                     if (i32StepActual == 0)
  1497.                     {
  1498.                         i32StepActual = 1;
  1499.                     }
  1500.                     iTorqueValue = (i32absffPosition / i32StepActual) + 1;
  1501.                 }
  1502.                 if (iTorqueValue > 8)
  1503.                 {
  1504.                     iTorqueValue = 8;
  1505.                 }
  1506.                 drv->od_vars->od_2033_msp_debug_monitor[1] = iTorqueValue;
  1507.                 drv->od_vars->od_2033_msp_debug_monitor[2] = i32StepActual;
  1508.  
  1509.                 if (iTorqueValue == 1)
  1510.                 {
  1511.                     if (i32SollScaleFactor[drv->dev_nr] < 100)
  1512.                     {
  1513.                         i32TorqueValue1 = ((int32_t)drv->od_vars->od_2113_msp_ff_force_profile[iTorqueValue] * (int32_t)i32SollScaleFactor[drv->dev_nr]) / 100;
  1514.  
  1515.                     }
  1516.                     else
  1517.                     {
  1518.                         i32TorqueValue1 = drv->od_vars->od_2113_msp_ff_force_profile[iTorqueValue];
  1519.                     }
  1520.                 }
  1521.                 else
  1522.                 {
  1523.                     if (iTorqueValue == 2)
  1524.                     {
  1525.                         if (i32SollScaleFactor[drv->dev_nr] > 200)
  1526.                         {
  1527.                             i32temp = 200;
  1528.  
  1529.                         }
  1530.                         else
  1531.                         {
  1532.                             i32temp = i32SollScaleFactor[drv->dev_nr];
  1533.                         }
  1534.                         i32TorqueValue1 = ((int32_t)drv->od_vars->od_2113_msp_ff_force_profile[iTorqueValue] * i32temp) / 100;
  1535.                     }
  1536.                     else
  1537.                     {
  1538.                         i32TorqueValue1 = ((int32_t)drv->od_vars->od_2113_msp_ff_force_profile[iTorqueValue] * (int32_t)i32SollScaleFactor[drv->dev_nr]) / 100;
  1539.                     }
  1540.                 }
  1541.  
  1542.                 if (iTorqueValue == 1)
  1543.                 {
  1544.                     if (i32SollScaleFactor[drv->dev_nr] > 200)
  1545.                     {
  1546.                         i32temp = 200;
  1547.                     }
  1548.                     else
  1549.                     {
  1550.                         i32temp = i32SollScaleFactor[drv->dev_nr];
  1551.                     }
  1552.                 }
  1553.                 else
  1554.                 {
  1555.                     i32temp = i32SollScaleFactor[drv->dev_nr];
  1556.                 }
  1557.  
  1558.                 i32TorqueValue2 = ((int32_t)drv->od_vars->od_2113_msp_ff_force_profile[iTorqueValue + 1] * i32temp) / 100;
  1559.                 if (i32TorqueValue1 < drv->od_vars->od_6071_p402_target_torque)
  1560.                 {
  1561.                     i32TorqueValue1 = drv->od_vars->od_6071_p402_target_torque;
  1562.                 }
  1563.                 if (i32TorqueValue2 < drv->od_vars->od_6071_p402_target_torque)
  1564.                 {
  1565.                     i32TorqueValue2 = drv->od_vars->od_6071_p402_target_torque;
  1566.                 }
  1567.                 iDiff = i32TorqueValue2 - i32TorqueValue1;
  1568.                 //drv->od_vars->od_2033_msp_debug_monitor[3]=iDiff;
  1569.                 //drv->od_vars->od_2033_msp_debug_monitor[4]=i32TorqueValue1;
  1570.                 //drv->od_vars->od_2033_msp_debug_monitor[5]= i32TorqueValue2;
  1571.  
  1572.                 if (iDiff == 0)
  1573.                 {
  1574.                     iTargetTorqueShadow = i32TorqueValue1;
  1575.                 }
  1576.                 else
  1577.                 {
  1578.                     i32SmoothPos = (i32PositionWindowShadow[drv->dev_nr] * 2) - i32absffPosition;
  1579.                     if (i32SmoothPos <= 0)
  1580.                     {
  1581.                         i32SmoothPos = 0;
  1582.                     }
  1583.                     if (drv->od_vars->od_20A1_msp_ff_force_profile_position[1] == 0)
  1584.                     {
  1585.                         iTargetTorqueShadow = i32TorqueValue1 + ((i32absffPosition - i32SmoothPos - (i32StepActual * (iTorqueValue - 1))) * iDiff) / i32StepActual;
  1586.                     }
  1587.                     else
  1588.                     {
  1589.                         iTargetTorqueShadow = i32TorqueValue1 + (iDiff * (i32absffPosition - i32SmoothPos - drv->od_vars->od_20A1_msp_ff_force_profile_position[iTorqueValue])) / i32StepActual;
  1590.                     }
  1591.                 }
  1592.                 // drv->od_vars->od_2033_msp_debug_monitor[7]= iTargetTorqueShadow;
  1593.                 // drv->od_vars->od_2033_msp_debug_monitor[6]= i32SmoothPos;
  1594.  
  1595.                 if (i32absffPosition <  i32PositionWindowShadow[drv->dev_nr])
  1596.                 {
  1597.  
  1598.                     i32temp = i32PositionWindowShadow[drv->dev_nr] - (i32PositionWindowShadow[drv->dev_nr] >> 2);
  1599.                     if (i32absffPosition > i32temp)
  1600.                     {
  1601.                         iTargetTorqueShadow = drv->od_vars->od_6071_p402_target_torque + ((i32TorqueValue1 - drv->od_vars->od_6071_p402_target_torque) * (i32absffPosition - i32temp)) / (i32PositionWindowShadow[drv->dev_nr] >> 2); //(i32TorqueValue1*i32absffPosition)/i32PositionWindowShadow-();
  1602.                     }
  1603.                     else
  1604.                     {
  1605.                         iTargetTorqueShadow = drv->od_vars->od_6071_p402_target_torque;
  1606.  
  1607.                         iTargetTorqueShadow = drv->od_vars->od_6071_p402_target_torque - (i32PositionWindowShadow[drv->dev_nr] - i32absffPosition);
  1608.                         if (iTargetTorqueShadow < 0)
  1609.                         {
  1610.                             iTargetTorqueShadow = 0;
  1611.                         }
  1612.                     }
  1613.                 }
  1614.  
  1615.                 if (i32SollScaleFactor[drv->dev_nr] < drv->od_vars->od_211F_msp_manual_offset_off_pos)
  1616.                 {
  1617.                     if (iTargetTorqueShadow > drv->od_vars->od_6071_p402_target_torque)
  1618.                     {
  1619.                         iTargetTorqueShadow = ((iTargetTorqueShadow - drv->od_vars->od_6071_p402_target_torque) * (i32SollScaleFactor[drv->dev_nr] - 19)) / (drv->od_vars->od_211F_msp_manual_offset_off_pos - 19);
  1620.                     }
  1621.                 }
  1622.  
  1623.  
  1624.  
  1625.                 if (iTargetTorqueShadow < drv->od_vars->od_6071_p402_target_torque)
  1626.                 {
  1627.                     iTargetTorqueShadow = drv->od_vars->od_6071_p402_target_torque;
  1628.                 }
  1629.                 drv->od_vars->od_2093_msp_ff_virtual_force = iTargetTorqueShadow;
  1630.  
  1631.             }
  1632.             else
  1633.             {
  1634.  
  1635.                 //if (drv->FFsyncRunning==true)
  1636.                 //{
  1637.                 /*      if (labs(drv->od_vars->od_2103_msp_sync_torqu_soll) > labs(drv->od_vars->od_6071_p402_target_torque))
  1638.                         {
  1639.                             iTargetTorqueShadow=drv->od_vars->od_2103_msp_sync_torqu_soll*-1;
  1640.                             drv->od_vars->od_6071_p402_target_torque = iTargetTorqueShadow;
  1641.                         }
  1642.                         else
  1643.                         {
  1644.                             iTargetTorqueShadow=drv->od_vars->od_6071_p402_target_torque;
  1645.                         }*/
  1646.                 //}
  1647.                 //else
  1648.                 //{
  1649.                 //  iTargetTorqueShadow=drv->od_vars->od_6071_p402_target_torque;
  1650.                 //}
  1651.  
  1652.                 iTargetTorqueShadow = drv->od_vars->od_6071_p402_target_torque;
  1653.  
  1654.                 if ((drv->ForceFeedbackMode.FFMode_Reg.bit.Move_Back == 1) && (drv->ForceFeedbackMode.FFMode_Reg.bit.EXT_Control == 1))
  1655.                 {
  1656.                     if (iTargetTorqueShadow < 0)
  1657.                     {
  1658.                         iTargetTorqueShadow *= -1;
  1659.                     }
  1660.                 }
  1661.  
  1662.                 drv->od_vars->od_2093_msp_ff_virtual_force = iTargetTorqueShadow;
  1663.             }
  1664.             i32temp = Drive[0].od_vars->od_6072_p402_max_torque - (Drive[0].od_vars->od_6072_p402_max_torque / 7);
  1665.  
  1666.             if (iTargetTorqueShadow >   i32temp)
  1667.             {
  1668.                 iTargetTorqueShadow = i32temp;
  1669.             }
  1670.  
  1671.  
  1672.  
  1673.  
  1674.             if (drv->od_vars->od_2101_msp_ff_settings.autopilotMode == 1)
  1675.             {
  1676.  
  1677.                 if (drv->od_vars->od_2118_msp_ff_move_to_position_force > drv->od_vars->od_6072_p402_max_torque)
  1678.                 {
  1679.                     drv->od_vars->od_2118_msp_ff_move_to_position_force = drv->od_vars->od_6072_p402_max_torque;
  1680.                 }
  1681.                 iTargetTorqueShadow = drv->od_vars->od_2118_msp_ff_move_to_position_force;
  1682.             }
  1683.  
  1684.             //BRS NEW
  1685.             if (((drv->od_vars->od_2100_msp_ff_regulator.MiscOptions >> 1) & 0x1) == 0)
  1686.             {
  1687.  
  1688.                 if ((drv->od_vars->od_1002_p301_mnf_status_reg.bit.Neg_soft_limit == 1) || (drv->od_vars->od_1002_p301_mnf_status_reg.bit.Pos_soft_limit == 1))
  1689.                 {
  1690.  
  1691.  
  1692.                     if (drv->od_vars->od_1002_p301_mnf_status_reg.bit.Neg_soft_limit == 1)
  1693.                     {
  1694.                         i32temp = iTargetTorqueShadow + ((i32absffPosition + i32tempNegativePosLimit) << drv->od_vars->od_2100_msp_ff_regulator.softLimitStop);
  1695.                     }
  1696.                     else
  1697.                     {
  1698.                         i32temp = iTargetTorqueShadow + ((i32absffPosition - drv->od_vars->od_2119_msp_ff_soft_limit) << drv->od_vars->od_2100_msp_ff_regulator.softLimitStop);
  1699.                     }
  1700.                     if (i32temp > 0x7FFF)
  1701.                     {
  1702.                         i32temp = 0x7FFF;
  1703.                     }
  1704.  
  1705.                     iTargetTorqueShadow = i32temp;
  1706.                     if (iTargetTorqueShadow > drv->od_vars->od_6072_p402_max_torque)
  1707.                     {
  1708.  
  1709.                         iTargetTorqueShadow = drv->od_vars->od_6072_p402_max_torque;
  1710.  
  1711.                     }
  1712.  
  1713.  
  1714.  
  1715.                     i32SollScaleFactor[drv->dev_nr] = 100;
  1716.  
  1717.  
  1718.                 }
  1719.  
  1720.  
  1721.                 if (drv->od_vars->od_2117_msp_ff_move_to_position < i32tempNegativePosLimit)
  1722.                 {
  1723.  
  1724.                     drv->od_vars->od_2117_msp_ff_move_to_position = i32tempNegativePosLimit;
  1725.                 }
  1726.  
  1727.                 else
  1728.                 {
  1729.                     if (drv->od_vars->od_2117_msp_ff_move_to_position > drv->od_vars->od_2119_msp_ff_soft_limit)
  1730.                     {
  1731.                         drv->od_vars->od_2117_msp_ff_move_to_position = drv->od_vars->od_2119_msp_ff_soft_limit;
  1732.                     }
  1733.  
  1734.                 }
  1735.  
  1736.             }
  1737. #ifndef FORCEREDUCEOFF
  1738.             if (drv->od_vars->od_2096_msp_ff_master_slave_parameter_ist != 2)
  1739.             {
  1740.  
  1741.                 if (i32OverTime[drv->dev_nr] > 40000)
  1742.                 {
  1743.                     ui16PeakTorque[drv->dev_nr] = drv->od_vars->od_6072_p402_max_torque / 2;
  1744.                     bReducePeak[drv->dev_nr] = true;
  1745.                     i32OverTime[drv->dev_nr] = 15000;
  1746.  
  1747.                 }
  1748.                 else
  1749.                 {
  1750.                     if (i32OverTime[drv->dev_nr] < 1000)
  1751.                     {
  1752.                         ui16PeakTorque[drv->dev_nr] = drv->od_vars->od_6072_p402_max_torque + (drv->od_vars->od_6072_p402_max_torque >> 3);
  1753.                         bReducePeak[drv->dev_nr] = false;
  1754.  
  1755.                     }
  1756.                 }
  1757.             }
  1758. #else
  1759.             if (iSetOverheat[drv->dev_nr] == 0)
  1760.             {
  1761.                 ui16PeakTorque[drv->dev_nr] = drv->od_vars->od_6072_p402_max_torque;
  1762.             }
  1763.             else
  1764.             {
  1765.                 ui16PeakTorque[drv->dev_nr] = drv->od_vars->od_6072_p402_max_torque / 3;
  1766.             }
  1767. #endif
  1768.             if ((abs(iTargetTorqueShadow) > drv->od_vars->od_6072_p402_max_torque) ||
  1769.                     (abs(drv->od_vars->od_6077_p402_torque_actual_value) > drv->od_vars->od_6072_p402_max_torque))
  1770.             {
  1771.                 //Autopilot Off when overpress (protect the motor)
  1772.                 drv->od_vars->od_2101_msp_ff_settings.autopilotMode = 0;
  1773.                 if (i32OverTime[drv->dev_nr] < 41000)
  1774.                 {
  1775.                     i32OverTime[drv->dev_nr] += 2;
  1776.                 }
  1777.             }
  1778.             else
  1779.             {
  1780.                 if (drv->od_vars->od_2096_msp_ff_master_slave_parameter_ist == 2)
  1781.                 {
  1782.                     bReducePeak[drv->dev_nr] = false;
  1783.                     i32OverTime[drv->dev_nr] = 0;
  1784.  
  1785.                 }
  1786.                 if (i32OverTime[drv->dev_nr] > 0)
  1787.                 {
  1788.                     i32OverTime[drv->dev_nr]--;
  1789.                 }
  1790.             }
  1791.  
  1792.             if (abs(iTargetTorqueShadow) > ui16PeakTorque[drv->dev_nr])
  1793.             {
  1794.                 if (iTargetTorqueShadow >= 0)
  1795.                 {
  1796.                     iTargetTorqueShadow = ui16PeakTorque[drv->dev_nr];
  1797.                 }
  1798.                 else
  1799.                 {
  1800.                     iTargetTorqueShadow = (int16_t)ui16PeakTorque[drv->dev_nr] * -1;
  1801.                 }
  1802.  
  1803.             }
  1804.             if ((abs(drv->od_vars->od_6077_p402_torque_actual_value) > drv->od_vars->od_6072_p402_max_torque) && (abs(drv->od_vars->od_6077_p402_torque_actual_value) > ui16PeakTorque[drv->dev_nr] + 500))
  1805.             {
  1806.  
  1807.                 if (iForceErrorCounter[drv->dev_nr] < 8000)
  1808.                 {
  1809.                     iForceErrorCounter[drv->dev_nr]++;
  1810.                 }
  1811.                 else
  1812.                 {
  1813.                     drv->FFSensorError = 1;
  1814.  
  1815.                     drv->bInitFirstInit = false;
  1816.                     iForceErrorCounter[drv->dev_nr] = 0;
  1817.                 }
  1818.  
  1819.             }
  1820.             else
  1821.             {
  1822.                 iForceErrorCounter[drv->dev_nr] = 0;
  1823.             }
  1824.  
  1825.  
  1826.  
  1827.             if ((labs(i32ForceSensor[drv->dev_nr]) > (ui16PeakTorque[drv->dev_nr] + (ui16PeakTorque[drv->dev_nr] >> 2)) ) && (drv->od_vars->od_2107_msp_torque_sensor == 1))
  1828.             {
  1829.                 //Turbulence Off
  1830.                 i16OverForce = 1;
  1831.             }
  1832.             else
  1833.             {
  1834.                 i16OverForce = 0;
  1835.             }
  1836.  
  1837.  
  1838.             if (i32OffsetCounter[drv->dev_nr] >= 0)
  1839.             {
  1840.                 iTargetTorqueShadow = drv->od_vars->od_211D_msp_friction_regulator[6] >> 1;
  1841.             }
  1842.  
  1843.  
  1844.  
  1845.             switch (drv->ForceFeedbackMode.FFMode_Reg.bit.Turbulence)
  1846.             {
  1847.                 case 0:
  1848.                 {
  1849.                     iTurbulenceTime[drv->dev_nr] = drv->od_vars->od_2109_msp_turbulence_time;
  1850.                     iTurbulenceTime2[drv->dev_nr] = drv->od_vars->od_209A_msp_turbulence_time2;
  1851.                     iTurbulenceForce[drv->dev_nr] = 0;
  1852.                     iTurbulenceForce2[drv->dev_nr] = 0;
  1853.  
  1854.                     break;
  1855.                 }
  1856.                 case 1:
  1857.                 {
  1858.                     if (iTurbulenceTime[drv->dev_nr] >= 1)
  1859.                     {
  1860.  
  1861.                         if (labs(drv->od_vars->od_2108_msp_turbulence_force) > (drv->od_vars->od_211B_msp_ff_Sensor_Error >> 1))
  1862.                         {
  1863.                             if (drv->od_vars->od_2108_msp_turbulence_force > 0)
  1864.                             {
  1865.                                 drv->od_vars->od_2108_msp_turbulence_force = drv->od_vars->od_211B_msp_ff_Sensor_Error >> 1;
  1866.                             }
  1867.                             else
  1868.                             {
  1869.                                 drv->od_vars->od_2108_msp_turbulence_force = (int16_t)(drv->od_vars->od_211B_msp_ff_Sensor_Error >> 1) * -1;
  1870.                             }
  1871.                         }
  1872.                         iTurbulenceForce[drv->dev_nr] = drv->od_vars->od_2108_msp_turbulence_force;
  1873.  
  1874.                     }
  1875.                     else
  1876.                     {
  1877.                         drv->ForceFeedbackMode.FFMode_Reg.bit.Turbulence = 0;
  1878.                         iTurbulenceForce[drv->dev_nr] = 0;
  1879.                     }
  1880.                     iTurbulenceTime[drv->dev_nr] -= 2;
  1881.                     break;
  1882.                 }
  1883.                 case 2:
  1884.                 {
  1885.  
  1886.  
  1887.                     if (drv->od_vars->od_2108_msp_turbulence_force < 0)
  1888.                     {
  1889.                         drv->od_vars->od_2108_msp_turbulence_force *= -1;
  1890.                     }
  1891.                     if (drv->od_vars->od_2099_msp_turbulence_force2 < 0)
  1892.                     {
  1893.                         drv->od_vars->od_2099_msp_turbulence_force2 *= -1;
  1894.                     }
  1895.                     if (drv->od_vars->od_2108_msp_turbulence_force > (drv->od_vars->od_211B_msp_ff_Sensor_Error >> 1))
  1896.                     {
  1897.                         drv->od_vars->od_2108_msp_turbulence_force = drv->od_vars->od_211B_msp_ff_Sensor_Error >> 1;
  1898.                     }
  1899.                     if (drv->od_vars->od_2099_msp_turbulence_force2 > (drv->od_vars->od_607F_p402_max_profile_velocity >> 2))
  1900.                     {
  1901.                         drv->od_vars->od_2099_msp_turbulence_force2 = drv->od_vars->od_607F_p402_max_profile_velocity >> 2;
  1902.                     }
  1903.                     if (iTurbulenceTime[drv->dev_nr] >= 1)
  1904.                     {
  1905.                         iTurbulenceForce[drv->dev_nr] = drv->od_vars->od_2108_msp_turbulence_force;
  1906.                     }
  1907.                     else
  1908.                     {
  1909.                         iTurbulenceForce[drv->dev_nr] = drv->od_vars->od_2108_msp_turbulence_force * -1;
  1910.                         if (iTurbulenceTime[drv->dev_nr] <= (drv->od_vars->od_2109_msp_turbulence_time * -1))
  1911.                         {
  1912.                             iTurbulenceTime[drv->dev_nr] = drv->od_vars->od_2109_msp_turbulence_time;
  1913.                         }
  1914.                     }
  1915.                     if (drv->od_vars->od_209A_msp_turbulence_time2 < 45)
  1916.                     {
  1917.                         drv->od_vars->od_209A_msp_turbulence_time2 = 45;
  1918.                     }
  1919.                     if (iTurbulenceTime2[drv->dev_nr] >= 1)
  1920.                     {
  1921.                         iTurbulenceForce2[drv->dev_nr] = drv->od_vars->od_2099_msp_turbulence_force2;
  1922.                     }
  1923.                     else
  1924.                     {
  1925.                         iTurbulenceForce2[drv->dev_nr] = drv->od_vars->od_2099_msp_turbulence_force2 * -1;
  1926.                         if (iTurbulenceTime2[drv->dev_nr] <= (drv->od_vars->od_209A_msp_turbulence_time2 * -1))
  1927.                         {
  1928.                             iTurbulenceTime2[drv->dev_nr] = drv->od_vars->od_209A_msp_turbulence_time2;
  1929.                         }
  1930.                     }
  1931.                     if (i16UnitForceOff[drv->dev_nr] == 1)
  1932.                     {
  1933.                         iTurbulenceForce[drv->dev_nr] = 0;
  1934.                         iTurbulenceTime[drv->dev_nr] = 0;
  1935.                     }
  1936.                     iTurbulenceTime[drv->dev_nr] -= 2;
  1937.                     iTurbulenceTime2[drv->dev_nr] -= 2;
  1938.  
  1939.                     break;
  1940.                 }
  1941.                 case 3:
  1942.                 {
  1943.                     drv->ForceFeedbackMode.FFMode_Reg.bit.Turbulence = 2;
  1944.                     break;
  1945.                 }
  1946.             }
  1947.             static int LastSwingFactor = 0;
  1948.             if (drv->od_vars->od_2101_msp_ff_settings.autopilotMode == 0)
  1949.             {
  1950.                 if ((drv->ForceFeedbackMode.FFMode_Reg.bit.Move_Back == 1) && (i32SollScaleFactor[drv->dev_nr] > 25))
  1951.                 {
  1952.  
  1953.  
  1954.                     if (i32absffPosition > position_window_half)
  1955.                     {
  1956.                         iOffsetShadow[drv->dev_nr] = iTargetTorqueShadow;
  1957.                         LastSwingFactor = 0;
  1958.                     }
  1959.                     else
  1960.                     {
  1961.  
  1962.                         if (LastSwingFactor != drv->od_vars->od_2092_msp_ff_Swing_Factor)
  1963.                         {
  1964.                             if (drv->od_vars->od_2092_msp_ff_Swing_Factor > 0)
  1965.                             {
  1966.                                 drv->od_vars->od_2092_msp_ff_Swing_Factor--;
  1967.                             }
  1968.                         }
  1969.                         LastSwingFactor = drv->od_vars->od_2092_msp_ff_Swing_Factor;
  1970.                         iOffsetShadow[drv->dev_nr] = (((int32_t)iTargetTorqueShadow * i32absffPosition) / position_window_half);
  1971.  
  1972.  
  1973.                         //Position Mode Off
  1974.  
  1975.                     }
  1976.                     iOffset[drv->dev_nr] = iOffsetShadow[drv->dev_nr];
  1977.  
  1978.  
  1979.  
  1980.  
  1981.                     if (i32ffPosition[drv->dev_nr] < 0)
  1982.                     {
  1983.                         iOffset[drv->dev_nr] *= -1;
  1984.                     }
  1985.                 }
  1986.                 else
  1987.                 {
  1988.                     if ((drv->ForceFeedbackMode.FFMode_Reg.bit.EXT_Control == true) && (drv->ForceFeedbackMode.FFMode_Reg.bit.Move_Back == false))
  1989.                     {
  1990.                         /*if (labs(drv->od_vars->od_2103_msp_sync_torqu_soll) > labs(iTargetTorqueShadow))
  1991.                         {
  1992.                             iOffset[drv->dev_nr]=drv->od_vars->od_2103_msp_sync_torqu_soll;
  1993.                             if (iTargetTorqueShadow>0)
  1994.                             {
  1995.                                 if (iOffset[drv->dev_nr]<0)
  1996.                                 {
  1997.                                     iOffset[drv->dev_nr]*=-1;
  1998.                                 }
  1999.                             }
  2000.                             else
  2001.                             {
  2002.                                 if (iOffset[drv->dev_nr]>0)
  2003.                                 {
  2004.                                     iOffset[drv->dev_nr]*=-1;
  2005.                                 }
  2006.                             }
  2007.                         }
  2008.                         else
  2009.                         {*/
  2010.                         iOffset[drv->dev_nr] = iTargetTorqueShadow;
  2011.                         //}
  2012.  
  2013.  
  2014.  
  2015.  
  2016.  
  2017.                     }
  2018.                     else
  2019.                     {
  2020.                         if (((drv->od_vars->od_2100_msp_ff_regulator.MiscOptions >> 1) & 0x1) == 0)
  2021.                         {
  2022.                             if (drv->od_vars->od_6063_p402_position_actual_value < i32tempNegativePosLimit)
  2023.                             {
  2024.  
  2025.                                 iOffset[drv->dev_nr] = iTargetTorqueShadow * -1;
  2026.                             }
  2027.                             else
  2028.                             {
  2029.                                 if (drv->od_vars->od_6063_p402_position_actual_value > drv->od_vars->od_2119_msp_ff_soft_limit)
  2030.                                 {
  2031.                                     iOffset[drv->dev_nr] = iTargetTorqueShadow;
  2032.                                 }
  2033.                                 else
  2034.                                 {
  2035.                                     iOffset[drv->dev_nr] = 0;
  2036.                                 }
  2037.                             }
  2038.                         }
  2039.                         else
  2040.                         {
  2041.                             iOffset[drv->dev_nr] = 0;
  2042.                         }
  2043.                     }
  2044.                 }
  2045.             }
  2046.             else
  2047.             {
  2048.                 if (i32ffPosition[drv->dev_nr] > (drv->od_vars->od_2117_msp_ff_move_to_position + position_window_half))
  2049.                 {
  2050.  
  2051.                     iOffset[drv->dev_nr] = iTargetTorqueShadow;
  2052.  
  2053.                 }
  2054.                 else
  2055.                 {
  2056.                     if (i32ffPosition[drv->dev_nr] < (drv->od_vars->od_2117_msp_ff_move_to_position - position_window_half))
  2057.                     {
  2058.                         iOffset[drv->dev_nr] = iTargetTorqueShadow;
  2059.                         iOffset[drv->dev_nr] *= -1;
  2060.                     }
  2061.                     else
  2062.                     {
  2063.                         iOffset[drv->dev_nr] = 0;
  2064.                     }
  2065.                 }
  2066.             }
  2067.  
  2068.  
  2069.             drv->od_vars->od_2112_msp_ff_sync_delta = i32ffPosition[drv->dev_nr] - drv->od_vars->od_2104_msp_sync_position_soll;
  2070.  
  2071.             if ((drv->ForceFeedbackMode.FFMode_Reg.bit.OffsetFFSensor == 1) && (drv->bOffsetAnin0 == false))
  2072.             {
  2073.  
  2074.  
  2075.                 msp_sync_torqu_ist_shadow[drv->dev_nr] = 0;
  2076.                 if (i32OffsetCounter[drv->dev_nr] > 49)
  2077.                 {
  2078.                     iTargetTorqueShadow = 10;
  2079.                     iOffset[drv->dev_nr] = 0;
  2080.                 }
  2081.  
  2082.             }
  2083.             else
  2084.             {
  2085.                 if ((i16UnitForceOff[drv->dev_nr] == 1) && (iTurbulenceForce[drv->dev_nr] > 0))
  2086.                 {
  2087.                     msp_sync_torqu_ist_shadow[drv->dev_nr] = i32ForceSensor[drv->dev_nr] + iTurbulenceForce[drv->dev_nr] + drv->od_vars->od_209B_msp_user_force_offset;
  2088.  
  2089.                 }
  2090.                 else
  2091.                 {
  2092.                     msp_sync_torqu_ist_shadow[drv->dev_nr] = i32Force[drv->dev_nr] + iTurbulenceForce[drv->dev_nr] + drv->od_vars->od_209B_msp_user_force_offset;
  2093.  
  2094.                 }
  2095.  
  2096.             }
  2097.  
  2098.  
  2099.             drv->od_vars->od_6077_p402_torque_actual_value = i32Force[drv->dev_nr];
  2100.             if ((drv->ForceFeedbackMode.FFMode_Reg.bit.MoveNegLimit == true) || (drv->ForceFeedbackMode.FFMode_Reg.bit.MovePosLimit == true))
  2101.             {
  2102.                 i16UnitForceOff[drv->dev_nr] = 0;
  2103.  
  2104.                 if (drv->od_vars->od_2097_msp_ff_master_slave_parameter_soll == 1)
  2105.                 {
  2106.                     drv->od_vars->od_2097_msp_ff_master_slave_parameter_soll = 0;
  2107.                 }
  2108.                 if (i32FFEndPosCounter[drv->dev_nr] < 100000)
  2109.                 {
  2110.                     i32FFEndPosCounter[drv->dev_nr]++;
  2111.                 }
  2112.                 else
  2113.                 {
  2114.                     //Shutdown Device
  2115.                     drv->od_vars->od_6040_p402_controlword = OPERATIONMODEBITENABLE + SHUTDOWN;
  2116.                     drv->ForceFeedbackMode.FFMode_Reg.bit.MoveNegLimit = false;
  2117.                     drv->ForceFeedbackMode.FFMode_Reg.bit.MovePosLimit = false;
  2118.                     i32FFEndPosCounter[drv->dev_nr] = 0;
  2119.                     i16DynamicOffset[drv->dev_nr] = 0;
  2120.                 }
  2121.                 iTargetTorqueShadow = 10;
  2122.                 if (drv->ForceFeedbackMode.FFMode_Reg.bit.MoveNegLimit == true)
  2123.                 {
  2124.                     if (drv->ForceFeedbackMode.FFMode_Reg.bit.MovePosLimit == true)
  2125.                     {
  2126.                         i32FFEndPosCounter[drv->dev_nr] = 0;
  2127.                         i16DynamicOffset[drv->dev_nr] = 0;
  2128.                         drv->ForceFeedbackMode.FFMode_Reg.bit.Init = 0;
  2129.                         drv->bInitFirstInit = false;
  2130.  
  2131.                     }
  2132.                     else
  2133.                     {
  2134.  
  2135.  
  2136.                         if (abs (drv->od_vars->od_606C_p402_velocity_actual_value) < drv->od_vars->od_6099_p402_homing_speeds.fastHomingSpeed - 5)
  2137.                         {
  2138.                             if ((i16DynamicOffset[drv->dev_nr] < 32760) && (i32FFEndPosCounter[drv->dev_nr] < 32765))
  2139.                             {
  2140.                                 if (i16DynamicOffset[drv->dev_nr] < (drv->od_vars->od_211B_msp_ff_Sensor_Error << 2))
  2141.                                 {
  2142.                                     i16DynamicOffset[drv->dev_nr] += 2;
  2143.                                     if (i16DynamicOffset[drv->dev_nr] < 2000)
  2144.                                     {
  2145.                                         i16DynamicOffset[drv->dev_nr] = 2000;
  2146.                                     }
  2147.                                 }
  2148.                             }
  2149.                         }
  2150.                         if ((drv->od_vars->od_6063_p402_position_actual_value - 100) < i32tempNegativePosLimit)
  2151.                         {
  2152.                             i16DynamicOffset[drv->dev_nr] = 0;
  2153.                             i32FFEndPosCounter[drv->dev_nr] = 100000;
  2154.                         }
  2155.                     }
  2156.                 }
  2157.                 else
  2158.                 {
  2159.  
  2160.                     if (abs(drv->od_vars->od_606C_p402_velocity_actual_value) < drv->od_vars->od_6099_p402_homing_speeds.fastHomingSpeed - 5)
  2161.                     {
  2162.                         if ((i16DynamicOffset[drv->dev_nr] > -32760) && (i32FFEndPosCounter[drv->dev_nr] < 32765))
  2163.                         {
  2164.                             if (abs(i16DynamicOffset[drv->dev_nr]) < (drv->od_vars->od_211B_msp_ff_Sensor_Error << 2))
  2165.                             {
  2166.                                 i16DynamicOffset[drv->dev_nr] -= 2;
  2167.                                 if (i16DynamicOffset[drv->dev_nr] > -2000)
  2168.                                 {
  2169.                                     i16DynamicOffset[drv->dev_nr] = -2000;
  2170.                                 }
  2171.                             }
  2172.                         }
  2173.                     }
  2174.  
  2175.  
  2176.                     if ((drv->od_vars->od_6063_p402_position_actual_value + 100) > drv->od_vars->od_2119_msp_ff_soft_limit)
  2177.                     {
  2178.                         i16DynamicOffset[drv->dev_nr] = 0;
  2179.                         i32FFEndPosCounter[drv->dev_nr] = 100000;
  2180.                     }
  2181.  
  2182.  
  2183.                 }
  2184.                 iOffset[drv->dev_nr] = (i16DynamicOffset[drv->dev_nr] >> 5);
  2185.             }
  2186.             else
  2187.             {
  2188.                 i16DynamicOffset[drv->dev_nr] = 0;
  2189.                 i32FFEndPosCounter[drv->dev_nr] = 0;
  2190.             }
  2191.  
  2192.  
  2193.             if ((drv->ForceFeedbackMode.FFMode_Reg.bit.OffsetFFSensor == 1) && (drv->bOffsetAnin0 == false))
  2194.             {
  2195.                 i32Force[drv->dev_nr] = i32Force[drv->dev_nr] + drv->od_vars->od_2103_msp_sync_torqu_soll;
  2196.             }
  2197.             else
  2198.             {
  2199.                 i32Force[drv->dev_nr] = msp_sync_torqu_ist_shadow[drv->dev_nr] + drv->od_vars->od_2103_msp_sync_torqu_soll;
  2200.             }
  2201.             i32ForceSave[drv->dev_nr] = i32Force[drv->dev_nr];
  2202.  
  2203.             if (((drv->ForceFeedbackMode.FFMode_Reg.bit.Force_Linear_Up == 0) && (drv->ForceFeedbackMode.FFMode_Reg.bit.EXT_Control == true) && (drv->ForceFeedbackMode.FFMode_Reg.bit.Move_Back == false)) || (drv->od_vars->od_2101_msp_ff_settings.autopilotMode == 1))
  2204.             {
  2205.                 if (iOffset[drv->dev_nr] < 0)
  2206.                 {
  2207.                     if (i32Force[drv->dev_nr] < (iOffset[drv->dev_nr] * -1))
  2208.                     {
  2209.                         i32Force[drv->dev_nr] = (((int32_t)iOffset[drv->dev_nr] * (int32_t)drv->od_vars->od_2102_msp_ff_max_move_back_speed) / 100) + i32Force[drv->dev_nr];
  2210.  
  2211.  
  2212.                     }
  2213.                 }
  2214.                 else
  2215.                 {
  2216.                     if (i32Force[drv->dev_nr] > (iOffset[drv->dev_nr] * -1))
  2217.                     {
  2218.                         i32Force[drv->dev_nr] = (((int32_t)iOffset[drv->dev_nr] * (int32_t)drv->od_vars->od_2102_msp_ff_max_move_back_speed) / 100) + i32Force[drv->dev_nr];
  2219.                     }
  2220.                 }
  2221.  
  2222.  
  2223.             }
  2224.             else
  2225.             {
  2226.                 if ( ( labs(i32Force[drv->dev_nr]) < labs(iOffset[drv->dev_nr])) || ( (i32Force[drv->dev_nr] > 0) && (i32ffPosition[drv->dev_nr] > 0) ) || ( (i32Force[drv->dev_nr] < 0) && (i32ffPosition[drv->dev_nr] < 0) ))
  2227.                 {
  2228.  
  2229.  
  2230.                     if (drv->od_vars->od_2102_msp_ff_max_move_back_speed > 200)
  2231.                     {
  2232.                         i32pFactorMoveBack = (550 - drv->od_vars->od_2102_msp_ff_max_move_back_speed) / 10;
  2233.                         i32TempCalcSoll[drv->dev_nr] = 200 + labs(drv->od_vars->od_606C_p402_velocity_actual_value / i32pFactorMoveBack);
  2234.                         if (i32TempCalcSoll[drv->dev_nr] > drv->od_vars->od_2102_msp_ff_max_move_back_speed)
  2235.                         {
  2236.                             i32TempCalcSoll[drv->dev_nr] = drv->od_vars->od_2102_msp_ff_max_move_back_speed;
  2237.                         }
  2238.                         if (i32TempCalc[drv->dev_nr] < i32TempCalcSoll[drv->dev_nr])
  2239.                         {
  2240.                             i32TempCalc[drv->dev_nr] += 2;
  2241.                         }
  2242.                         else
  2243.                         {
  2244.                             if (i32TempCalc[drv->dev_nr] > i32TempCalcSoll[drv->dev_nr])
  2245.                             {
  2246.                                 i32TempCalc[drv->dev_nr] -= 2;
  2247.                             }
  2248.                         }
  2249.                     }
  2250.                     else
  2251.                     {
  2252.                         if (i32TempCalc[drv->dev_nr] < drv->od_vars->od_2102_msp_ff_max_move_back_speed)
  2253.                         {
  2254.                             i32TempCalc[drv->dev_nr] += 2;
  2255.                         }
  2256.                         else
  2257.                         {
  2258.                             if (i32TempCalc[drv->dev_nr] > drv->od_vars->od_2102_msp_ff_max_move_back_speed)
  2259.                             {
  2260.                                 i32TempCalc[drv->dev_nr] -= 2;
  2261.                             }
  2262.                         }
  2263.                     }
  2264.  
  2265.  
  2266.  
  2267.  
  2268.                     if (iOffset[drv->dev_nr] < 0)
  2269.                     {
  2270.                         i32temp = ((drv->od_vars->od_2092_msp_ff_Swing_Factor * -100) * labs(drv->od_vars->od_606C_p402_velocity_actual_value)) / 1000;
  2271.                     }
  2272.                     else
  2273.                     {
  2274.                         i32temp = ((drv->od_vars->od_2092_msp_ff_Swing_Factor * 100) * labs(drv->od_vars->od_606C_p402_velocity_actual_value)) / 1000;
  2275.                     }
  2276.  
  2277.  
  2278.  
  2279.                     if (i32SwingSoll < i32temp)
  2280.                     {
  2281.                         i32SwingSoll += 5;
  2282.                     }
  2283.                     else
  2284.                     {
  2285.                         if (i32SwingSoll > i32temp)
  2286.                         {
  2287.                             i32SwingSoll -= 5;
  2288.                         }
  2289.                     }
  2290.  
  2291.  
  2292.                     i32Force[drv->dev_nr] = (((int32_t)iOffset[drv->dev_nr] * (int32_t)i32TempCalc[drv->dev_nr]) / 100) + i32Force[drv->dev_nr] + i32SwingSoll;
  2293.                 }
  2294.                 else
  2295.                 {
  2296.                     if (drv->od_vars->od_2102_msp_ff_max_move_back_speed > 200)
  2297.                     {
  2298.                         i32TempCalc[drv->dev_nr] = 200;
  2299.                     }
  2300.  
  2301.                     i32SwingSoll = 0;
  2302.                     i32Force[drv->dev_nr] = i32Force[drv->dev_nr] + i32SwingSoll;
  2303.                 }
  2304.             }
  2305.  
  2306.  
  2307.             if (drv->ForceFeedbackMode.FFMode_Reg.bit.SyncMode == 1)
  2308.             {
  2309.                 i32Force[drv->dev_nr] = 0;
  2310.             }
  2311.             if (labs(i32ForceSensor[drv->dev_nr]) > drv->od_vars->od_211D_msp_friction_regulator[2])
  2312.             {
  2313.                 drv->od_vars->od_2101_msp_ff_settings.humanMaster = 1;
  2314.                 i32DynamicCenter[drv->dev_nr] = 0;
  2315.             }
  2316.             else
  2317.             {
  2318.                 drv->od_vars->od_2101_msp_ff_settings.humanMaster = 0;
  2319.                 //i16DynamicCounter[drv->dev_nr]=0;
  2320.  
  2321.             }
  2322.  
  2323.  
  2324.  
  2325.             iTargetTorqueShadow = iTargetTorqueShadow + abs(drv->od_vars->od_211D_msp_friction_regulator[11]);
  2326.             if ( labs(i32Force[drv->dev_nr]) > labs(iTargetTorqueShadow - drv->od_vars->od_2100_msp_ff_regulator.permanentOff))
  2327.             {
  2328.  
  2329.  
  2330.                 i32ActualTorque = (labs(i32Force[drv->dev_nr]) - labs(iTargetTorqueShadow));
  2331.                 iAxisForceSlave[drv->dev_nr] = drv->od_vars->od_2100_msp_ff_regulator.pFactForceDenum;
  2332.  
  2333.  
  2334.  
  2335.  
  2336.                 //Reduce Friction when Scale Factor is low
  2337.                 if ((i32SollScaleFactor[drv->dev_nr] <= drv->od_vars->od_211F_msp_manual_offset_off_pos) && ( drv->od_vars->od_211A_msp_ff_manual_offset_force == 0))
  2338.                 {
  2339.                     if (iAxisForceSlave[drv->dev_nr] > 1)
  2340.                     {
  2341.                         iAxisForceSlave[drv->dev_nr] = iAxisForceSlave[drv->dev_nr] - (drv->od_vars->od_211F_msp_manual_offset_off_pos - i32SollScaleFactor[drv->dev_nr]);
  2342.                         if ( iAxisForceSlave[drv->dev_nr] < (drv->od_vars->od_2100_msp_ff_regulator.pFactForceDenum >> 1))
  2343.                         {
  2344.                             iAxisForceSlave[drv->dev_nr] = (drv->od_vars->od_2100_msp_ff_regulator.pFactForceDenum >> 1);
  2345.                         }
  2346.                         if (iAxisForceSlave[drv->dev_nr] < 2)
  2347.                         {
  2348.                             iAxisForceSlave[drv->dev_nr] = 2;
  2349.                         }
  2350.                     }
  2351.  
  2352.                 }
  2353.  
  2354.  
  2355.  
  2356.  
  2357.  
  2358.                 //Sync mode reduce vibrate when both unit is give force
  2359.                 /* if (drv->od_vars->od_2107_msp_torque_sensor<=1)
  2360.                  {
  2361.                      i32temp =drv->od_vars->od_211D_msp_friction_regulator[2];
  2362.                      if (i32temp<10)
  2363.                      {
  2364.                          i32temp=10;
  2365.                      }
  2366.                      if (drv->od_vars->od_2107_msp_torque_sensor==1)
  2367.                      {
  2368.                          if (i32temp<250)
  2369.                          {
  2370.                              i32temp=250;
  2371.                          }
  2372.                      }
  2373.                      iAxisForceSlave[drv->dev_nr]=drv->od_vars->od_2100_msp_ff_regulator.pFactForceDenum;
  2374.  
  2375.  
  2376.                      if (drv->od_vars->od_2103_msp_sync_torqu_soll!=0)
  2377.                      {
  2378.                          if (labs (drv->od_vars->od_2103_msp_sync_torqu_soll)> (i32temp*5))
  2379.                          {
  2380.                              if (labs (msp_sync_torqu_ist_shadow[drv->dev_nr])> (i32temp*5))
  2381.                              {
  2382.                                  i32temp2=labs(msp_sync_torqu_ist_shadow[drv->dev_nr]-drv->od_vars->od_2103_msp_sync_torqu_soll)-labs(iTargetTorqueShadow);
  2383.  
  2384.                                  iAxisForceSlave[drv->dev_nr]=i32temp2/(i32temp*5);
  2385.                                  if (iAxisForceSlave[drv->dev_nr]>1)
  2386.                                  {
  2387.                                      iAxisForceSlave[drv->dev_nr] = (i32temp2*(int32_t)drv->od_vars->od_2100_msp_ff_regulator.pFactForceDenum)/(i32temp*5);
  2388.                                  }
  2389.                                  else
  2390.                                  {
  2391.                                      iAxisForceSlave[drv->dev_nr]=drv->od_vars->od_2100_msp_ff_regulator.pFactForceDenum;
  2392.                                  }
  2393.                              }
  2394.                          }
  2395.                      }
  2396.                  }*/
  2397.                 iAxisForceSlave[drv->dev_nr] = drv->od_vars->od_2100_msp_ff_regulator.pFactForceDenum;
  2398.                 iMaxMotorSpeed[drv->dev_nr] = (i32ActualTorque * drv->od_vars->od_2100_msp_ff_regulator.pFactForceNum) / iAxisForceSlave[drv->dev_nr];
  2399.  
  2400.                 /*else
  2401.                 {
  2402.                     i32BackupAutopilotPosition=
  2403.                 }*/
  2404.  
  2405.                 if (i32Force[drv->dev_nr] < 0)
  2406.                 {
  2407.                     drv->od_vars->od_60FF_p402_target_velocity = iMaxMotorSpeed[drv->dev_nr];
  2408.                 }
  2409.                 else
  2410.                 {
  2411.                     drv->od_vars->od_60FF_p402_target_velocity = iMaxMotorSpeed[drv->dev_nr] * -1;
  2412.                 }
  2413.  
  2414.                 if (labs(drv->od_vars->od_60FF_p402_target_velocity) > ui32MaxVelocity)
  2415.                 {
  2416.                     if (drv->od_vars->od_60FF_p402_target_velocity < 0)
  2417.                     {
  2418.                         drv->od_vars->od_60FF_p402_target_velocity = ui32MaxVelocity;
  2419.                         drv->od_vars->od_60FF_p402_target_velocity *= -1;
  2420.                     }
  2421.                     else
  2422.                     {
  2423.                         drv->od_vars->od_60FF_p402_target_velocity = ui32MaxVelocity;
  2424.                     }
  2425.                 }
  2426.                 if ((drv->ForceFeedbackMode.FFMode_Reg.bit.MoveNegLimit == true) || (drv->ForceFeedbackMode.FFMode_Reg.bit.MovePosLimit == true))
  2427.                 {
  2428.                     if (labs(drv->od_vars->od_60FF_p402_target_velocity) > drv->od_vars->od_6099_p402_homing_speeds.fastHomingSpeed)
  2429.                     {
  2430.                         if (drv->od_vars->od_60FF_p402_target_velocity < 0)
  2431.                         {
  2432.                             drv->od_vars->od_60FF_p402_target_velocity = (int32_t)drv->od_vars->od_6099_p402_homing_speeds.fastHomingSpeed * -1;
  2433.                         }
  2434.                         else
  2435.                         {
  2436.                             drv->od_vars->od_60FF_p402_target_velocity = drv->od_vars->od_6099_p402_homing_speeds.fastHomingSpeed;
  2437.                         }
  2438.                     }
  2439.                 }
  2440.  
  2441.  
  2442.                 if (drv->od_vars->od_2101_msp_ff_settings.autopilotMode == 1)
  2443.                 {
  2444.                     i32MaxSpeed = (labs(i32ffPosition[drv->dev_nr] - drv->od_vars->od_2117_msp_ff_move_to_position)) / (int32_t)drv->od_vars->od_2100_msp_ff_regulator.corrFactVelPos;
  2445.                     if (drv->FFsyncRunning == true)
  2446.                     {
  2447.                         i32MaxSpeed = i32MaxSpeed + labs(drv->od_vars->od_2112_msp_ff_sync_delta / (int32_t)drv->od_vars->od_2100_msp_ff_regulator.corrFactVelPos);
  2448.                     }
  2449.                     iAutopilotSpeed[drv->dev_nr] = drv->od_vars->od_6081_p402_profile_velocity;
  2450.  
  2451.                     //Min Speed = 6RPM
  2452.                     if (iAutopilotSpeed[drv->dev_nr] < 6)
  2453.                     {
  2454.                         iAutopilotSpeed[drv->dev_nr] = 6;
  2455.                     }
  2456.                     //Todo Check if working
  2457.                     if ( labs(i32ForceSensor[drv->dev_nr]) > drv->od_vars->od_2118_msp_ff_move_to_position_force)
  2458.                     {
  2459.                         i32MaxSpeed =  labs(i32ForceSensor[drv->dev_nr]) - drv->od_vars->od_2118_msp_ff_move_to_position_force; //* msp_force_feedback_regulator[1])/iAxisForceSlave;
  2460.                     }
  2461.  
  2462.  
  2463.                     if (i32MaxSpeed > iAutopilotSpeed[drv->dev_nr])
  2464.                     {
  2465.                         i32MaxSpeed = iAutopilotSpeed[drv->dev_nr];
  2466.                     }
  2467.                     if (i32MaxSpeed > drv->od_vars->od_6080_p402_max_motor_speed)
  2468.                     {
  2469.                         i32MaxSpeed = drv->od_vars->od_6080_p402_max_motor_speed;
  2470.                     }
  2471.  
  2472.                     if (i32MaxSpeedSoll[drv->dev_nr] < i32MaxSpeed)
  2473.                     {
  2474.                         i32MaxSpeedSoll[drv->dev_nr] += 2;
  2475.                     }
  2476.                     else
  2477.                     {
  2478.                         if (i32MaxSpeedSoll[drv->dev_nr] > i32MaxSpeed)
  2479.                         {
  2480.                             i32MaxSpeedSoll[drv->dev_nr] -= 2;
  2481.                         }
  2482.                     }
  2483.  
  2484.                     if (labs(drv->od_vars->od_60FF_p402_target_velocity) > i32MaxSpeedSoll[drv->dev_nr])
  2485.                     {
  2486.                         if (drv->od_vars->od_60FF_p402_target_velocity < 0)
  2487.                         {
  2488.                             drv->od_vars->od_60FF_p402_target_velocity = i32MaxSpeedSoll[drv->dev_nr] * -1;
  2489.                         }
  2490.                         else
  2491.                         {
  2492.                             drv->od_vars->od_60FF_p402_target_velocity = i32MaxSpeedSoll[drv->dev_nr];
  2493.                         }
  2494.                     }
  2495.                 }
  2496.                 // iCountSwing=10;
  2497.  
  2498.                 //iSwingSpeed=(drv->od_vars->od_60FF_p402_target_velocity>>2)*(drv->od_vars->od_60FF_p402_target_velocity>>2);
  2499.  
  2500.                 //iLastVel=drv->od_vars->od_60FF_p402_target_velocity;
  2501.                 //if (labs(i32Force[drv->dev_nr])<200)
  2502.                 //{
  2503.                 //if (iLastVel<0)
  2504.                 //      {
  2505.                 //      drv->od_vars->od_60FF_p402_target_velocity-=iSwingSpeed;
  2506.                 //  }
  2507.                 //else
  2508.                 //{
  2509.                 //  drv->od_vars->od_60FF_p402_target_velocity+=iSwingSpeed;
  2510.                 //}
  2511.                 //}
  2512.  
  2513.  
  2514.             }
  2515.             else
  2516.             {
  2517.                 drv->od_vars->od_2101_msp_ff_settings.humanMaster = 0;
  2518.  
  2519.  
  2520.                 //ToDo
  2521.  
  2522.  
  2523.  
  2524.  
  2525.                 // if (((labs(msp_sync_torqu_ist_shadow[drv->dev_nr])<labs(drv->od_vars->od_2103_msp_sync_torqu_soll))&&(msp_sync_torqu_ist_shadow[drv->dev_nr]!=0))||(drv->ForceFeedbackMode.FFMode_Reg.bit.SyncMode==1)||(i16UnitForceOff[drv->dev_nr]==1))
  2526.                 // {
  2527.  
  2528.  
  2529.                 if (( drv->ForceFeedbackSyncStateSoll.FFSyncState_Reg.bit.State != 1) && (drv->ForceFeedbackSyncStateIst.FFSyncState_Reg.bit.State != 1) && (drv->FFsyncLost == false))
  2530.                 {
  2531.                     if (labs(drv->od_vars->od_2112_msp_ff_sync_delta) > 20)
  2532.                     {
  2533.                         iMaxSynCorr[drv->dev_nr] = (drv->od_vars->od_2100_msp_ff_regulator.corrFactVelSync * 250) / labs(drv->od_vars->od_2112_msp_ff_sync_delta);
  2534.                         if (iMaxSynCorr[drv->dev_nr] < 5 )
  2535.                         {
  2536.                             iMaxSynCorr[drv->dev_nr] = 5;
  2537.                         }
  2538.                         else
  2539.                         {
  2540.                             if (iMaxSynCorr[drv->dev_nr] > drv->od_vars->od_2100_msp_ff_regulator.corrFactVelSync )
  2541.                             {
  2542.                                 iMaxSynCorr[drv->dev_nr] = drv->od_vars->od_2100_msp_ff_regulator.corrFactVelSync;
  2543.                             }
  2544.                         }
  2545.                     }
  2546.                     else
  2547.                     {
  2548.                         iMaxSynCorr[drv->dev_nr] = drv->od_vars->od_2100_msp_ff_regulator.corrFactVelSync;
  2549.                     }
  2550.  
  2551.                     iMaxMotorSpeed[drv->dev_nr] = labs(drv->od_vars->od_2112_msp_ff_sync_delta / iMaxSynCorr[drv->dev_nr]);
  2552.  
  2553.  
  2554.                     if (iMaxMotorSpeed[drv->dev_nr] > drv->od_vars->od_607F_p402_max_profile_velocity)
  2555.                     {
  2556.                         iMaxMotorSpeed[drv->dev_nr] = drv->od_vars->od_607F_p402_max_profile_velocity;
  2557.                     }
  2558.  
  2559.  
  2560.                     if (i32ffPosition[drv->dev_nr] > (drv->od_vars->od_2104_msp_sync_position_soll + i32PositionWindowShadow[drv->dev_nr]) )
  2561.                     {
  2562.                         drv->od_vars->od_60FF_p402_target_velocity = (iMaxMotorSpeed[drv->dev_nr] * -1);
  2563.                     }
  2564.                     else
  2565.                     {
  2566.                         if (i32ffPosition[drv->dev_nr] < (drv->od_vars->od_2104_msp_sync_position_soll - i32PositionWindowShadow[drv->dev_nr]))
  2567.                         {
  2568.                             drv->od_vars->od_60FF_p402_target_velocity = iMaxMotorSpeed[drv->dev_nr];
  2569.  
  2570.                         }
  2571.                         else
  2572.                         {
  2573.                             drv->od_vars->od_60FF_p402_target_velocity = 0;
  2574.                         }
  2575.                     }
  2576.                 }
  2577.                 else
  2578.                 {
  2579.                     drv->od_vars->od_60FF_p402_target_velocity = 0;
  2580.                 }
  2581.  
  2582.                 //   }
  2583.                 // else
  2584.                 // {
  2585.                 /* if (i32ffPosition[drv->dev_nr]> (drv->od_vars->od_2104_msp_sync_position_soll+position_window_half) )
  2586.                  {
  2587.                       iSwingCMD=0;
  2588.                  }
  2589.                  else
  2590.                  {
  2591.                      if (i32ffPosition[drv->dev_nr]< (drv->od_vars->od_2104_msp_sync_position_soll-position_window_half))
  2592.                      {
  2593.                          iSwingCMD=0;
  2594.                      }
  2595.                      else
  2596.                      {
  2597.                           iSwingCMD=1;
  2598.  
  2599.  
  2600.                      }
  2601.                  }*/
  2602.  
  2603.                 //static int32_t iCountSwing=10;
  2604.                 //  static int32_t iSwingSpeed=50;
  2605.                 /*if (labs(i32Force[drv->dev_nr])<200)
  2606.                 {
  2607.                     drv->od_vars->od_60FF_p402_target_velocity=(iSwingSpeed*iCountSwing)/10;
  2608.                 }
  2609.                 else
  2610.                 {
  2611.                     drv->od_vars->od_60FF_p402_target_velocity=0;
  2612.                 }*/
  2613.  
  2614.                 /* if ((labs(i32Force[drv->dev_nr])<200) && ( iSwingCMD==1))
  2615.                  {
  2616.                      if (iLastVel<0)
  2617.                      {
  2618.                          drv->od_vars->od_60FF_p402_target_velocity-=500;
  2619.                      }
  2620.                      else
  2621.                      {
  2622.                          drv->od_vars->od_60FF_p402_target_velocity+=500;
  2623.                      }
  2624.                  }
  2625.                  else
  2626.                  {*/
  2627.                 //   if (drv->od_vars->od_2101_msp_ff_settings.autopilotMode==1)
  2628.                 // {
  2629.                 //  drv->od_vars->od_60FF_p402_target_velocity=0;
  2630.                 // }
  2631.                 //}
  2632.  
  2633.  
  2634.  
  2635.                 //   }
  2636.  
  2637.             }
  2638.  
  2639.  
  2640.             //ToDo BRS
  2641.             /* if (drv->od_vars->od_2101_msp_ff_settings.humanMaster==0)
  2642.              {
  2643.  
  2644.                  if (drv->od_vars->od_2092_msp_ff_Swing_Factor>0)
  2645.                  {
  2646.                      if (drv->od_vars->od_2092_msp_ff_Swing_Factor>20)
  2647.                      {
  2648.                          drv->od_vars->od_2092_msp_ff_Swing_Factor=20;
  2649.                      }
  2650.                      if ( labs(i32ffPosition[drv->dev_nr]) < labs (i32DynamicCenter[drv->dev_nr]/4))
  2651.                      {
  2652.                            if (labs (i32DynamicCenter[drv->dev_nr]>50) )
  2653.                            {
  2654.                                    i32DynamicCenter[drv->dev_nr]=i32DynamicCenter[drv->dev_nr]/-2;
  2655.                            }
  2656.                            else
  2657.                            {
  2658.                                      i32DynamicCenter[drv->dev_nr]=0;
  2659.                            }
  2660.                      }
  2661.                  }
  2662.                             //drv->od_vars->od_2033_msp_debug_monitor[3] = labs(i32ffPosition[drv->dev_nr]);
  2663.                             //drv->od_vars->od_2033_msp_debug_monitor[4] = position_window_half;
  2664.              }*/
  2665.             //drv->od_vars->od_2033_msp_debug_monitor[5] = drv->od_vars->od_2101_msp_ff_settings.humanMaster;
  2666.  
  2667.  
  2668.             drv->od_vars->od_2800_msp_syn_vel_soll = labs(drv->od_vars->od_60FF_p402_target_velocity);
  2669.  
  2670.             if ((( (drv->od_vars->od_2100_msp_ff_regulator.MiscOptions >> 1) & 0x1) == 0) && (labs(i32ForceSensor[drv->dev_nr]) < drv->od_vars->od_6072_p402_max_torque))
  2671.             {
  2672.                 //if (drv->od_vars->od_2100_msp_ff_regulator.softLimitStop<10)
  2673.                 //{
  2674.                 if (drv->od_vars->od_6063_p402_position_actual_value < 0)
  2675.                 {
  2676.  
  2677.  
  2678.  
  2679.  
  2680.                     if (drv->od_vars->od_6063_p402_position_actual_value > (i32tempNegativePosLimit + 10))
  2681.                     {
  2682.                         i32temp = i32tempNegativePosLimit - drv->od_vars->od_6063_p402_position_actual_value;
  2683.                     }
  2684.  
  2685.                     else
  2686.                     {
  2687.                         i32temp = 10;
  2688.  
  2689.                     }
  2690.                     i32SoftEndPosDelta = labs(i32temp >> drv->od_vars->od_2100_msp_ff_regulator.softLimitStop);
  2691.                     if (i32SoftEndPosDelta < 50)
  2692.                     {