rushilwiz/engine-software
1
0
Fork 0
mirror of https://github.com/PotentiaRobotics/engine-software.git synced 2025-04-17 18:40:18 -04:00
Code Issues Actions Packages Projects Releases Wiki Activity

cleaned up PID code

Browse Source
This commit is contained in:
John_Kim 2021-02-20 00:43:19 -05:00
parent 48a5660645
commit 54a5226a31
1 changed files with 6 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
simulation/controllers/olympian/standingBalance.py
View File

@ -5,10 +5,10 @@
from controller import Robot, Accelerometer, Gyro from controller import Robot, Accelerometer, Gyro
def integral(error, priorIntegral): def integral(error, priorIntegral):
return priorIntegral + error * (TIMESTEP+1 - TIMESTEP) return priorIntegral + error * (TIMESTEP)
def derivative(error, priorError): def derivative(error, priorError):
return (error-priorError)/(TIMESTEP+1 - TIMESTEP) return (error-priorError)/(TIMESTEP)
def actuatorMovement(robot, pidOutput): def actuatorMovement(robot, pidOutput):
#Inverse Kinematic Equation #Inverse Kinematic Equation
@ -48,19 +48,19 @@ def controllerPID(robot, error, priorError, priorIntegral):
if(ux > maxMotor): if(ux > maxMotor):
ux = maxMotor ux = maxMotor
integralX = integralX - error[0]*(TIMESTEP+1 - TIMESTEP) integralX = integralX - error[0]*(TIMESTEP)
elif(ux < minMotor): elif(ux < minMotor):
ux = minMotor ux = minMotor
integralX = integralX - error[0]*(TIMESTEP+1 - TIMESTEP) integralX = integralX - error[0]*(TIMESTEP)
uy = nominalValue + Kc*error[1] + Kc/tauI * integralY + Kc * tauD * derivativeY uy = nominalValue + Kc*error[1] + Kc/tauI * integralY + Kc * tauD * derivativeY
if(uy > maxMotor): if(uy > maxMotor):
uy = maxMotor uy = maxMotor
integralY = integralY - error[1]*(TIMESTEP+1 - TIMESTEP) integralY = integralY - error[1]*(TIMESTEP)
elif(ux < minMotor): elif(ux < minMotor):
uy = minMotor uy = minMotor
integralY = integralY - error[1]*(TIMESTEP+1 - TIMESTEP) integralY = integralY - error[1]*(TIMESTEP)
priorError = error priorError = error
priorIntegral = [integralX, integralY] priorIntegral = [integralX, integralY]
@ -81,7 +81,6 @@ def calculateZMP(gyro, accel):
return xObs, yObs return xObs, yObs
# Might have to actually code this later
def calculateCOM(robot): def calculateCOM(robot):
# links needed to calculate COM # links needed to calculate COM
torso = robot.getDevice("torsoMotor") torso = robot.getDevice("torsoMotor")
@ -94,7 +93,6 @@ def calculateCOM(robot):
centerOfMass = [0, 0, 0, 0] centerOfMass = [0, 0, 0, 0]
for i in links: for i in links:
# instantiation of values # instantiation of values
# need to add code to get joint angles # need to add code to get joint angles
if i == torso: if i == torso: