@@ -878,13 +878,13 @@ void Integrate(float elapsedClockSeconds)
878878 {
879879 if ( elapsedClockSeconds <= 0 ) return ;
880880 double prevSpeedMpS = AxleSpeedMpS ;
881-
881+ /*
882882 if (Math.Abs(integratorError) > Math.Max((Math.Abs(SlipSpeedMpS) - 1) * 0.01, 0.001))
883883 {
884884 ++NumOfSubstepsPS;
885885 waitBeforeSpeedingUp = 100;
886886
887- // Trace.TraceInformation("Algorithim Increase - Steps {0} Err {1}", NumOfSubstepsPS, integratorError);
887+ // Trace.TraceInformation("Algorithim Increase - Steps {0} Err {1}", NumOfSubstepsPS, integratorError);
888888
889889 }
890890 else
@@ -894,34 +894,43 @@ void Integrate(float elapsedClockSeconds)
894894 --NumOfSubstepsPS;
895895 waitBeforeSpeedingUp = 10; //not so fast ;)
896896
897- // Trace.TraceInformation("Algorithim Decrease - Steps {0} Err {1}", NumOfSubstepsPS, integratorError);
897+ // Trace.TraceInformation("Algorithim Decrease - Steps {0} Err {1}", NumOfSubstepsPS, integratorError);
898898 }
899899 }
900900
901901 NumOfSubstepsPS = Math.Max(Math.Min(NumOfSubstepsPS, 130), 1);
902-
903- // use straight line graph approximation
904- // Points are 1 = (0, 150) and 2 = (threshold, 0)
905-
906-
907- if ( SlipSpeedMpS > 0.7 * WheelSlipThresholdMpS )
902+ */
903+ // use straight line graph approximation
904+ // Points are 1 = (0, upperLimit) and 2 = (threshold, lowerLimit)
905+
906+ var upperLimit = 130 ;
907+ var lowerLimit = 10 ;
908+ var AdhesGrad = ( ( upperLimit - lowerLimit ) / ( WheelSlipThresholdMpS - 0 ) ) ;
909+ var targetNumOfSubstepsPS = Math . Abs ( ( AdhesGrad * SlipSpeedMpS ) + lowerLimit ) ;
910+ if ( float . IsNaN ( ( float ) targetNumOfSubstepsPS ) ) targetNumOfSubstepsPS = 1 ;
911+ // Trace.TraceInformation("Grad - {0} AdhesGrad {1} SlipSpeedMps {2} Threshold {3}", temp, AdhesGrad, SlipSpeedMpS, WheelSlipThresholdMpS);
912+
913+ if ( targetNumOfSubstepsPS > NumOfSubstepsPS ) // increase substeps
908914 {
909- var upperLimit = 130 ;
910- var AdhesGrad = ( ( 0 - upperLimit ) / ( WheelSlipThresholdMpS - 0 ) ) ;
911- var temp = Math . Abs ( ( AdhesGrad * SlipSpeedMpS ) + upperLimit ) ;
912- if ( float . IsNaN ( ( float ) temp ) ) temp = 1 ;
913- // Trace.TraceInformation("Grad - {0} AdhesGrad {1} SlipSpeedMps {2} Threshold {3}", temp, AdhesGrad, SlipSpeedMpS, WheelSlipThresholdMpS);
914-
915- NumOfSubstepsPS = ( int ) temp ;
915+ if ( -- waitBeforeSpeedingUp <= 0 ) //wait for a while before speeding up the integration
916+ {
917+ NumOfSubstepsPS += 5 ;
918+ waitBeforeSpeedingUp = 5 ; //not so fast ;)
919+ }
920+ }
921+ else if ( targetNumOfSubstepsPS < NumOfSubstepsPS ) // decrease sub steps
922+ {
923+ NumOfSubstepsPS -= 5 ;
924+ waitBeforeSpeedingUp = 5 ;
925+ }
916926
917- if ( NumOfSubstepsPS < 1 )
918- NumOfSubstepsPS = 1 ;
927+ if ( NumOfSubstepsPS < lowerLimit )
928+ NumOfSubstepsPS = lowerLimit ;
919929
920- if ( NumOfSubstepsPS > upperLimit )
921- NumOfSubstepsPS = upperLimit ;
930+ if ( NumOfSubstepsPS > upperLimit )
931+ NumOfSubstepsPS = upperLimit ;
922932
923- // Trace.TraceInformation("Number of Steps {0}", NumOfSubstepsPS);
924- }
933+ // Trace.TraceInformation("Grad - {0} AdhesGrad {1} SlipSpeedMps {2} Threshold {3} NumStepsPS {4}", targetNumOfSubstepsPS, AdhesGrad, SlipSpeedMpS, WheelSlipThresholdMpS, NumOfSubstepsPS);
925934
926935 double dt = elapsedClockSeconds / NumOfSubstepsPS ;
927936 double hdt = dt / 2 ;
@@ -934,16 +943,16 @@ void Integrate(float elapsedClockSeconds)
934943 {
935944 if ( k1 . Item1 * dt > Math . Max ( ( Math . Abs ( SlipSpeedMpS ) - 1 ) * 10 , 1 ) / 100 )
936945 {
937- NumOfSubstepsPS = Math . Min ( NumOfSubstepsPS + 5 , 130 ) ;
938- // Trace.TraceInformation("Algorithim Change - k1 - Number of Steps {0}", NumOfSubstepsPS);
946+ // NumOfSubstepsPS = Math.Min(NumOfSubstepsPS + 5, 130);
947+ // Trace.TraceInformation("Algorithim Change - k1 - Number of Steps {0}", NumOfSubstepsPS);
939948 dt = elapsedClockSeconds / NumOfSubstepsPS ;
940949 hdt = dt / 2 ;
941- }
950+ }
942951 }
943952 var k2 = GetAxleMotionVariation ( AxleSpeedMpS + k1 . Item1 * hdt , hdt ) ;
944953 var k3 = GetAxleMotionVariation ( AxleSpeedMpS + k2 . Item1 * hdt , hdt ) ;
945954 var k4 = GetAxleMotionVariation ( AxleSpeedMpS + k3 . Item1 * dt , dt ) ;
946-
955+
947956 AxleSpeedMpS += ( integratorError = ( k1 . Item1 + 2 * ( k2 . Item1 + k3 . Item1 ) + k4 . Item1 ) * dt / 6 ) ;
948957 AxlePositionRad += ( k1 . Item2 + 2 * ( k2 . Item2 + k3 . Item2 ) + k4 . Item2 ) * dt / 6 ;
949958 axleOutForceSumN += ( k1 . Item3 + 2 * ( k2 . Item3 + k3 . Item3 ) + k4 . Item3 ) ;
0 commit comments