[actuators] Update controller faulhaber (#3347)

conf/modules/actuators_faulhaber.xml

@@ -4,20 +4,18 @@
   <doc>
     <description>
       Actuators Driver for the Faulhaber controller
-      Currently initialization is done manually.
+      Based on the velocity controller inside the MC5004 controller, a simple position controller is implemented.
-      Set mode to:
-       - 2
-       - 4
-       - 2 (it will home slowly)
-       - Wait until homing is finished!
-       - Back to 1 and the wing will move
     </description>
     <configure name="FAULHABER_DEV" value="UARTX" description="UART port (default UART4)"/>
+    <define name="FAULHABER_P_GAIN" value="0.07" description="Proportional gain for the position controller"/>
+    <define name="FAULHABER_MAX_VELOCITY" value="14000" description="Maximum velocity for the position controller"/>
   </doc>
   <settings>
     <dl_settings>
       <dl_settings NAME="Faulhaber">
-        <dl_setting var="faulhaber.mode" min="0" step="1" max="4" shortname="mode" module="modules/actuators/actuators_faulhaber" handler="SetMode"/>
+        <dl_setting var="faulhaber.mode" min="0" step="1" max="3" shortname="mode" values="INIT|VELOCITY|ERROR" module="modules/actuators/actuators_faulhaber" handler="SetMode"/>
+        <dl_setting var="faulhaber.p_gain" min="0" step="0.001" max="1"/>
+        <dl_setting var="faulhaber.max_velocity" min="0" step="100" max="100000"/>
         <dl_setting var="faulhaber.setpoint_position" min="0" step="1000" max="3600000"/>
         <dl_setting var="faulhaber.real_position" min="0" step="1000" max="3300000"/>
       </dl_settings>
@@ -32,6 +30,7 @@
     <file name="actuators_faulhaber.h"/>
   </header>
   <init fun="actuators_faulhaber_init()"/>
+  <!-- Controller runs at half the speed of the periodic -->
   <periodic fun="actuators_faulhaber_periodic()" freq="50"/>
   <event fun="actuators_faulhaber_event()" />
   <makefile target="!sim">

sw/airborne/modules/actuators/actuators_faulhaber.c

@@ -32,8 +32,19 @@
 #include <stdio.h>
 #include <string.h>
 
+/* Proportional gain on the velocity for the position controller */
+#ifndef FAULHABER_P_GAIN
+#define FAULHABER_P_GAIN 0.07
+#endif
+
+/* Maximum velocity for the position controller */
+#ifndef FAULHABER_MAX_VELOCITY
+#define FAULHABER_MAX_VELOCITY 14000
+#endif
+
 static struct uart_periph *faulhaber_dev = &(FAULHABER_DEV);
 
+/* Faulhaber message parser */
 struct faulhaber_parser_t {
   uint8_t state;
   uint8_t node_nb;
@@ -160,145 +171,164 @@ void actuators_faulhaber_init(void)
   faulhaber.mode = FH_MODE_INIT;
   faulhaber.state = 0;
   faulhaber.setpoint_position = 0;
-  faulhaber.target_position = 0;
+  faulhaber.real_position = 0;
+  faulhaber.homing_completed = false;
+  faulhaber.position_ready = false;
+  faulhaber.target_reached = false;
+  faulhaber.p_gain = FAULHABER_P_GAIN;
+  faulhaber.max_velocity = FAULHABER_MAX_VELOCITY;
 }
 
-
 void actuators_faulhaber_periodic(void)
 {
+  static float last_time = 0;
   switch (faulhaber.mode) {
 
-    /* HOME MODE */
+    /* Initialize and home the motor */
-    case FH_MODE_HOME:
+    case FH_MODE_INIT:
-      switch (faulhaber.state) {
+      switch(faulhaber.state) {
         case 0: {
-          // Set the homing mode
+          // Start the controller boot timer
-          uint8_t data[] = { 0x60, 0x60, 0x00, 0x06 }; // Set 0x6060.00 to 0x06:  Homing mode
+          last_time = get_sys_time_float();
-          faulhaber_send_command(faulhaber_dev, 0x02, data, 4);
           faulhaber.state++;
           break;
         }
         case 1: {
+          // Check if the controller is ready, 5 seconds delay
+          if (get_sys_time_float() - last_time < 5.0)
+            break;
+          
+          // Enable power on the motor
+          static uint8_t data[] = { 0x40, 0x60, 0x00, 0x0E, 0x00}; // Set 0x6040.00 to 0x000E: Try to start
+          faulhaber_send_command(faulhaber_dev, 0x02, data, 5);
+          last_time = get_sys_time_float();
+          faulhaber.state++;
+          break;
+        }
+        case 2: {
+          // Wait 10ms for the next command
+          if(get_sys_time_float() - last_time < 0.01)
+            break;
+          
+          // Set to homing mode
+          static uint8_t data[] = { 0x60, 0x60, 0x00, 0x06 }; // Set 0x6060.00 to 0x06:  Homing mode
+          faulhaber_send_command(faulhaber_dev, 0x02, data, 4);
+          last_time = get_sys_time_float();
+          faulhaber.state++;
+          break;
+        }
+        case 3: {
+          // Wait 10ms for the next command
+          if(get_sys_time_float() - last_time < 0.01)
+            break;
+          
           // Set the homing method
-          uint8_t data[] = { 0x98, 0x60, 0x00, 0x11 }; // Set 0x6098.00 to 0x11:  Homing method to 17
+          static uint8_t data[] = { 0x98, 0x60, 0x00, 0x11 }; // Set 0x6098.00 to 0x11:  Homing method to 17
           faulhaber_send_command(faulhaber_dev, 0x02, data, 4);
+          last_time = get_sys_time_float();
           faulhaber.state++;
           break;
         }
-        case 2: {
+        case 4: {
+          // Wait 10ms for the next command
+          if(get_sys_time_float() - last_time < 0.01)
+            break;
+          
           // Enable operation
-          uint8_t data[] = { 0x40, 0x60, 0x00, 0x0F, 0x00}; // Set 0x6040.00 to 0x000F: Enable operation
+          static uint8_t data[] = { 0x40, 0x60, 0x00, 0x0F, 0x00}; // Set 0x6040.00 to 0x000F: Enable operation
           faulhaber_send_command(faulhaber_dev, 0x02, data, 5);
+          last_time = get_sys_time_float();
           faulhaber.state++;
           break;
         }
-        case 3: {
+        case 5: {
+          // Wait 10ms for the next command
+          if(get_sys_time_float() - last_time < 0.01)
+            break;
+          
           // Start moving
-          uint8_t data[] = { 0x40, 0x60, 0x00, 0x1F, 0x00}; // Set 0x6040.00 to 0x001F: Start moving
+          static uint8_t data[] = { 0x40, 0x60, 0x00, 0x1F, 0x00}; // Set 0x6040.00 to 0x001F: Start moving
           faulhaber_send_command(faulhaber_dev, 0x02, data, 5);
+          last_time = get_sys_time_float();
           faulhaber.state++;
           break;
         }
-        default:
+        case 6: {
-          // Goto position mode
+          // Check if homing is completed
-          faulhaber.mode = FH_MODE_INIT;
+          if(faulhaber.homing_completed) {
-          faulhaber.state = 0;
+            // Continue initialization
+            last_time = get_sys_time_float();
+            faulhaber.state++;
+          }
+          // Timeout after 40 seconds
+          else if(get_sys_time_float() - last_time > 40.0) {
+            faulhaber.mode = FH_MODE_ERROR;
+            faulhaber.state = 0;
+          }
           break;
+        }
+        case 7: {
+          // Wait 10ms for the next command
+          if(get_sys_time_float() - last_time < 0.01)
+            break;
+          
+          // Set to position mode
+          static uint8_t data[] = { 0x60, 0x60, 0x00, 0x03 }; // Set 0x6060.00 to 0x03: Velocity mode
+          faulhaber_send_command(faulhaber_dev, 0x02, data, 4);
+          last_time = get_sys_time_float();
+          faulhaber.state++;
+          break;
+        }
+        case 8: {
+          // Wait 10ms for the next command
+          if(get_sys_time_float() - last_time < 0.01)
+            break;
+          
+          // Enable operation
+          static uint8_t data[] = { 0x40, 0x60, 0x00, 0x0F, 0x00}; // Set 0x6040.00 to 0x000F: Enable operation
+          faulhaber_send_command(faulhaber_dev, 0x02, data, 5);
+          last_time = get_sys_time_float();
+          faulhaber.state++;
+          break;
+        }
+        case 9: {
+          // Wait 10ms for the next command
+          if(get_sys_time_float() - last_time < 0.01)
+            break;
+          
+          // Go to position mode
+          faulhaber.mode = FH_MODE_VELOCITY;
+          faulhaber.state = 0;
+        }
       }
       break;
 
-    /* POSITION MODE */
+    /* FH_MODE_VELOCITY */
-    case FH_MODE_POSITION:
+    case FH_MODE_VELOCITY: {
+      // Request position and set the new velocity target
       switch (faulhaber.state) {
         case 0: {
-          uint8_t data[] = { 0x60, 0x60, 0x00, 0x01 }; // Set 0x6060.00 to 0x01:  Position mode
+          // Request the position
-          faulhaber_send_command(faulhaber_dev, 0x02, data, 4);
+          uint8_t data[] = { 0x64, 0x60, 0x00}; // Get 0x6064.00: Get the actual position
+          faulhaber_send_command(faulhaber_dev, 0x01, data, 3);
           faulhaber.state++;
           break;
         }
-        case 1: {
+        default: {
-          // Set the target position
+          // Calculate the new velocity target
-          faulhaber.target_position = faulhaber.setpoint_position;
+          int32_t poss_err = (faulhaber.setpoint_position - faulhaber.real_position);
-          uint8_t data[] = { 0x7A, 0x60, 0x00, (faulhaber.target_position & 0xFF), ((faulhaber.target_position >> 8) & 0xFF), ((faulhaber.target_position >> 16) & 0xFF), ((faulhaber.target_position >> 24) & 0xFF) }; // Set 0x607A.00 to 0x00000000:  Target position 0
+          faulhaber.target_velocity = poss_err * faulhaber.p_gain;
+          BoundAbs(faulhaber.target_velocity, faulhaber.max_velocity);
+
+          // Set the new velocity target
+          uint8_t data[] = { 0xFF, 0x60, 0x00, (faulhaber.target_velocity & 0xFF), ((faulhaber.target_velocity >> 8) & 0xFF), ((faulhaber.target_velocity >> 16) & 0xFF), ((faulhaber.target_velocity >> 24) & 0xFF) }; // Set 0x60FF.00 to [velocity]:  Target velocity
           faulhaber_send_command(faulhaber_dev, 0x02, data, 7);
-          faulhaber.state++;
-          break;
-        }
-        case 2: {
-          // Enable operation
-          uint8_t data[] = { 0x40, 0x60, 0x00, 0x0F, 0x00}; // Set 0x6040.00 to 0x000F: Enable operation
-          faulhaber_send_command(faulhaber_dev, 0x02, data, 5);
-          faulhaber.state++;
-          break;
-        }
-        case 3: {
-          // Start moving
-          uint8_t data[] = { 0x40, 0x60, 0x00, 0x3F, 0x00}; // Set 0x6040.00 to 0x003F: Start moving
-          faulhaber_send_command(faulhaber_dev, 0x02, data, 5);
-          faulhaber.state++;
-          break;
-        }
-        default:
-          faulhaber.mode = FH_MODE_IDLE;
           faulhaber.state = 0;
           break;
-
+        }
       }
-      break;
-
-    /* FH_MODE_IDLE */
-    case FH_MODE_IDLE: {
-      // Move to the new position
-      if (faulhaber.target_position != faulhaber.setpoint_position) {
-        faulhaber.mode = FH_MODE_POSITION;
-        faulhaber.state = 0;
-        break;
-      }
-
-      // Request the position
-      uint8_t data[] = { 0x64, 0x60, 0x00}; // Get 0x6064.00: Get the actual position
-      faulhaber_send_command(faulhaber_dev, 0x01, data, 3);
-      faulhaber.state = 0;
-      break;
     }
 
-    case FH_MODE_ENABLE:
-      switch (faulhaber.state) {
-        case 0: {
-          // Disable drive
-          uint8_t data[] = { 0x40, 0x60, 0x00, 0x80, 0x00}; // Set 0x6040.00 to 0x0007: Disable drive
-          faulhaber_send_command(faulhaber_dev, 0x02, data, 5);
-          faulhaber.state++;
-          //break;
-        }
-        case 1: {
-          // Disable drive
-          uint8_t data[] = { 0x40, 0x60, 0x00, 0x06, 0x00}; // Set 0x6040.00 to 0x0006: Disable drive
-          faulhaber_send_command(faulhaber_dev, 0x02, data, 5);
-          faulhaber.state++;
-          //break;
-        }
-        case 2: {
-          // Enable drive
-          uint8_t data[] = { 0x40, 0x60, 0x00, 0x07, 0x00}; // Set 0x6040.00 to 0x0007: Enable drive
-          faulhaber_send_command(faulhaber_dev, 0x02, data, 5);
-          faulhaber.state++;
-          //break;
-        }
-        case 3: {
-          // Execute
-          uint8_t data[] = { 0x40, 0x60, 0x00, 0x0F, 0x00}; // Set 0x6040.00 to 0x000F: Execute
-          faulhaber_send_command(faulhaber_dev, 0x02, data, 5);
-          faulhaber.state++;
-          //break;
-        }
-        default:
-          faulhaber.mode = FH_MODE_INIT;
-          faulhaber.state = 0;
-          break;
-      }
-      break;
-
     /* Do nothing */
     default:
       break;
@@ -321,6 +351,22 @@ static void faulhaber_parse_msg(struct faulhaber_parser_t *p)
     // Send ABI message
     AbiSendMsgACT_FEEDBACK(ACT_FEEDBACK_FAULHABER_ID, &feedback, 1);
   }
+  // Parse the statuscode message
+  else if(p->cmd_code == 0x05) {
+    uint16_t status_code = p->data[0] | (p->data[1] << 8);
+
+    // Homing
+    if(!faulhaber.homing_completed && status_code&0x1000 && status_code&0x400)
+      faulhaber.homing_completed = true;
+    
+    // Position accepted
+    if(!faulhaber.position_ready && status_code&0x1000)
+      faulhaber.position_ready = true;
+      
+    // Target reached
+    if(!faulhaber.target_reached && status_code&0x400)
+      faulhaber.target_reached = true;
+  }
 }
 
 

sw/airborne/modules/actuators/actuators_faulhaber.h

@@ -31,19 +31,24 @@
 
 enum faulhaber_modes_t {
   FH_MODE_INIT,
-  FH_MODE_IDLE,
+  FH_MODE_VELOCITY,
-  FH_MODE_HOME,
+  FH_MODE_ERROR
-  FH_MODE_POSITION,
-  FH_MODE_ENABLE
 };
 
 struct faulhaber_t {
-  enum faulhaber_modes_t mode;
+  enum faulhaber_modes_t mode;    ///< Current mode of the controller
-  uint8_t state;
+  uint8_t state;                  ///< The state of the mode
+  
+  float p_gain;                   ///< The proportional gain of the velocity controller
+  int32_t max_velocity;           ///< The maximum velocity of the controller
 
-  int32_t setpoint_position;
+  int32_t setpoint_position;      ///< The setpoint position controlled from the actuator
-  int32_t target_position;
+  int32_t real_position;          ///< The real position from the feedback of the controller
-  int32_t real_position;
+  int32_t target_velocity;        ///< The target velocity send to the controller
+
+  bool homing_completed;      ///< Once the homing is completed
+  bool position_ready;        ///< Ready for receiving (new) positions
+  bool target_reached;        ///< When the target position is reached
 };
 extern struct faulhaber_t faulhaber;