Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-25 23:46:04 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

[actuators] Update controller faulhaber (#3347)

Browse Source
This commit is contained in:
Freek van Tienen
2024-07-30 11:02:42 +02:00
committed by GitHub
parent dc2ba583e8
commit 7f13148139
3 changed files with 165 additions and 115 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/modules/actuators_faulhaber.xml
+7 -8
View File
@@ -4,20 +4,18 @@
<doc>
<description>
Actuators Driver for the Faulhaber controller
Currently initialization is done manually.
Set mode to:
- 2
- 4
- 2 (it will home slowly)
- Wait until homing is finished!
- Back to 1 and the wing will move
Based on the velocity controller inside the MC5004 controller, a simple position controller is implemented.
</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
+144 -98
View File
@@ -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 */
case FH_MODE_HOME:
switch (faulhaber.state) {
/* Initialize and home the motor */
case FH_MODE_INIT:
switch(faulhaber.state) {
case 0: {
// Set the homing mode
uint8_t data[] = { 0x60, 0x60, 0x00, 0x06 }; // Set 0x6060.00 to 0x06: Homing mode
faulhaber_send_command(faulhaber_dev, 0x02, data, 4);
// Start the controller boot timer
last_time = get_sys_time_float();
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:
// Goto position mode
faulhaber.mode = FH_MODE_INIT;
faulhaber.state = 0;
case 6: {
// Check if homing is completed
if(faulhaber.homing_completed) {
// 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 */
case FH_MODE_POSITION:
/* FH_MODE_VELOCITY */
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
faulhaber_send_command(faulhaber_dev, 0x02, data, 4);
// 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++;
break;
}
case 1: {
// Set the target position
faulhaber.target_position = faulhaber.setpoint_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
default: {
// Calculate the new velocity target
int32_t poss_err = (faulhaber.setpoint_position - faulhaber.real_position);
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
+14 -9
View File
@@ -31,19 +31,24 @@
enum faulhaber_modes_t {
FH_MODE_INIT,
FH_MODE_IDLE,
FH_MODE_HOME,
FH_MODE_POSITION,
FH_MODE_ENABLE
FH_MODE_VELOCITY,
FH_MODE_ERROR
};
struct faulhaber_t {
enum faulhaber_modes_t mode;
uint8_t state;
enum faulhaber_modes_t mode; ///< Current mode of the controller
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 target_position;
int32_t real_position;
int32_t setpoint_position; ///< The setpoint position controlled from the actuator
int32_t real_position; ///< The real position from the feedback of the controller
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;