From cdb9ee6cf4dfcfdcb6ac9f10bf3a0ef9120e4f6b Mon Sep 17 00:00:00 2001
From: Kevin Harrington <mad.hephaestus@gmail.com>
Date: Fri, 22 Dec 2023 01:46:38 -0500
Subject: [PATCH] walking better detect the real-time breakage

---
 MarcosWalk.groovy | 9 ++++-----
 1 file changed, 4 insertions(+), 5 deletions(-)

diff --git a/MarcosWalk.groovy b/MarcosWalk.groovy
index 97594ef..ba7f172 100644
--- a/MarcosWalk.groovy
+++ b/MarcosWalk.groovy
@@ -362,19 +362,19 @@ class BodyController{
 			bodyLoop=new Thread({
 				try {
 					// initialize real-time initial conditions
-					long start = System.currentTimeMillis();
-					long index=0;
 					// run body controller until disconnected
 					while(availible) {
+						long start = System.currentTimeMillis();
+						
 						// update the gait generation
 						loop();
 						// update the dynamics controller
 						runDynamics();
 						// compute the real-time condition
-						long elapsed =  System.currentTimeMillis()-(start +(numMsOfLoop*index) )
+						long elapsed =  System.currentTimeMillis()-(start )
 						def numMsOfLoopElapsed = numMsOfLoop-elapsed
 						// check for real-time overrun
-						if(numMsOfLoopElapsed<=16) {
+						if(numMsOfLoopElapsed<0) {
 							println "Real time in Body Controller broken! Loop took:"+elapsed+" sleep time "+numMsOfLoopElapsed
 							// this controller must run slower than the UI thread
 							Thread.sleep(16);
@@ -383,7 +383,6 @@ class BodyController{
 							Thread.sleep(numMsOfLoopElapsed);
 						}
 						// update the real-time index
-						index++
 					}
 				}catch(Throwable t) {
 					BowlerStudio.printStackTrace(t)