This script can be used to change the PID gains depending on the speed of the vehicle. The gains will change based on two speed ranges, and the change will happen the time that the the motor's speed will cross the critical point that separates the two ranges. To determine when to change the gains, the script uses only the operating parameters of channel one. When the speed of channel one has exceeded the critical point, the PID gains of both channels will be changed.
This script provides three ways of changing the PID gains:
- In the first implementation, the PID gains will change according to the command given by the user. This command in Roboteq controllers corresponds to the M query
- In the second implementation, the PID gains change according to the ramped command of the controller. So even if the given command by the user fluctuates across the critical point, the actual PID gains change will happen only when the ramped command will jump from the one range to the other. The ramp command on the Roboteq controller corresponds to the RMP query. Assuming a well tuned system, the ramped command and the speed of the vehicle will be very close
- In the third implementation, the PID gains will change according to the actual speed of the vehicle (F parameter in speed mode). In this mode, a speed dead band is being introduced, to avoid unnecessary PID gains change, when the speed fluctuates around the critical point.
Any of these three implementation can be configure by modifying the CHANGE_MODE parameter. Once the user have decided which implementation is better, he can remove the extra lines of code and integrate the script in his implementation.
The user must be aware that this is a sample script that has not been fully tested and he should modify and evaluate it under his own responsibility.
The script follows:
'*******************************************************************************************
' DYNAMIC PID GAINS CHANGE
'*******************************************************************************************
'Purpose:
'
'The purpose of this script is to give the user the ability to use different sets of
'PID gains at two different speed zones
'
'Use:
'
'The user can select between 3 different change modes:
'1.The PID gains change according to the speed command
'2.The PID gains change according to the ramped command
'3.The PID gains change according to the motor speed
'
'The user can configure any of the three modes by changing the CHANGE_METHOD PARAMETER
'The different PID gains, critical speed and speed deadband (applpicable ony on mode 3)
'can be configured in the 1st section of the script
'
'Disclaimer:
'
'The script is tested and validated by Roboteq and is believed to be fault-free.
'The possibility always exists, however, that the particular configuration and/or use
'condition uncovers a fault that escaped our validation test coverage. Always
'extensively test the script under your use conditions prior to deploying it in the field.
'
'*******************************************************************************************
option explicit ' Add this command at the begining of the script to ensure that any variable misspelling will be noticed by the compiler
'------------------------------------------------------------------------------------------
' 1. Script Parameters
'------------------------------------------------------------------------------------------
' Change these variables to set the different PID gains and speed threshold
' These variables should be defined at the start of the script
#define INITIAL_P 50000 'Speed P gain that will be used for slow speed. Values /1000000
#define NEXT_P 100000 'Speed P gain that will be used for higher speeds. Values /1000000
#define INITIAL_I 100000 'Speed I gain that will be used for slow speed. Values /1000000
#define NEXT_I 200000 'Speed I gain that will be used for higher speeds. Values /1000000
#define INITIAL_D 0 'Speed D gain that will be used for slow speed. Values /1000000
#define NEXT_D 0 'Speed D gain that will be used for higher speed. Values /1000000
#define SPEED_CHANGE 200 'The speed that the PID gains will change
#define SPEED_DEADBAND 10 'Configure this, to avoid needless PID change when speed flacuates around critical value. Applicable only in CHANE_MODE 3
#define CHANGE_METHOD 3 '1: PID gains will change according to the speed command
'2: PID gains will change according to the ramped command
'3: PID gains will change according to the speed command
'0: Only the "NEXT" gains will be used
'------------------------------------------------------------------------------------------
' 2. Parameter declarations and initializations
'------------------------------------------------------------------------------------------
' Add the following lines before the main loop
dim ramp1 as integer 'previous values
dim command1 as integer
dim speed1 as integer
dim nxtRamp1 as integer 'next values
dim nxtCommand1 as integer
dim nxtSpeed1 as integer
if (CHANGE_METHOD > 0) ' If the dynamic PID gains change is enabled, start with initial gains
setconfig(_KPG,1,INITIAL_P) 'P gain for channel 1
wait(1)
setconfig(_KPG,2,INITIAL_P) 'P gain for channel 2
wait(1)
setconfig(_KIG,1,INITIAL_I) 'I gain for channel 1
wait(1)
setconfig(_KIG,2,INITIAL_I) 'I gain for channel 2
wait(1)
setconfig(_KDG,1,INITIAL_D) 'I gain for channel 1
wait(1)
setconfig(_KDG,2,INITIAL_D) 'I gain for channel 2
else ' Else use only the values declared as "next"
setconfig(_KPG,1,NEXT_P) 'P gain for channel 1
wait(1)
setconfig(_KPG,2,NEXT_P) 'P gain for channel 2
wait(1)
setconfig(_KIG,1,NEXT_I) 'I gain for channel 1
wait(1)
setconfig(_KIG,2,NEXT_I) 'I gain for channel 2
wait(1)
setconfig(_KDG,1,NEXT_D) 'I gain for channel 1
wait(1)
setconfig(_KDG,2,NEXT_D) 'I gain for channel 2
end if
'------------------------------------------------------------------------------------------
' 3. Main Loop
'------------------------------------------------------------------------------------------
'Add the below lines to your main loop
'This script can work at any loop period
Main:
command1 = getvalue(_M, 1) 'Speed command
ramp1 = getvalue(_RMP,1) 'Ramped Command
speed1 = getvalue(_F,1) 'Feedback value
wait(1) 'Loop period is 1 ms
if (CHANGE_METHOD>0) then 'If change method is enabled go to the sub routine
goSub dynamic_PID 'The sub routine call should be after the wait function
end if 'So any change in the operating parameters can be determined by the script
'setcommand(_VAR,1,getconfig(_KPG,1)) 'For debugging. To monitor PID gains on VARs 1~3 on the run tab
'setcommand(_VAR,2,getconfig(_KIG,1))
'setcommand(_VAR,3,getconfig(_KDG,1))
goto main
'------------------------------------------------------------------------------------------
' 4. PID ROUTINES
'------------------------------------------------------------------------------------------
dynamic_PID:
'The previous and the current parameters are compared to see if the PID parameters need to change
'Only channel 1 will be taken as a reference
if (CHANGE_METHOD = 1) 'change according to command
nxtCommand1 = getvalue(_M,1) 'next motor command
if ((abs(nxtCommand1) > abs(SPEED_CHANGE)) and (abs(command1)<=abs(SPEED_CHANGE))) then 'if current motor command is beyond critical speed and previous command was below
gosub give_Next 'Give next PID values
elseif ((abs(nxtCommand1) < abs(SPEED_CHANGE)) and (abs(command1)>=abs(SPEED_CHANGE))) then 'if current motor command is below critical speed and previous command was beyond
gosub give_Initial 'Give initial PID values
end if
elseif (CHANGE_METHOD = 2) 'change according to ramped command
nxtRamp1 = getvalue(_RMP,1) 'next ramped command
if ((abs(nxtRamp1) > abs(SPEED_CHANGE)) and (abs(Ramp1)<=abs(SPEED_CHANGE))) then 'if the ramped command has just surpassed the critical speed
gosub give_Next 'Give next PID values
elseif ((abs(nxtRamp1) < abs(SPEED_CHANGE)) and (abs(Ramp1)>=abs(SPEED_CHANGE))) then 'if the ramped command has been just decreased below critical speed
gosub give_Initial 'Give initial PID values
end if
else 'Change according to feedback
nxtSpeed1 = getvalue(_F,1) 'next feedback value
if ((abs(nxtSpeed1) > abs(SPEED_CHANGE+SPEED_DEADBAND)) and (abs(speed1)<=abs(SPEED_CHANGE+SPEED_DEADBAND))) then 'if the vehicle has just surpassed the critical speed
gosub give_Next 'Give next PID values
elseif ((abs(nxtSpeed1) < abs(SPEED_CHANGE-SPEED_DEADBAND)) and (abs(speed1)>=abs(SPEED_CHANGE-SPEED_DEADBAND))) then 'if the vehicle speed has been just decreased below critical speed
gosub give_Initial 'Give initial PID values
end if
end if
return
give_Initial:
setconfig(_KPG,1,INITIAL_P)
setconfig(_KPG,2,INITIAL_P)
setconfig(_KIG,1,INITIAL_I)
setconfig(_KIG,2,INITIAL_I)
setconfig(_KDG,1,INITIAL_D)
setconfig(_KDG,2,INITIAL_D)
return
give_Next:
setconfig(_KPG,1,NEXT_P)
setconfig(_KPG,2,NEXT_P)
setconfig(_KIG,1,NEXT_I)
setconfig(_KIG,2,NEXT_I)
setconfig(_KDG,1,NEXT_D)
setconfig(_KDG,2,NEXT_D)
return