Scenario

 

There are a lot of cases where we desire that the motors will hold position when our motors are idle. For example, if our vehicle is placed on an inclined surface and we do not want it to roll back/forward or if we want our vehicle to stay still while some other process is taking place on it, it might be a case where it is loaded or unloaded.


One of our closed loop modes, closed loop speed position, offers such a functionality, where the motors are kept in position and the controller will react to any force trying to move the motors from their intended position. On the same time a successful tuning in speed position mode is considerably harder to achieve than what it is in speed mode, when the application has high speeds involved. 


In order to get the best out of the two modes we can create a script where the controller will switch from one mode to the other depending on the command that we are applying to the motor.


How it works


In order to make sure that there will be no mode change, from speed mode to speed position mode, while the motors are still spinning. We must make sure that we check the speed, the power and the command of the controller, so that we are able to identify that the motors are going to stop and the power applied is low. Indeed the script checks the conditions above and makes sure that we are about to stop when this change happens.


The same check is not required when switching back to speed mode since the motors are already stopped.


In order for the speed position mode to be effectively holding position the speed position mode must also be tuned, and then it should be tested how good the motors hold position according to the Position PID gains. We have to then tune accordingly in order to find a balance between holding power and stability.


!Warning!

The script below is just an example of how this functionality can be implemented. No exhaustive tests have taken place on this code and no guarantees can be given regarding its good operation. Test the script and modify it according to the application requirements. Roboteq is not liable for any issues, injuries or damaged hardware that might occur from the use of this script.


 
 
'#############################################################################################
'This script changes the Operating mode of the Closed Loop. When the command is 0 and the robot
'stops the controller will switch to Closed Loop Speed Position Mode to hold the position. 
'When the Motors start moving it will change to Closed Loop Speed Mode. Feedback for speed and
' position is read from Encoder. Limits for Command, Speed, Power and Ramp have been set for safety
' and proper operation, when changing modes.                                                                      
'#############################################################################################
'THE SCRIPT IS PROVIDED AS IS, WITHOUT ANY WARRANTIES. PLEASE MAKE SURE IT PERFORMS AS EXPECTED 
'AND TEST THOROUGHLY
'#############################################################################################
 
option explicit
 
'Set below the normal operating mode (RUN_MODE) and the mode to use when the vehicle stops (STOP_MODE)
'define as follows:
 
 
#define RUN_MODE 1
#define STOP_MODE 0
#define SENSOR _S 'Use _S for encoder and _BS for internal sensor
 
'modes:
 
'0: Open Loop
'1: Closed Loop Speed
'2: Closed Loop Position Relative
'3: Closed Loop Count Position
'4: Closed Loop Position Tracking
'5: Closed Loop Torque
'6: Closed Loop Speed Position
 
dim speed1 as integer
dim speed2 as integer
dim command1 as integer
dim command2 as integer
dim power1 as integer
dim power2 as integer
dim stop as integer
 
stop = 1
setconfig(_MMOD,1,STOP_MODE)
wait(1)        
setconfig(_MMOD,2,STOP_MODE)
wait(1)       
 
main: 
   
speed1=abs(getvalue(SENSOR,1))      'get speed value motor 1'
speed2=abs(getvalue(SENSOR,2))      'get speed value motor 2'
command1=abs(getvalue(_M,1))     'get command 1 value'
command2=abs(getvalue(_M,2))     'get command 2 value'
power1=abs(getvalue(_P,1))      'get power 1 value'
power2=abs(getvalue(_P,2))      'get power 2 value'
 
'Sets the Mode to STOP_MODE when motors stop'
 
If ((speed1<20) and (power1<50) and (command1=0) and (speed2<20) and (power2<50) and (command2=0) and stop = 0) then
                              
        setconfig(_MMOD,1,STOP_MODE)  
        wait(1)
        setconfig(_MMOD,2,STOP_MODE)
        wait(1)
        stop=1
        
end if    
 
'Sets the Mode to RUN_MODE when motors start moving
 
If ((command1>0) and (command2>0) and (stop=1)) then 
 
        setconfig(_MMOD,1,RUN_MODE)
        wait(1)        
        setconfig(_MMOD,2,RUN_MODE)
        wait(1)    
        stop=0
        
end if
 
wait(1) 'Loop period 1 ms
 
goto main