Commits

iorodeo  committed 7a2a8cd

Modified so that the controller gains can be changed while the system is
running. Also, added a separate set of controller gains for inertial
trajectory mode. The robot_control object automatically swaps the "position
control" and "inertial control" gains as needed.

  • Participants
  • Parent commits c8edbd1

Comments (0)

Files changed (3)

File software/motor_cmd_source/nodes/base_controller.py

         igain = req.igain
         dgain = req.dgain
 
+        if self.enabled:
+            flag = False
+            message = 'controller must be disabled to set gains'
+
         if pgain < 0:
             flag = False
             message = 'pgain must be >= 0,'
         self.PIDMsg.pTerm = self.controller.pTerm
         self.PIDMsg.iTerm = self.controller.iTerm
         self.PIDMsg.dTerm = self.controller.dTerm
+        self.PIDMsg.error = self.controller.lastError
         self.PIDPub.publish(self.PIDMsg)
 
-
-
-
 # -----------------------------------------------------------------------------
 if __name__ == '__main__':
     rospy.init_node('base_controller')

File software/msg_and_srv/msg/PIDMsg.msg

 float32 pTerm
 float32 iTerm
 float32 dTerm
+float32 error
 

File software/utilities/src/utilities/robot_control.py

 from msg_and_srv.srv import SetLogFile
 from msg_and_srv.srv import RelToAbsCmd
 from msg_and_srv.srv import SetDynamParams
+from msg_and_srv.srv import SetControllerGains
+from msg_and_srv.srv import GetControllerGains
 
 # Actions
 from actions.msg import SetptOutscanAction
                 NodeEnable,
                 )
 
+        rospy.wait_for_service('controller_set_gains')
+        self.controllerSetGainsProxy = rospy.ServiceProxy(
+                'controller_set_gains',
+                SetControllerGains,
+                )
+
+        rospy.wait_for_service('controller_get_gains')
+        self.controllerGetGainsProxy = rospy.ServiceProxy(
+                'controller_get_gains',
+                GetControllerGains,
+                )
+
         if self.mode == 'captive trajectory':
             rospy.wait_for_service('dynamics_enable')
             self.dynamicsEnableProxy = rospy.ServiceProxy(
         return resp.status, resp.message
 
     def enableControllerMode(self):
+        """
+        Enables the position controller
+        """
         resp = self.controllerEnableProxy(True)
         return resp.status, resp.message
 
     def disableControllerMode(self):
+        """
+        Disabled the position controller
+        """
         resp = self.controllerEnableProxy(False)
         return resp.status, resp.message
 
+    def setControllerGains(self,pgain,igain,dgain):
+        """
+        Set the controller gains. Note, the position controller node must
+        be disabled in order for you to do this.
+        """
+        resp = self.controllerSetGainsProxy(pgain,igain,dgain)
+        return resp.flag, resp.message
+
+    def getControllerGains(self):
+        """
+        Get the current controller gains.
+        """
+        resp = self.controllerGetGainsProxy()
+        return resp.pgain, resp.igain, resp.dgain
+
+    def setPositionControlGains(self):
+        """
+        Set controller gains to values appropriate to position control mode.
+        """
+        pgain = rospy.get_param('controller_pgain')
+        igain = rospy.get_param('controller_igain')
+        dgain = rospy.get_param('controller_dgain')
+        flag, message = self.setControllerGains(pgain,igain,dgain)
+        #print 'setting position control gains', flag, message
+        return flag, message
+
+    def setInertialControlGains(self):
+        """
+        Set controller gains to values appropriate for inertial trajectory mode.
+        """
+        pgain = rospy.get_param('inertial_pgain')
+        igain = rospy.get_param('inertial_igain')
+        dgain = rospy.get_param('inertial_dgain')
+        flag, message = self.setControllerGains(pgain,igain,dgain)
+        #print 'setting inertial control gains', flag, message
+        return flag, message
+
     def enableDynamics(self):
         """
         Enables the dynamics node output.
             goal.position_array = list(setptValues)
 
             # Send goal to action server
+            self.setPositionControlGains()
             self.enableControllerMode()
             self.setptActionClient.send_goal(
                     goal,
         goal.actuator_array = actuatorArray
 
         # Send goal to action sever
-        self.enableControllerMode()
         if self.mode == 'captive trajectory':
+            self.setPositionControlGains()
             self.setPointRelToAbsReset()
             self.enableDynamics()
         elif self.mode == 'inertial trajectory':
+            self.setInertialControlGains()
             self.enableLaserSetpt()
         else:
             raise ValueError, 'unknown mode {0}'.format(self.mode)
+        self.enableControllerMode()
 
         self.actuatorActionClient.send_goal(
                 goal,