novirium
/
airseeder-firmware
1
0
Fork 0
Code Issues 4 Pull Requests Projects Releases Wiki Activity

Split motor settings out, clean up queue init. Closes #2

Browse Source
master
Tom Wilson 4 years ago
parent a669b248e2
commit 4ac40d45fe
3 changed files with 102 additions and 64 deletions
Show Stats Download Patch File Download Diff File

35
src/motor-esp32/main.cpp
Unescape View File

@ -12,15 +12,32 @@ void setup() {
}
void loop() {
delay(100);
// Serial.println(digitalRead(MOTOR_SPEED_SENSE));
delay(500);
Serial.print("Rsp:");
Serial.print(get_roller_speed(0));
Serial.print(",");
Serial.print(get_roller_speed(1));
Serial.print(",");
Serial.print(get_roller_speed(2));
Serial.print(" Mdty:");
Serial.print(get_motor_duty(0));
Serial.print(", ");
Serial.print(",");
Serial.print(get_motor_duty(1));
Serial.print(", ");
Serial.println(get_roller_speed(0));
set_motor(0, true, ((millis()%5000)/5000.0)*0.65);
set_motor(1, true, 0.3);
set_motor(2, true, 0.3);
Serial.print(",");
Serial.println(get_motor_duty(2));
// Serial.println(digitalRead(MOTOR_SPEED_SENSE));
// Serial.print(get_motor_duty(0));
// Serial.print(", ");
// Serial.print(get_motor_duty(1));
// Serial.print(", ");
// Serial.println(get_roller_speed(0));
motor_settings_t m_set;
m_set.enabled = true;
m_set.desired_speed = ((millis() % 5000) / 5000.0) * 0.65;
set_motor_settings(0,m_set);
m_set.desired_speed = 0.3;
set_motor_settings(1,m_set);
set_motor_settings(2,m_set);
}

111
src/motor-esp32/motor_control.cpp
Unescape View File

@ -13,17 +13,9 @@
#define MC_TASK_STACK_SIZE 2048
typedef struct {
bool enabled = false;
float desired_speed = 0.0;
float kp = 0.5;
float ki = 0.0;
float kd = 0.0;
float min_duty = 0.1;
} motor_settings_t;
// Like with the speed sensors, use a single-length queue as a way to safely copy data to another non-synchronised task
static xQueueHandle motor_settings_queue[3];
static xQueueHandle motor_parameters_queue[3];
typedef struct {
float duty = 0.0;
@ -32,7 +24,7 @@ typedef struct {
} motor_status_t;
// Work with a local copy, use the queue to send to others
motor_status_t m_stat[3];
motor_status_t m_status[3];
static xQueueHandle motor_status_queue[3];
const mcpwm_timer_t motor_timer[3] = {MCPWM_TIMER_0, MCPWM_TIMER_1, MCPWM_TIMER_2};
@ -41,6 +33,7 @@ const gpio_num_t motor_en_pins[3] = {GPIO_ROLLER1_EN, GPIO_ROLLER2_EN, GPIO_ROLL
static void motor_control_task(void *pvParameters) {
TickType_t xLastWakeTime;
motor_settings_t m_set;
motor_parameters_t m_param;
float actual_speed;
float error;
float d_error;
@ -51,18 +44,19 @@ static void motor_control_task(void *pvParameters) {
for (;;) {
for (int m_id = 0; m_id < 3; m_id++) {
// Get the latest motor settings
// Get the latest motor settings and parameters
xQueuePeek(motor_settings_queue[m_id], &m_set, 0);
xQueuePeek(motor_settings_queue[m_id], &m_param, 0);
if (m_set.enabled) {
actual_speed = get_roller_speed(m_id);
error = m_set.desired_speed - actual_speed;
m_stat[m_id].i_error += error * MC_ITERATION_TIME;
d_error = (error - m_stat[m_id].last_error) / MC_ITERATION_TIME;
m_stat[m_id].last_error = error;
m_status[m_id].i_error += error * MC_ITERATION_TIME;
d_error = (error - m_status[m_id].last_error) / MC_ITERATION_TIME;
m_status[m_id].last_error = error;
duty_correction = m_set.kp * error + m_set.ki * m_stat[m_id].i_error + m_set.kd * d_error;
duty_correction = m_param.kp * error + m_param.ki * m_status[m_id].i_error + m_param.kd * d_error;
// Clip duty correction to the maximum allowed ramp
if (duty_correction > (MAX_DUTY_RAMP * MC_ITERATION_TIME)) {
@ -72,15 +66,15 @@ static void motor_control_task(void *pvParameters) {
}
// Apply duty correction, and clip
m_stat[m_id].duty += duty_correction;
if (m_stat[m_id].duty < 0.0) {
m_stat[m_id].duty = 0.0;
} else if (m_stat[m_id].duty > 1.0) {
m_stat[m_id].duty = 1.0;
m_status[m_id].duty += duty_correction;
if (m_status[m_id].duty < 0.0) {
m_status[m_id].duty = 0.0;
} else if (m_status[m_id].duty > 1.0) {
m_status[m_id].duty = 1.0;
}
// Map duty to fit between min and tri-phase for actual motor PWM
mapped_duty = 0.33 * (m_stat[m_id].duty * (MAX_DUTY - m_set.min_duty) + m_set.min_duty);
mapped_duty = 0.33 * (m_status[m_id].duty * (MAX_DUTY - m_param.min_duty) + m_param.min_duty);
// This function takes duty as a float percent...
mcpwm_set_duty(MCPWM_UNIT_0, motor_timer[m_id], MCPWM_GEN_A, mapped_duty * 100.0);
@ -90,16 +84,16 @@ static void motor_control_task(void *pvParameters) {
} else {
// Motor disabled, so make sure EN pin is off, and reset cumulative error
// _don't_ stop MCPWM timer, as starting it again could glitch the phase
m_stat[m_id].i_error = 0;
m_stat[m_id].last_error = 0;
m_stat[m_id].duty = 0;
m_status[m_id].i_error = 0;
m_status[m_id].last_error = 0;
m_status[m_id].duty = 0;
gpio_set_level(motor_en_pins[m_id], 0);
mcpwm_set_duty(MCPWM_UNIT_0, motor_timer[m_id], MCPWM_GEN_A, 0.0);
}
// Save the motor status for others to retrieve
xQueueOverwrite(motor_status_queue[m_id], &m_stat[m_id]);
xQueueOverwrite(motor_status_queue[m_id], &m_status[m_id]);
}
// Suspend task until the next control loop iteration
@ -107,17 +101,26 @@ static void motor_control_task(void *pvParameters) {
}
}
void set_motor(int motor_id, bool enabled, float desired_speed) {
motor_settings_t new_settings;
// Get copy of current settings
xQueuePeek(motor_settings_queue[motor_id], &new_settings, 0);
new_settings.enabled = enabled;
new_settings.desired_speed = desired_speed;
// Send new modified settings
void set_motor_settings(int motor_id, motor_settings_t new_settings) {
xQueueOverwrite(motor_settings_queue[motor_id], &new_settings);
}
void set_motor_params(int motor_id, motor_parameters_t new_params) {
xQueueOverwrite(motor_parameters_queue[motor_id], &new_params);
}
motor_settings_t get_motor_settings(int motor_id) {
motor_settings_t current_motor_settings;
xQueuePeek(motor_settings_queue[motor_id], &current_motor_settings, 0);
return current_motor_settings;
}
motor_parameters_t get_motor_parameters(int motor_id) {
motor_parameters_t current_motor_params;
xQueuePeek(motor_parameters_queue[motor_id], &current_motor_params, 0);
return current_motor_params;
}
float get_motor_duty(int motor_id) {
motor_status_t motor_status;
@ -127,27 +130,25 @@ float get_motor_duty(int motor_id) {
void motor_control_setup(void) {
// Allocate queues
motor_settings_queue[0] = xQueueCreate(1, sizeof(motor_settings_t));
motor_settings_queue[1] = xQueueCreate(1, sizeof(motor_settings_t));
motor_settings_queue[2] = xQueueCreate(1, sizeof(motor_settings_t));
motor_status_queue[0] = xQueueCreate(1, sizeof(motor_status_t));
motor_status_queue[1] = xQueueCreate(1, sizeof(motor_status_t));
motor_status_queue[2] = xQueueCreate(1, sizeof(motor_status_t));
// Prime queues
motor_settings_t init_motor_settings;
xQueueOverwrite(motor_settings_queue[0], &init_motor_settings);
xQueueOverwrite(motor_settings_queue[1], &init_motor_settings);
xQueueOverwrite(motor_settings_queue[2], &init_motor_settings);
motor_status_t blank_motor_status;
xQueueOverwrite(motor_status_queue[0], &blank_motor_status);
xQueueOverwrite(motor_status_queue[1], &blank_motor_status);
xQueueOverwrite(motor_status_queue[2], &blank_motor_status);
for (int m_id = 0; m_id < 3; m_id++) {
// Allocate queues
motor_settings_queue[m_id] = xQueueCreate(1, sizeof(motor_settings_t));
motor_parameters_queue[m_id] = xQueueCreate(1, sizeof(motor_parameters_t));
motor_status_queue[m_id] = xQueueCreate(1, sizeof(motor_status_t));
// Prime queues - these are single item queues, and so functions reading from them just
// assume something is there.
// TODO - this is where we'd read in the saved motor params from flash.
motor_settings_t init_motor_settings;
motor_parameters_t init_motor_params;
motor_status_t blank_motor_status;
xQueueOverwrite(motor_settings_queue[m_id], &init_motor_settings);
xQueueOverwrite(motor_parameters_queue[m_id], &init_motor_params);
xQueueOverwrite(motor_status_queue[m_id], &blank_motor_status);
}
// setup MCPWM timer
// Setup MCPWM timers.
mcpwm_config_t pwm_config = {
.frequency = MOTOR_PWM_FREQ,
.cmpr_a = 0,
@ -162,6 +163,8 @@ void motor_control_setup(void) {
// Set Timer 0 to generate sync signal when it reaches the top
mcpwm_set_timer_sync_output(MCPWM_UNIT_0, MCPWM_TIMER_0, MCPWM_SWSYNC_SOURCE_TEP);
// Configure Timer 1 and 2 to receive sync signal from Timer 0, and apply 33% and 66% phase offset.
// (yes, it's weird that the phase offset is taken as a value from 0 to 1000)
mcpwm_sync_config_t sync_conf = {
.sync_sig = MCPWM_SELECT_TIMER0_SYNC,
.timer_val = 333,
@ -171,7 +174,7 @@ void motor_control_setup(void) {
sync_conf.timer_val = 666;
mcpwm_sync_configure(MCPWM_UNIT_0, MCPWM_TIMER_2, &sync_conf);
// Bind to output
// Bind PWM timers to outputs
mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM0A, GPIO_ROLLER1_PWM);
mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM1A, GPIO_ROLLER2_PWM);
mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM2A, GPIO_ROLLER3_PWM);

20
src/motor-esp32/motor_control.h
Unescape View File

@ -9,7 +9,25 @@
#define MAX_DUTY 0.99 // max duty in each phase - have slightly below 1.0 to give more dead time between phases
void set_motor(int motor_id, bool enabled, float desired_speed);
typedef struct {
bool enabled = false;
float desired_speed = 0.0;
} motor_settings_t;
typedef struct {
float kp = 0.5;
float ki = 0.0;
float kd = 0.0;
float min_duty = 0.1;
} motor_parameters_t;
void set_motor_settings(int motor_id, motor_settings_t new_settings);
void set_motor_params(int motor_id, motor_parameters_t new_params);
motor_settings_t get_motor_settings(int motor_id);
motor_parameters_t get_motor_parameters(int motor_id);
float get_motor_duty(int motor_id);

Write Preview
Loading…
Cancel
Save