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

Fixed some of the PID code

Browse Source
This commit is contained in:
Ram Reddy 2021-02-12 15:03:51 -08:00
parent bf120ab2b3
commit fbb9fa7455
No known key found for this signature in database
GPG Key ID: 2CD6B38BE14BDE53
2 changed files with 40 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -5,12 +5,10 @@
from controller import Robot, Accelerometer, Gyro from controller import Robot, Accelerometer, Gyro
def integral(error, priorIntegral): def integral(error, priorIntegral):
global timestep return priorIntegral + error * (TIMESTEP+1 - TIMESTEP)
return priorIntegral + error * timestep
def derivative(error, priorError): def derivative(error, priorError):
global timestep return (error-priorError)/(TIMESTEP+1 - TIMESTEP)
return (error-priorError)/timestep
def actuatorMovement(robot, pidOutput): def actuatorMovement(robot, pidOutput):
#Inverse Kinematic Equation #Inverse Kinematic Equation
@ -24,18 +22,45 @@ def controllerPID(robot, error, priorError, priorIntegral):
Kp = 1 Kp = 1
Ki = 0 Ki = 0
Kd = 0 Kd = 0
#Usually not needed, but just in case we ever need nonstop motion #Usually not needed, but just in case we ever need nonstop motion
xBias = 0 xBias = 0
yBias = 0 yBias = 0
derivativeX = derivative(error[0], priorError[0]) nominalValue = 0.0
derivativeY = derivative(error[1], priorError[1]) 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]) integralX = integral(error[0], priorIntegral[0])
integralY = integral(error[1], priorIntegral[1]) integralY = integral(error[1], priorIntegral[1])
ux = Kp*error[0] + Ki * integralX + Kd * derivativeX + xBias if(TIMESTEP >= 1):
uy = Kp*error[1] + Ki * integralY + Kd * derivativeY + yBias derivativeX = derivative(error[0], priorError[0])
derivativeY = derivative(error[1], priorError[1])
else:
derivativeX = 0.0
derivativeY = 0.0
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 priorError = error
priorIntegral = [integralX, integralY] priorIntegral = [integralX, integralY]
@ -65,8 +90,8 @@ def main():
robot = Robot() robot = Robot()
# get the time step of the current world. # get the time step of the current world.
global timestep global TIMESTEP
timestep = int(robot.getBasicTimeStep()) TIMESTEP = int(robot.getBasicTimeStep())
# gyroscope, accelorometer # gyroscope, accelorometer
gyro = robot.getDevice("gyro.wbt name") gyro = robot.getDevice("gyro.wbt name")
@ -84,7 +109,7 @@ def main():
priorError = [0,0] priorError = [0,0]
priorIntegral = [0, 0] priorIntegral = [0, 0]
while robot.step(timestep) != -1: while robot.step(TIMESTEP) != -1:
xObs, yObs = calculateZMP(gyro, accel) xObs, yObs = calculateZMP(gyro, accel)
error = [desiredXZMP-xObs, desiredYZMP-yObs] error = [desiredXZMP-xObs, desiredYZMP-yObs]
controllerPID(robot, error, priorError, priorIntegral) controllerPID(robot, error, priorError, priorIntegral)

5
simulation/worlds/.olympian.wbproj
View File

@ -1,8 +1,9 @@
Webots Project File version R2021a Webots Project File version R2021a
perspectives: 000000ff00000000fd00000003000000000000000000000000fc0100000002fc00000000ffffffff0000000000fffffffc0200000001fb00000012005300630065006e0065005400720065006501000000000000039f0000000000000000fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000006900ffffff000000010000023a000003b6fc0200000002fb0000001400540065007800740045006400690074006f0072010000001a000003b60000004100fffffffb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c01000001b5000003d000000000000000000000000300000780000001e8fc0100000001fb0000000e0043006f006e0073006f006c0065010000000000000740000000000000000000000544000003b600000004000000040000000100000008fc00000000 perspectives: 000000ff00000000fd00000003000000000000000000000000fc0100000002fc00000000ffffffff0000000000fffffffc0200000001fb00000012005300630065006e0065005400720065006501000000000000039f0000000000000000fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000006900ffffff000000010000023a000003ccfc0200000002fb0000001400540065007800740045006400690074006f00720100000016000001830000008900fffffffb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c010000019b000002470000003900ffffff0000000300000780000001e8fc0100000001fb0000000e0043006f006e0073006f006c00650100000000000007400000000000000000000004fc000003cc00000004000000040000000100000008fc00000000
simulationViewPerspectives: 000000ff00000001000000020000016c000003d60100000002010000000101 simulationViewPerspectives: 000000ff00000001000000020000016c000003d60100000002010000000101
sceneTreePerspectives: 000000ff0000000100000002000000c0000000fa0100000002010000000201 sceneTreePerspectives: 000000ff0000000100000002000000c0000000fa0100000002010000000201
maximizedDockId: -1 maximizedDockId: -1
centralWidgetVisible: 1 centralWidgetVisible: 1
orthographicViewHeight: 1 orthographicViewHeight: 1
textFiles: -1 textFiles: 0 "controllers/olympian/standingBalance.py"
consoles: Console:All:All