diff --git a/simulation/controllers/olympian/standingBalance.py b/simulation/controllers/olympian/standingBalance.py
index 7fc1576..488a09e 100644
--- a/simulation/controllers/olympian/standingBalance.py
+++ b/simulation/controllers/olympian/standingBalance.py
@@ -5,12 +5,10 @@
 from controller import Robot, Accelerometer, Gyro
 
 def integral(error, priorIntegral):
-  global timestep
-  return priorIntegral + error * timestep
+  return priorIntegral + error * (TIMESTEP+1 - TIMESTEP)
 
 def derivative(error, priorError):
-  global timestep
-  return (error-priorError)/timestep
+  return (error-priorError)/(TIMESTEP+1 - TIMESTEP)
 
 def actuatorMovement(robot, pidOutput):
   #Inverse Kinematic Equation
@@ -24,18 +22,45 @@ def controllerPID(robot, error, priorError, priorIntegral):
   Kp = 1
   Ki = 0
   Kd = 0
-
   #Usually not needed, but just in case we ever need nonstop motion
   xBias = 0
   yBias = 0
 
-  derivativeX = derivative(error[0], priorError[0])
-  derivativeY = derivative(error[1], priorError[1])
+  nominalValue = 0.0
+  Kc = 1.0 # Kc is the controller gain
+  tauI = 0.0 # tauI is the reset time, which is a tuning param for integral
+  tauD = 0.0 # tauD is derivative time. Tuning param for derivative
+  maxMotor = 1.0 # How big the signal that goes to the actuator is
+  minMotor = 0.0 # How small the signal that goes to the actuator is
+  
   integralX = integral(error[0], priorIntegral[0])
   integralY = integral(error[1], priorIntegral[1])
+  
+  if(TIMESTEP >= 1):
+    derivativeX = derivative(error[0], priorError[0])
+    derivativeY = derivative(error[1], priorError[1])
+  else:
+    derivativeX = 0.0
+    derivativeY = 0.0
 
-  ux = Kp*error[0] + Ki * integralX + Kd * derivativeX + xBias
-  uy = Kp*error[1] + Ki * integralY + Kd * derivativeY + yBias
+
+  ux = nominalValue + Kc*error[0] + Kc/tauI * integralX + Kc * tauD * derivativeX
+  
+  if(ux > maxMotor):
+    ux = maxMotor
+    integralX = integralX - error[0]*(TIMESTEP+1 - TIMESTEP)
+  elif(ux < minMotor):
+    ux = minMotor
+    integralX = integralX - error[0]*(TIMESTEP+1 - TIMESTEP)
+  
+  uy = nominalValue + Kc*error[1] + Kc/tauI * integralY + Kc * tauD * derivativeY
+
+  if(uy > maxMotor):
+    uy = maxMotor
+    integralY = integralY - error[1]*(TIMESTEP+1 - TIMESTEP)
+  elif(ux < minMotor):
+    uy = minMotor
+    integralY = integralY - error[1]*(TIMESTEP+1 - TIMESTEP)
 
   priorError = error
   priorIntegral = [integralX, integralY]
@@ -65,8 +90,8 @@ def main():
   robot = Robot()
   
   # get the time step of the current world.
-  global timestep
-  timestep = int(robot.getBasicTimeStep())
+  global TIMESTEP
+  TIMESTEP = int(robot.getBasicTimeStep())
   
   # gyroscope, accelorometer
   gyro = robot.getDevice("gyro.wbt name")
@@ -84,7 +109,7 @@ def main():
   priorError = [0,0]
   priorIntegral = [0, 0]
 
-  while robot.step(timestep) != -1:
+  while robot.step(TIMESTEP) != -1:
     xObs, yObs = calculateZMP(gyro, accel)
     error = [desiredXZMP-xObs, desiredYZMP-yObs]
     controllerPID(robot, error, priorError, priorIntegral)
diff --git a/simulation/worlds/.olympian.wbproj b/simulation/worlds/.olympian.wbproj
index 0a21dad..35e72f3 100644
--- a/simulation/worlds/.olympian.wbproj
+++ b/simulation/worlds/.olympian.wbproj
@@ -1,8 +1,9 @@
 Webots Project File version R2021a
-perspectives: 000000ff00000000fd00000003000000000000000000000000fc0100000002fc00000000ffffffff0000000000fffffffc0200000001fb00000012005300630065006e0065005400720065006501000000000000039f0000000000000000fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000006900ffffff000000010000023a000003b6fc0200000002fb0000001400540065007800740045006400690074006f0072010000001a000003b60000004100fffffffb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c01000001b5000003d000000000000000000000000300000780000001e8fc0100000001fb0000000e0043006f006e0073006f006c0065010000000000000740000000000000000000000544000003b600000004000000040000000100000008fc00000000
+perspectives: 000000ff00000000fd00000003000000000000000000000000fc0100000002fc00000000ffffffff0000000000fffffffc0200000001fb00000012005300630065006e0065005400720065006501000000000000039f0000000000000000fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000006900ffffff000000010000023a000003ccfc0200000002fb0000001400540065007800740045006400690074006f00720100000016000001830000008900fffffffb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c010000019b000002470000003900ffffff0000000300000780000001e8fc0100000001fb0000000e0043006f006e0073006f006c00650100000000000007400000000000000000000004fc000003cc00000004000000040000000100000008fc00000000
 simulationViewPerspectives: 000000ff00000001000000020000016c000003d60100000002010000000101
 sceneTreePerspectives: 000000ff0000000100000002000000c0000000fa0100000002010000000201
 maximizedDockId: -1
 centralWidgetVisible: 1
 orthographicViewHeight: 1
-textFiles: -1
+textFiles: 0 "controllers/olympian/standingBalance.py"
+consoles: Console:All:All