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

Fix default structs, add explicit array indexes

Browse Source
master
Tom Wilson 4 years ago
parent 87026eb63f
commit c51c71dc06
7 changed files with 69 additions and 60 deletions
Show Stats Download Patch File Download Diff File

16
src/common/shared_structs.h
Unescape View File

@ -1,16 +1,20 @@
#ifndef shared_structs_h #ifndef shared_structs_h
#define shared_structs_h #define shared_structs_h
typedef enum { MOTOR_1 = 0, MOTOR_2 = 1, MOTOR_3 = 2 } motor_id_t;
typedef enum { ROLLER_1 = 0, ROLLER_2 = 1, ROLLER_3 = 2 } roller_id_t;
typedef enum { BIN_1 = 0, BIN_2 = 1, BIN_3 = 2 } bin_id_t;
typedef struct { typedef struct {
bool enabled = false; bool enabled;
float desired_speed = 0.0; float desired_speed;
} motor_control_t; } motor_control_t;
typedef struct { typedef struct {
float kp = 0.5; float kp;
float ki = 0.0; float ki;
float kd = 0.0; float kd;
float min_duty = 0.1; float min_duty;
} motor_parameters_t; } motor_parameters_t;
typedef enum { MOTOR_CONTROL_PACKET, MOTOR_PARAMETERS_PACKET } packet_type_t; typedef enum { MOTOR_CONTROL_PACKET, MOTOR_PARAMETERS_PACKET } packet_type_t;

42
src/motor-esp32/i2c_comms.cpp
Unescape View File

@ -17,21 +17,21 @@ void on_I2C_request() {
// Hopefully because most of our data is pulled in with queues, rather than larger locks, // Hopefully because most of our data is pulled in with queues, rather than larger locks,
// assembling the reply won't take too long. // assembling the reply won't take too long.
i2c_reply.roller_1_speed = get_roller_speed(0); i2c_reply.roller_1_speed = get_roller_speed(ROLLER_1);
i2c_reply.roller_2_speed = get_roller_speed(1); i2c_reply.roller_2_speed = get_roller_speed(ROLLER_2);
i2c_reply.roller_3_speed = get_roller_speed(2); i2c_reply.roller_3_speed = get_roller_speed(ROLLER_3);
i2c_reply.roller_1_revs = get_roller_revs(0); i2c_reply.roller_1_revs = get_roller_revs(ROLLER_1);
i2c_reply.roller_2_revs = get_roller_revs(1); i2c_reply.roller_2_revs = get_roller_revs(ROLLER_2);
i2c_reply.roller_3_revs = get_roller_revs(2); i2c_reply.roller_3_revs = get_roller_revs(ROLLER_3);
i2c_reply.motor_1_duty = get_motor_duty(0); i2c_reply.motor_1_duty = get_motor_duty(MOTOR_1);
i2c_reply.motor_2_duty = get_motor_duty(1); i2c_reply.motor_2_duty = get_motor_duty(MOTOR_2);
i2c_reply.motor_3_duty = get_motor_duty(2); i2c_reply.motor_3_duty = get_motor_duty(MOTOR_3);
i2c_reply.motor_1_control = get_motor_control(0); i2c_reply.motor_1_control = get_motor_control(MOTOR_1);
i2c_reply.motor_2_control = get_motor_control(1); i2c_reply.motor_2_control = get_motor_control(MOTOR_2);
i2c_reply.motor_3_control = get_motor_control(2); i2c_reply.motor_3_control = get_motor_control(MOTOR_3);
i2c_reply.motor_1_params = get_motor_parameters(0); i2c_reply.motor_1_params = get_motor_parameters(MOTOR_1);
i2c_reply.motor_2_params = get_motor_parameters(1); i2c_reply.motor_2_params = get_motor_parameters(MOTOR_2);
i2c_reply.motor_3_params = get_motor_parameters(2); i2c_reply.motor_3_params = get_motor_parameters(MOTOR_3);
Wire.write((byte *)&i2c_reply, sizeof(i2c_reply)); Wire.write((byte *)&i2c_reply, sizeof(i2c_reply));
} }
@ -43,15 +43,15 @@ void on_I2C_receive(int len){
Wire.readBytes((byte *)&motor_write_packet, len); Wire.readBytes((byte *)&motor_write_packet, len);
if (motor_write_packet.packet_type == MOTOR_CONTROL_PACKET){ if (motor_write_packet.packet_type == MOTOR_CONTROL_PACKET){
// We got a motor control packet // We got a motor control packet
set_motor_control(0, motor_write_packet.control.motor_1_control); set_motor_control(MOTOR_1, motor_write_packet.control.motor_1_control);
set_motor_control(1, motor_write_packet.control.motor_1_control); set_motor_control(MOTOR_2, motor_write_packet.control.motor_2_control);
set_motor_control(2, motor_write_packet.control.motor_1_control); set_motor_control(MOTOR_3, motor_write_packet.control.motor_3_control);
} else if (motor_write_packet.packet_type == MOTOR_PARAMETERS_PACKET){ } else if (motor_write_packet.packet_type == MOTOR_PARAMETERS_PACKET){
// We got a motor parameters packet // We got a motor parameters packet
set_motor_params(0, motor_write_packet.params.motor_1_params); set_motor_params(MOTOR_1, motor_write_packet.params.motor_1_params);
set_motor_params(1, motor_write_packet.params.motor_1_params); set_motor_params(MOTOR_2, motor_write_packet.params.motor_2_params);
set_motor_params(2, motor_write_packet.params.motor_1_params); set_motor_params(MOTOR_3, motor_write_packet.params.motor_3_params);
} else { } else {
// I2C error // I2C error
} }

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

@ -18,20 +18,20 @@ motor_control_t m_ctrl;
void loop() { void loop() {
delay(500); delay(500);
Serial.print("Dsr:"); Serial.print("Dsr:");
m_ctrl = get_motor_control(0); m_ctrl = get_motor_control(MOTOR_1);
Serial.print(m_ctrl.desired_speed); Serial.print(m_ctrl.desired_speed);
Serial.print(" Rsp:"); Serial.print(" Rsp:");
Serial.print(get_roller_speed(0)); Serial.print(get_roller_speed(ROLLER_1));
Serial.print(","); Serial.print(",");
Serial.print(get_roller_speed(1)); Serial.print(get_roller_speed(ROLLER_2));
Serial.print(","); Serial.print(",");
Serial.print(get_roller_speed(2)); Serial.print(get_roller_speed(ROLLER_3));
Serial.print(" Mdty:"); Serial.print(" Mdty:");
Serial.print(get_motor_duty(0)); Serial.print(get_motor_duty(MOTOR_1));
Serial.print(","); Serial.print(",");
Serial.print(get_motor_duty(1)); Serial.print(get_motor_duty(MOTOR_2));
Serial.print(","); Serial.print(",");
Serial.println(get_motor_duty(2)); Serial.println(get_motor_duty(MOTOR_3));
// Serial.println(digitalRead(MOTOR_SPEED_SENSE)); // Serial.println(digitalRead(MOTOR_SPEED_SENSE));
// Serial.print(get_motor_duty(0)); // Serial.print(get_motor_duty(0));
@ -42,8 +42,8 @@ void loop() {
m_ctrl.enabled = true; m_ctrl.enabled = true;
m_ctrl.desired_speed = ((millis() % 10000) / 10000.0) * 0.65; m_ctrl.desired_speed = ((millis() % 10000) / 10000.0) * 0.65;
set_motor_control(0,m_ctrl); set_motor_control(MOTOR_1,m_ctrl);
m_ctrl.desired_speed = 0.3; m_ctrl.desired_speed = 0.3;
set_motor_control(1,m_ctrl); set_motor_control(MOTOR_2,m_ctrl);
set_motor_control(2,m_ctrl); set_motor_control(MOTOR_3,m_ctrl);
} }

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

@ -50,7 +50,7 @@ static void motor_control_task(void *pvParameters) {
if (m_ctrl.enabled) { if (m_ctrl.enabled) {
actual_speed = get_roller_speed(m_id); actual_speed = get_roller_speed((roller_id_t)m_id);
error = m_ctrl.desired_speed - actual_speed; error = m_ctrl.desired_speed - actual_speed;
m_status[m_id].i_error += error * MC_ITERATION_TIME; m_status[m_id].i_error += error * MC_ITERATION_TIME;
d_error = (error - m_status[m_id].last_error) / MC_ITERATION_TIME; d_error = (error - m_status[m_id].last_error) / MC_ITERATION_TIME;
@ -101,27 +101,27 @@ static void motor_control_task(void *pvParameters) {
} }
} }
void set_motor_control(int motor_id, motor_control_t new_control) { void set_motor_control(motor_id_t motor_id, motor_control_t new_control) {
xQueueOverwrite(motor_control_queue[motor_id], &new_control); xQueueOverwrite(motor_control_queue[motor_id], &new_control);
} }
void set_motor_params(int motor_id, motor_parameters_t new_params) { void set_motor_params(motor_id_t motor_id, motor_parameters_t new_params) {
xQueueOverwrite(motor_parameters_queue[motor_id], &new_params); xQueueOverwrite(motor_parameters_queue[motor_id], &new_params);
} }
motor_control_t get_motor_control(int motor_id) { motor_control_t get_motor_control(motor_id_t motor_id) {
motor_control_t current_motor_control; motor_control_t current_motor_control;
xQueuePeek(motor_control_queue[motor_id], &current_motor_control, 0); xQueuePeek(motor_control_queue[motor_id], &current_motor_control, 0);
return current_motor_control; return current_motor_control;
} }
motor_parameters_t get_motor_parameters(int motor_id) { motor_parameters_t get_motor_parameters(motor_id_t motor_id) {
motor_parameters_t current_motor_params; motor_parameters_t current_motor_params;
xQueuePeek(motor_parameters_queue[motor_id], &current_motor_params, 0); xQueuePeek(motor_parameters_queue[motor_id], &current_motor_params, 0);
return current_motor_params; return current_motor_params;
} }
float get_motor_duty(int motor_id) { float get_motor_duty(motor_id_t motor_id) {
motor_status_t motor_status; motor_status_t motor_status;
xQueuePeek(motor_status_queue[motor_id], &motor_status, 0); xQueuePeek(motor_status_queue[motor_id], &motor_status, 0);
@ -139,12 +139,13 @@ void motor_control_setup(void) {
// Prime queues - these are single item queues, and so functions reading from them just // Prime queues - these are single item queues, and so functions reading from them just
// assume something is there. // assume something is there.
// TODO - this is where we'd read in the saved motor params from flash. // TODO - this is where we'd read in the saved motor params from flash.
motor_control_t init_motor_control;
motor_parameters_t init_motor_params;
motor_status_t blank_motor_status; motor_status_t blank_motor_status;
motor_control_t default_motor_control = {.enabled = false, .desired_speed = 0.0};
motor_parameters_t default_motor_parameters = {
.kp = DEFAULT_KP, .ki = DEFAULT_KI, .kd = DEFAULT_KD, .min_duty = DEFAULT_MIN_DUTY};
xQueueOverwrite(motor_control_queue[m_id], &init_motor_control); xQueueOverwrite(motor_control_queue[m_id], &default_motor_control);
xQueueOverwrite(motor_parameters_queue[m_id], &init_motor_params); xQueueOverwrite(motor_parameters_queue[m_id], &default_motor_parameters);
xQueueOverwrite(motor_status_queue[m_id], &blank_motor_status); xQueueOverwrite(motor_status_queue[m_id], &blank_motor_status);
} }

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

@ -11,17 +11,20 @@
#define MAX_DUTY 0.99 // max duty in each phase - have slightly below 1.0 to give more dead time between phases #define MAX_DUTY 0.99 // max duty in each phase - have slightly below 1.0 to give more dead time between phases
#define DEFAULT_KP 0.5
#define DEFAULT_KI 0.0
#define DEFAULT_KD 0.0
#define DEFAULT_MIN_DUTY 0.1
void set_motor_control(motor_id_t motor_id, motor_control_t new_control);
void set_motor_control(int motor_id, motor_control_t new_control); void set_motor_params(motor_id_t motor_id, motor_parameters_t new_params);
void set_motor_params(int motor_id, motor_parameters_t new_params); motor_control_t get_motor_control(motor_id_t motor_id);
motor_control_t get_motor_control(int motor_id); motor_parameters_t get_motor_parameters(motor_id_t motor_id);
motor_parameters_t get_motor_parameters(int motor_id); float get_motor_duty(motor_id_t motor_id);
float get_motor_duty(int motor_id);
void motor_control_setup(void); void motor_control_setup(void);

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

@ -3,6 +3,7 @@
#include "roller_speed.h" #include "roller_speed.h"
#include "io.h" #include "io.h"
#include "freertos/FreeRTOS.h"
#include "driver/mcpwm.h" #include "driver/mcpwm.h"
#include "freertos/queue.h" #include "freertos/queue.h"
#include "soc/rtc.h" #include "soc/rtc.h"
@ -69,7 +70,7 @@ static bool speed_pulse_isr_handler(mcpwm_unit_t mcpwm, mcpwm_capture_channel_id
return high_task_wakeup == pdTRUE; return high_task_wakeup == pdTRUE;
} }
float get_roller_speed(int roller_id) { float get_roller_speed(roller_id_t roller_id) {
pulse_measurement_t pulse_measurement; pulse_measurement_t pulse_measurement;
// Get the pulse period in APB clock ticks. // Get the pulse period in APB clock ticks.
xQueuePeek(roller_measurement_queue[roller_id], &pulse_measurement, 0); xQueuePeek(roller_measurement_queue[roller_id], &pulse_measurement, 0);
@ -81,7 +82,7 @@ float get_roller_speed(int roller_id) {
return ((float)rtc_clk_apb_freq_get() / (float)(pulse_measurement.period)) / (float)(ROLLER_PULSES_PER_REV); return ((float)rtc_clk_apb_freq_get() / (float)(pulse_measurement.period)) / (float)(ROLLER_PULSES_PER_REV);
} }
float get_roller_revs(int roller_id) { float get_roller_revs(roller_id_t roller_id) {
pulse_measurement_t pulse_measurement; pulse_measurement_t pulse_measurement;
xQueuePeek(roller_measurement_queue[roller_id], &pulse_measurement, 0); xQueuePeek(roller_measurement_queue[roller_id], &pulse_measurement, 0);
return (float)pulse_measurement.pulses / (float)ROLLER_PULSES_PER_REV; return (float)pulse_measurement.pulses / (float)ROLLER_PULSES_PER_REV;

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

@ -1,7 +1,7 @@
#ifndef roller_speed_h #ifndef roller_speed_h
#define roller_speed_h #define roller_speed_h
#include "freertos/FreeRTOS.h" #include "../common/shared_structs.h"
#define REV_OVERFLOW 10000 // Number at which rev counts overflow back to 0 #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 ROLLER_PULSES_PER_REV 40 // Number of sensor pulses per roller rev
@ -10,10 +10,10 @@
void roller_speed_setup(void); void roller_speed_setup(void);
// Returns the last calculated speed for roller in revs/second. Only pass 0, 1 or 2 // Returns the last calculated speed for roller in revs/second.
float get_roller_speed(int roller_id); float get_roller_speed(roller_id_t roller_id);
// Returns the total rev count for a roller. Only pass 0, 1 or 2. Overflows back to 0 at REV_OVERFLOW // Returns the total rev count for a roller. Overflows back to 0 at REV_OVERFLOW
float get_roller_revs(int roller_id); float get_roller_revs(roller_id_t roller_id);
#endif #endif
Write Preview
Loading…
Cancel
Save