diff --git a/tools/pidmayuresh3.py b/tools/pidmayuresh3.py index 446a651..1fd1f2d 100644 --- a/tools/pidmayuresh3.py +++ b/tools/pidmayuresh3.py @@ -1,53 +1,31 @@ -import numpy as np -import math -import concore -import sys -dT = 0.1 - -sp = concore.tryparam('sp', 67.5) -Kp = concore.tryparam('Kp', 0.075) -Ki = concore.tryparam('Ki', 0.02) -Kd = concore.tryparam('Kd', 0.005) -freq = concore.tryparam('freq',30) -sigout = concore.tryparam('sigout',True) -cin = concore.tryparam('cin', 'hr') - -def pid_controller(state, ym, sp, Kp, Ki, Kd, sigout, cin, low, up): - Prev_Error = state[0] - I = state[1] - if cin == 'hr': - Error = sp - ym[1] - elif cin == 'map': - Error = sp - ym[0] - else: - print('invalid control input '+cin) - sys.exit(1) - P = Error - I = I + Error*dT - D = (Error - Prev_Error )/dT - amp = Kp*P + Ki*I + Kd*D - Prev_Error = Error - if sigout: - amp = (up-low)/(1.0 + math.exp(amp)) + low - state = [Prev_Error, I] - return (state, amp) - - -concore.default_maxtime(150) -concore.delay = 0.02 -init_simtime_ym = "[0.0, 70.0,91]" -ym = np.array(concore.initval(init_simtime_ym)) -state = [0.0, 0.0] -print("Mayuresh's PID controller: sp is "+str(sp)) -print(concore.params) -while(concore.simtime