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

Change indent style, add other 2 motors

Browse Source
master
Tom Wilson 4 years ago
parent 2e7ccce082
commit 82b21b357d
5 changed files with 254 additions and 213 deletions
Show Stats Download Patch File Download Diff File

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

@ -1,22 +1,25 @@
#include <Arduino.h>
#include "roller_speed.h"
#include "motor_control.h"
#include "roller_speed.h"
void setup() {
Serial.setTimeout(1);
Serial.begin(57600);
Serial.setTimeout(1);
Serial.begin(57600);
roller_speed_setup();
motor_control_setup();
roller_speed_setup();
motor_control_setup();
}
void loop() {
delay(100);
//Serial.println(digitalRead(MOTOR_SPEED_SENSE));
Serial.print(get_motor_1_duty());
Serial.print(", ");
Serial.println(getRoller1Speed());
set_motor_1(true, 0.3);
delay(100);
// Serial.println(digitalRead(MOTOR_SPEED_SENSE));
Serial.print(get_motor_1_duty());
Serial.print(", ");
Serial.print(get_motor_2_duty());
Serial.print(", ");
Serial.println(getRoller1Speed());
set_motor_1(true, 0.3);
set_motor_2(true, 0.3);
}

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

@ -9,31 +9,26 @@
#include "freertos/queue.h"
#include "freertos/task.h"
#define MOTOR_PWM_FREQ 1000 // PWM frequency in Hz
#define MOTOR_CONTROL_PERIOD_MS 100 // Period of the motor control loop
#define MAX_DUTY_RAMP 5.0 // max change in duty per second - where 1.0 is full duty
#define MC_ITERATION_TIME 1.0 / MOTOR_CONTROL_PERIOD_MS
#define MC_TASK_STACK_SIZE 2048
typedef struct {
bool enabled = false;
float desired_speed = 0.0;
float kp = 1.0;
float ki = 0.0;
float kd = 0.0;
float min_duty = 0.1;
bool enabled = false;
float desired_speed = 0.0;
float kp = 1.0;
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];
typedef struct {
float duty = 0.0;
float last_error = 0.0;
float i_error = 0.0;
float duty = 0.0;
float last_error = 0.0;
float i_error = 0.0;
} motor_status_t;
// Work with a local copy, use the queue to send to others
@ -44,135 +39,170 @@ const mcpwm_timer_t motor_timer[3] = {MCPWM_TIMER_0, MCPWM_TIMER_1, MCPWM_TIMER_
const gpio_num_t motor_en_pins[3] = {GPIO_ROLLER1_EN, GPIO_ROLLER2_EN, GPIO_ROLLER3_EN};
static void motor_control_task(void *pvParameters) {
TickType_t xLastWakeTime;
motor_settings_t m_set;
float actual_speed;
float error;
float d_error;
float duty_correction;
float mapped_duty;
xLastWakeTime = xTaskGetTickCount();
for (;;) {
for (int m_id = 0; m_id < 1; m_id++) {
// Get the latest motor settings
xQueuePeek(motor_settings_queue[m_id], &m_set, 0);
if (m_set.enabled) {
actual_speed = getRoller1Speed();
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;
duty_correction = m_set.kp * error + m_set.ki * m_stat[m_id].i_error + m_set.kd * d_error;
// Clip duty correction to the maximum allowed ramp
if (duty_correction > (MAX_DUTY_RAMP * MC_ITERATION_TIME)) {
duty_correction = (MAX_DUTY_RAMP * MC_ITERATION_TIME);
} else if (duty_correction < -(MAX_DUTY_RAMP * MC_ITERATION_TIME)) {
duty_correction = -(MAX_DUTY_RAMP * MC_ITERATION_TIME);
TickType_t xLastWakeTime;
motor_settings_t m_set;
float actual_speed;
float error;
float d_error;
float duty_correction;
float mapped_duty;
xLastWakeTime = xTaskGetTickCount();
for (;;) {
for (int m_id = 0; m_id < 3; m_id++) {
// Get the latest motor settings
xQueuePeek(motor_settings_queue[m_id], &m_set, 0);
if (m_set.enabled) {
actual_speed = getRoller1Speed();
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;
duty_correction = m_set.kp * error + m_set.ki * m_stat[m_id].i_error + m_set.kd * d_error;
// Clip duty correction to the maximum allowed ramp
if (duty_correction > (MAX_DUTY_RAMP * MC_ITERATION_TIME)) {
duty_correction = (MAX_DUTY_RAMP * MC_ITERATION_TIME);
} else if (duty_correction < -(MAX_DUTY_RAMP * MC_ITERATION_TIME)) {
duty_correction = -(MAX_DUTY_RAMP * MC_ITERATION_TIME);
}
// 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;
}
// Map duty to fit between min and tri-phase for actual motor PWM
mapped_duty = 0.33 * (m_stat[m_id].duty * (1.0 - m_set.min_duty) + m_set.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);
// Enable driver GPIO en
gpio_set_level(motor_en_pins[m_id], 1);
} 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;
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]);
}
// 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;
}
// Suspend task until the next control loop iteration
vTaskDelayUntil(&xLastWakeTime, pdMS_TO_TICKS(MOTOR_CONTROL_PERIOD_MS));
}
}
// Map duty to fit between min and tri-phase for actual motor PWM
mapped_duty = 0.33 * (m_stat[m_id].duty * (1.0 - m_set.min_duty) + m_set.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);
void set_motor_1(bool enabled, float desired_speed) {
motor_settings_t new_settings;
// Enable driver GPIO en
gpio_set_level(motor_en_pins[m_id], 1);
xQueuePeek(motor_settings_queue[0], &new_settings, 0);
new_settings.enabled = enabled;
new_settings.desired_speed = desired_speed;
xQueueOverwrite(motor_settings_queue[0], &new_settings);
}
} 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;
void set_motor_2(bool enabled, float desired_speed) {
motor_settings_t new_settings;
gpio_set_level(motor_en_pins[m_id], 0);
mcpwm_set_duty(MCPWM_UNIT_0, motor_timer[m_id], MCPWM_GEN_A, 0.0);
}
xQueuePeek(motor_settings_queue[1], &new_settings, 0);
new_settings.enabled = enabled;
new_settings.desired_speed = desired_speed;
xQueueOverwrite(motor_settings_queue[1], &new_settings);
}
// Save the motor status for others to retrieve
xQueueOverwrite(motor_status_queue[m_id], &m_stat[m_id]);
}
void set_motor_3(bool enabled, float desired_speed) {
motor_settings_t new_settings;
// Suspend task until the next control loop iteration
vTaskDelayUntil(&xLastWakeTime, pdMS_TO_TICKS(MOTOR_CONTROL_PERIOD_MS));
}
xQueuePeek(motor_settings_queue[2], &new_settings, 0);
new_settings.enabled = enabled;
new_settings.desired_speed = desired_speed;
xQueueOverwrite(motor_settings_queue[2], &new_settings);
}
void set_motor_1(bool enabled, float desired_speed) {
motor_settings_t new_settings;
float get_motor_1_duty() {
motor_status_t motor_status;
xQueuePeek(motor_settings_queue[0], &new_settings, 0);
new_settings.enabled = enabled;
new_settings.desired_speed = desired_speed;
xQueueOverwrite(motor_settings_queue[0], &new_settings);
xQueuePeek(motor_status_queue[0], &motor_status, 0);
return motor_status.duty;
}
float get_motor_1_duty() {
motor_status_t motor_status;
float get_motor_2_duty() {
motor_status_t motor_status;
xQueuePeek(motor_status_queue[1], &motor_status, 0);
return motor_status.duty;
}
xQueuePeek(motor_status_queue[0], &motor_status, 0);
float get_motor_3_duty() {
motor_status_t motor_status;
return motor_status.duty;
xQueuePeek(motor_status_queue[2], &motor_status, 0);
return motor_status.duty;
}
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);
// setup MCPWM timer
mcpwm_config_t pwm_config = {
.frequency = MOTOR_PWM_FREQ,
.cmpr_a = 0,
.cmpr_b = 0,
.duty_mode = MCPWM_DUTY_MODE_0,
.counter_mode = MCPWM_UP_COUNTER,
};
mcpwm_init(MCPWM_UNIT_0, MCPWM_TIMER_0, &pwm_config);
// Bind to output
mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM0A, GPIO_ROLLER1_PWM);
// Setup EN pins
gpio_set_level(GPIO_ROLLER1_EN, 0);
gpio_set_level(GPIO_ROLLER2_EN, 0);
gpio_set_level(GPIO_ROLLER3_EN, 0);
gpio_set_direction(GPIO_ROLLER1_EN, GPIO_MODE_OUTPUT);
gpio_set_direction(GPIO_ROLLER2_EN, GPIO_MODE_OUTPUT);
gpio_set_direction(GPIO_ROLLER3_EN, GPIO_MODE_OUTPUT);
// Kick off our main motor control task
xTaskCreate(motor_control_task, "MotorCtrl", MC_TASK_STACK_SIZE, NULL, 5, NULL);
// 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);
// setup MCPWM timer
mcpwm_config_t pwm_config = {
.frequency = MOTOR_PWM_FREQ,
.cmpr_a = 0,
.cmpr_b = 0,
.duty_mode = MCPWM_DUTY_MODE_0,
.counter_mode = MCPWM_UP_COUNTER,
};
mcpwm_init(MCPWM_UNIT_0, MCPWM_TIMER_0, &pwm_config);
mcpwm_init(MCPWM_UNIT_0, MCPWM_TIMER_1, &pwm_config);
mcpwm_init(MCPWM_UNIT_0, MCPWM_TIMER_2, &pwm_config);
// Bind to output
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);
// Setup EN pins
gpio_set_level(GPIO_ROLLER1_EN, 0);
gpio_set_level(GPIO_ROLLER2_EN, 0);
gpio_set_level(GPIO_ROLLER3_EN, 0);
gpio_set_direction(GPIO_ROLLER1_EN, GPIO_MODE_OUTPUT);
gpio_set_direction(GPIO_ROLLER2_EN, GPIO_MODE_OUTPUT);
gpio_set_direction(GPIO_ROLLER3_EN, GPIO_MODE_OUTPUT);
// Kick off our main motor control task
xTaskCreate(motor_control_task, "MotorCtrl", MC_TASK_STACK_SIZE, NULL, 5, NULL);
}

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

@ -1,11 +1,20 @@
#ifndef motor_control_h
#define motor_control_h
#define MOTOR_PWM_FREQ 500 // PWM frequency in Hz
void set_motor_1(bool enabled, float desired_speed);
#define MOTOR_CONTROL_PERIOD_MS 100 // Period of the motor control loop
void motor_control_setup(void);
#define MAX_DUTY_RAMP 5.0 // max change in duty per second - where 1.0 is full duty
void set_motor_1(bool enabled, float desired_speed);
void set_motor_2(bool enabled, float desired_speed);
void set_motor_3(bool enabled, float desired_speed);
float get_motor_1_duty(void);
float get_motor_2_duty(void);
float get_motor_3_duty(void);
void motor_control_setup(void);
#endif

137
src/motor-esp32/roller_speed.cpp
Unescape View File

@ -18,85 +18,84 @@ static xQueueHandle roller_measurement_queue[3];
// struct to hold move our pulse measurements from our ISR via the queue
typedef struct {
uint32_t period; // period since last pulse in APB clock ticks
int64_t timestamp; // time measurement occurred in microseconds since boot
uint32_t period; // period since last pulse in APB clock ticks
int64_t timestamp; // time measurement occurred in microseconds since boot
} pulse_measurement_t;
// Gets called by the MCPWM capture module. Tells us which capture unit it was, which edge, and when it happened
static bool speed_pulse_isr_handler(mcpwm_unit_t mcpwm, mcpwm_capture_channel_id_t cap_sig,
const cap_event_data_t *edata, void *arg) {
// Not strictly necessary to do the wakeup check here, as we shouldn't be blokcing anything. Doesn't hurt though.
BaseType_t high_task_wakeup = pdFALSE;
uint r_id = 0;
if (cap_sig == MCPWM_SELECT_CAP0) {
r_id = 0;
} else if (cap_sig == MCPWM_SELECT_CAP1) {
r_id = 1;
} else if (cap_sig == MCPWM_SELECT_CAP2) {
r_id = 2;
} else {
return false;
}
// Add one to our revs counter, and overflow it if necessary
roller_pulses[r_id]++;
if ((roller_pulses[r_id] / ROLLER_PULSES_PER_REV) > REV_OVERFLOW) {
roller_pulses[0] = 0;
}
pulse_measurement_t new_measurement;
int64_t time_now;
time_now = esp_timer_get_time();
// Just make sure we have a previous edge to compare to, and that it was no more than ZERO_SPEED_TIMEOUT_US ago
if ((roller_last_timestamp[r_id] != 0) and ((time_now - roller_last_timestamp[r_id]) < ZERO_SPEED_TIMEOUT_US)) {
new_measurement.timestamp = time_now;
new_measurement.period = edata->cap_value - roller_last_edge_time[r_id];
xQueueOverwriteFromISR(roller_measurement_queue[r_id], &new_measurement, &high_task_wakeup);
}
roller_last_edge_time[r_id] = edata->cap_value;
roller_last_timestamp[r_id] = time_now;
return high_task_wakeup == pdTRUE;
// Not strictly necessary to do the wakeup check here, as we shouldn't be blokcing anything. Doesn't hurt though.
BaseType_t high_task_wakeup = pdFALSE;
uint r_id = 0;
if (cap_sig == MCPWM_SELECT_CAP0) {
r_id = 0;
} else if (cap_sig == MCPWM_SELECT_CAP1) {
r_id = 1;
} else if (cap_sig == MCPWM_SELECT_CAP2) {
r_id = 2;
} else {
return false;
}
// Add one to our revs counter, and overflow it if necessary
roller_pulses[r_id]++;
if ((roller_pulses[r_id] / ROLLER_PULSES_PER_REV) > REV_OVERFLOW) {
roller_pulses[0] = 0;
}
pulse_measurement_t new_measurement;
int64_t time_now;
time_now = esp_timer_get_time();
// Just make sure we have a previous edge to compare to, and that it was no more than ZERO_SPEED_TIMEOUT_US ago
if ((roller_last_timestamp[r_id] != 0) and ((time_now - roller_last_timestamp[r_id]) < ZERO_SPEED_TIMEOUT_US)) {
new_measurement.timestamp = time_now;
new_measurement.period = edata->cap_value - roller_last_edge_time[r_id];
xQueueOverwriteFromISR(roller_measurement_queue[r_id], &new_measurement, &high_task_wakeup);
}
roller_last_edge_time[r_id] = edata->cap_value;
roller_last_timestamp[r_id] = time_now;
return high_task_wakeup == pdTRUE;
}
float getRoller1Speed(void) {
pulse_measurement_t pulse_measurement;
// Get the pulse period in APB clock ticks.
xQueuePeek(roller_measurement_queue[0], &pulse_measurement, 0);
if ((pulse_measurement.period == 0) or
((esp_timer_get_time() - pulse_measurement.timestamp) >= ZERO_SPEED_TIMEOUT_US)) {
return 0.0;
}
// Convert that to revs/sec
return ((float)rtc_clk_apb_freq_get() / (float)(pulse_measurement.period)) / (float)(ROLLER_PULSES_PER_REV);
pulse_measurement_t pulse_measurement;
// Get the pulse period in APB clock ticks.
xQueuePeek(roller_measurement_queue[0], &pulse_measurement, 0);
if ((pulse_measurement.period == 0) or
((esp_timer_get_time() - pulse_measurement.timestamp) >= ZERO_SPEED_TIMEOUT_US)) {
return 0.0;
}
// Convert that to revs/sec
return ((float)rtc_clk_apb_freq_get() / (float)(pulse_measurement.period)) / (float)(ROLLER_PULSES_PER_REV);
}
void roller_speed_setup(void) {
// Create our measurement queues
roller_measurement_queue[0] = xQueueCreate(1, sizeof(pulse_measurement_t));
roller_measurement_queue[1] = xQueueCreate(1, sizeof(pulse_measurement_t));
roller_measurement_queue[2] = xQueueCreate(1, sizeof(pulse_measurement_t));
// Prime queues
pulse_measurement_t blank_measurement = {0,0};
xQueueOverwrite(roller_measurement_queue[0], &blank_measurement);
xQueueOverwrite(roller_measurement_queue[1], &blank_measurement);
xQueueOverwrite(roller_measurement_queue[2], &blank_measurement);
// Set MCPWM0 CAP0 on roller 1 speed, enable pullup input pin
mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM_CAP_0, GPIO_ROLLER1_SPEED);
// configure MCPWM0 CAP0
mcpwm_capture_config_t conf = {.cap_edge = MCPWM_POS_EDGE, // trigger on riding edge
.cap_prescale = 1, // pulses per interrupt
.capture_cb = speed_pulse_isr_handler,
.user_data = NULL};
// Enable MCWPM0 CAP0
mcpwm_capture_enable_channel(MCPWM_UNIT_0, MCPWM_SELECT_CAP0, &conf);
// Create our measurement queues
roller_measurement_queue[0] = xQueueCreate(1, sizeof(pulse_measurement_t));
roller_measurement_queue[1] = xQueueCreate(1, sizeof(pulse_measurement_t));
roller_measurement_queue[2] = xQueueCreate(1, sizeof(pulse_measurement_t));
// Prime queues
pulse_measurement_t blank_measurement = {0, 0};
xQueueOverwrite(roller_measurement_queue[0], &blank_measurement);
xQueueOverwrite(roller_measurement_queue[1], &blank_measurement);
xQueueOverwrite(roller_measurement_queue[2], &blank_measurement);
// Set MCPWM0 CAP0 on roller 1 speed, enable pullup input pin
mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM_CAP_0, GPIO_ROLLER1_SPEED);
// configure MCPWM0 CAP0
mcpwm_capture_config_t conf = {.cap_edge = MCPWM_POS_EDGE, // trigger on riding edge
.cap_prescale = 1, // pulses per interrupt
.capture_cb = speed_pulse_isr_handler,
.user_data = NULL};
// Enable MCWPM0 CAP0
mcpwm_capture_enable_channel(MCPWM_UNIT_0, MCPWM_SELECT_CAP0, &conf);
}

8
src/motor-esp32/roller_speed.h
Unescape View File

@ -3,10 +3,10 @@
#include "freertos/FreeRTOS.h"
#define REV_OVERFLOW 10000 // Number at which rev counts overflow back to 0
#define ROLLER_PULSES_PER_REV 40 // Number of sensor pulses per roller rev
#define ZERO_SPEED_TIMEOUT_US 1000000 // Note that with a clock at 80MHz, the pulse period counter overflows every 53 seconds
#define REV_OVERFLOW 10000 // Number at which rev counts overflow back to 0
#define ROLLER_PULSES_PER_REV 40 // Number of sensor pulses per roller rev
#define ZERO_SPEED_TIMEOUT_US \
1000000 // Note that with a clock at 80MHz, the pulse period counter overflows every 53 seconds
void roller_speed_setup(void);

Write Preview
Loading…
Cancel
Save