Skip to content

Increase readability of ctrl.c #214

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
juhum1 wants to merge 3 commits into
base: dev
Choose a base branch
from
Open

Increase readability of ctrl.c #214

Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
13342e4
added encoder enum, ternary operators, macros (Jacob)
juhum1 Nov 10, 2024
67f1bd6
clang formatting, array indexing
juhum1 Nov 15, 2024
45ba476
Merge branch 'dev' into user/juhum1/ctrl
juhum1 Nov 15, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
231 changes: 114 additions & 117 deletions components/ctrl/src/ctrl.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,10 +11,10 @@
#include <opencan_tx.h>
#include <selfdrive/pid.h>

#define ENCODER0_CHAN_A 3
#define ENCODER0_CHAN_B 4
#define ENCODER1_CHAN_A 36
#define ENCODER1_CHAN_B 35
#define ENCODER_LEFT_CHAN_A 3
#define ENCODER_LEFT_CHAN_B 4
#define ENCODER_RIGHT_CHAN_A 36
#define ENCODER_RIGHT_CHAN_B 35

#define ESP_INTR_FLAG_DEFAULT 0

Expand Down Expand Up @@ -51,18 +51,73 @@
#define PID_DEADBAND_LOWER 0
#define PID_DEADBAND_UPPER 0

enum encoders {
ENCODER_LEFT,
ENCODER_RIGHT,
ENCODER_COUNT,
};

#define ENCODER_CHANNEL_A 0
#define ENCODER_CHANNEL_B 1

#define ENCODER_CHANNEL_ISR_SYMBOL_( \
ENCODER_N, ENCODER_CHAN_A, ENCODER_CHAN_B, CHANNEL) \
(ENCODER_N##_channel_##CHANNEL)
#define ENCODER_CHANNEL_ISR_SYMBOL( \
ENCODER_N, ENCODER_CHAN_A, ENCODER_CHAN_B, CHANNEL) \
ENCODER_CHANNEL_ISR_SYMBOL_( \
ENCODER_N, ENCODER_CHAN_A, ENCODER_CHAN_B, CHANNEL)

#define ENCODER_CHANNEL_ISR_DECLARATION( \
ENCODER_N, ENCODERN_CHAN_A, ENCODERN_CHAN_B, CHANNEL) \
void ENCODER_CHANNEL_ISR_SYMBOL( \
ENCODER_N, ENCODERN_CHAN_A, ENCODERN_CHAN_B, CHANNEL)( \
void *arg)

//clang-format off
#define ENCODER_CHANNEL_ISR( \
ENCODER_N, ENCODERN_CHAN_A, ENCODERN_CHAN_B, CHANNEL) \
ENCODER_CHANNEL_ISR_DECLARATION( \
ENCODER_N, ENCODERN_CHAN_A, ENCODERN_CHAN_B, CHANNEL) \
{ \
(void) arg; \
\
const uint32_t chan_a = gpio_get_level(ENCODERN_CHAN_A); \
const uint32_t chan_b = gpio_get_level(ENCODERN_CHAN_B); \
\
pulse_cnt[ENCODER_N] += ((CHANNEL == ENCODER_CHANNEL_A) \
^ (chan_a ^ chan_b)) \
? 1 \
: -1; \
Comment on lines +88 to +91
Copy link
Member

@jacobkoziej jacobkoziej Nov 11, 2024

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Let's disable clang formatting for the macro definitions so that we don't have strange formatting like this.

}


static ENCODER_CHANNEL_ISR_DECLARATION(ENCODER_LEFT,
ENCODER_LEFT_CHAN_A,
ENCODER_LEFT_CHAN_B,
ENCODER_CHANNEL_A);
static ENCODER_CHANNEL_ISR_DECLARATION(ENCODER_LEFT,
ENCODER_LEFT_CHAN_A,
ENCODER_LEFT_CHAN_B,
ENCODER_CHANNEL_B);
static ENCODER_CHANNEL_ISR_DECLARATION(ENCODER_RIGHT,
ENCODER_LEFT_CHAN_A,
ENCODER_LEFT_CHAN_B,
ENCODER_CHANNEL_A);
static ENCODER_CHANNEL_ISR_DECLARATION(ENCODER_RIGHT,
ENCODER_LEFT_CHAN_A,
ENCODER_LEFT_CHAN_B,
ENCODER_CHANNEL_B);
// clang-format on

static void ctrl_init();
static void ctrl_100Hz();
static void calculate_average_velocity(
int16_t left_delta, int16_t right_delta);
static void encoder0_chan_a(void *arg);
static void encoder0_chan_b(void *arg);
static void encoder1_chan_a(void *arg);
static void encoder1_chan_b(void *arg);
static void velocity_control(
float desired_velocity, float desired_acceleration);

static volatile uint16_t pulse_cnt[2];
static volatile uint16_t pulse_cnt[ENCODER_COUNT];
static bool speed_alarm;
static uint8_t brake_percent;
static uint8_t throttle_percent;
Expand All @@ -80,22 +135,42 @@ ember_rate_funcs_S module_rf = {

static void ctrl_init()
{
gpio_set_direction(ENCODER0_CHAN_A, GPIO_MODE_INPUT);
gpio_set_direction(ENCODER0_CHAN_B, GPIO_MODE_INPUT);
gpio_set_direction(ENCODER1_CHAN_A, GPIO_MODE_INPUT);
gpio_set_direction(ENCODER1_CHAN_B, GPIO_MODE_INPUT);
gpio_set_direction(ENCODER_LEFT_CHAN_A, GPIO_MODE_INPUT);
gpio_set_direction(ENCODER_LEFT_CHAN_B, GPIO_MODE_INPUT);
gpio_set_direction(ENCODER_RIGHT_CHAN_A, GPIO_MODE_INPUT);
gpio_set_direction(ENCODER_RIGHT_CHAN_B, GPIO_MODE_INPUT);

gpio_set_intr_type(ENCODER0_CHAN_A, GPIO_INTR_ANYEDGE);
gpio_set_intr_type(ENCODER0_CHAN_B, GPIO_INTR_ANYEDGE);
gpio_set_intr_type(ENCODER1_CHAN_A, GPIO_INTR_ANYEDGE);
gpio_set_intr_type(ENCODER1_CHAN_B, GPIO_INTR_ANYEDGE);
gpio_set_intr_type(ENCODER_LEFT_CHAN_A, GPIO_INTR_ANYEDGE);
gpio_set_intr_type(ENCODER_LEFT_CHAN_B, GPIO_INTR_ANYEDGE);
gpio_set_intr_type(ENCODER_RIGHT_CHAN_A, GPIO_INTR_ANYEDGE);
gpio_set_intr_type(ENCODER_RIGHT_CHAN_B, GPIO_INTR_ANYEDGE);

gpio_install_isr_service(ESP_INTR_FLAG_DEFAULT);

gpio_isr_handler_add(ENCODER0_CHAN_A, encoder0_chan_a, NULL);
gpio_isr_handler_add(ENCODER0_CHAN_B, encoder0_chan_b, NULL);
gpio_isr_handler_add(ENCODER1_CHAN_A, encoder1_chan_a, NULL);
gpio_isr_handler_add(ENCODER1_CHAN_B, encoder1_chan_b, NULL);
gpio_isr_handler_add(ENCODER_LEFT_CHAN_A,
ENCODER_CHANNEL_ISR_SYMBOL(ENCODER_LEFT,
ENCODER_LEFT_CHAN_A,
ENCODER_LEFT_CHAN_B,
ENCODER_CHANNEL_A),
NULL);
gpio_isr_handler_add(ENCODER_LEFT_CHAN_B,
ENCODER_CHANNEL_ISR_SYMBOL(ENCODER_LEFT,
ENCODER_LEFT_CHAN_A,
ENCODER_LEFT_CHAN_B,
ENCODER_CHANNEL_B),
NULL);
gpio_isr_handler_add(ENCODER_RIGHT_CHAN_A,
ENCODER_CHANNEL_ISR_SYMBOL(ENCODER_RIGHT,
ENCODER_LEFT_CHAN_A,
ENCODER_LEFT_CHAN_B,
ENCODER_CHANNEL_A),
NULL);
gpio_isr_handler_add(ENCODER_RIGHT_CHAN_B,
ENCODER_CHANNEL_ISR_SYMBOL(ENCODER_RIGHT,
ENCODER_LEFT_CHAN_A,
ENCODER_LEFT_CHAN_B,
ENCODER_CHANNEL_B),
NULL);

selfdrive_pid_init(&pid,
KP,
Expand All @@ -112,15 +187,19 @@ static void ctrl_init()

static void ctrl_100Hz()
{
static uint16_t prv_pulse_cnt[2];
static uint16_t prv_pulse_cnt[ENCODER_COUNT];

const uint16_t cur_pulse_cnt[2] = {pulse_cnt[0], pulse_cnt[1]};
const uint16_t cur_pulse_cnt[ENCODER_COUNT]
= {[ENCODER_LEFT] = pulse_cnt[ENCODER_LEFT],
[ENCODER_RIGHT] = pulse_cnt[ENCODER_RIGHT]};

const int16_t left_delta = cur_pulse_cnt[0] - prv_pulse_cnt[0];
const int16_t right_delta = cur_pulse_cnt[1] - prv_pulse_cnt[1];
const int16_t left_delta
= cur_pulse_cnt[ENCODER_LEFT] - prv_pulse_cnt[ENCODER_RIGHT];
const int16_t right_delta
= cur_pulse_cnt[ENCODER_RIGHT] - prv_pulse_cnt[ENCODER_RIGHT];

prv_pulse_cnt[0] = cur_pulse_cnt[0];
prv_pulse_cnt[1] = cur_pulse_cnt[1];
prv_pulse_cnt[ENCODER_LEFT] = cur_pulse_cnt[ENCODER_RIGHT];
prv_pulse_cnt[ENCODER_RIGHT] = cur_pulse_cnt[ENCODER_RIGHT];

calculate_average_velocity(left_delta, right_delta);

Expand Down Expand Up @@ -206,94 +285,6 @@ static void calculate_average_velocity(int16_t left_delta, int16_t right_delta)
average_velocity = ticks_10ms * METERS_PER_TICK * 100;
}

static void IRAM_ATTR encoder0_chan_a(void *arg)
{
(void) arg;

const uint32_t chan_a = gpio_get_level(ENCODER0_CHAN_A);
const uint32_t chan_b = gpio_get_level(ENCODER0_CHAN_B);

if (chan_a) {
if (chan_b) {
--pulse_cnt[0];
} else {
++pulse_cnt[0];
}
} else {
if (chan_b) {
++pulse_cnt[0];
} else {
--pulse_cnt[0];
}
}
}

static void IRAM_ATTR encoder0_chan_b(void *arg)
{
(void) arg;

const uint32_t chan_a = gpio_get_level(ENCODER0_CHAN_A);
const uint32_t chan_b = gpio_get_level(ENCODER0_CHAN_B);

if (chan_b) {
if (chan_a) {
++pulse_cnt[0];
} else {
--pulse_cnt[0];
}
} else {
if (chan_a) {
--pulse_cnt[0];
} else {
++pulse_cnt[0];
}
}
}

static void IRAM_ATTR encoder1_chan_a(void *arg)
{
(void) arg;

const uint32_t chan_a = gpio_get_level(ENCODER1_CHAN_A);
const uint32_t chan_b = gpio_get_level(ENCODER1_CHAN_B);

if (chan_a) {
if (chan_b) {
--pulse_cnt[1];
} else {
++pulse_cnt[1];
}
} else {
if (chan_b) {
++pulse_cnt[1];
} else {
--pulse_cnt[1];
}
}
}

static void IRAM_ATTR encoder1_chan_b(void *arg)
{
(void) arg;

const uint32_t chan_a = gpio_get_level(ENCODER1_CHAN_A);
const uint32_t chan_b = gpio_get_level(ENCODER1_CHAN_B);

if (chan_b) {
if (chan_a) {
++pulse_cnt[1];
} else {
--pulse_cnt[1];
}
} else {
if (chan_a) {
--pulse_cnt[1];
} else {
++pulse_cnt[1];
}
}
}

static void velocity_control(
float desired_velocity, float desired_acceleration)
{
Expand Down Expand Up @@ -358,6 +349,12 @@ void CANTX_populateTemplate_VelocityCommand(
void CANTX_populateTemplate_EncoderData(
struct CAN_TMessage_EncoderData * const m)
{
m->encoderLeft = pulse_cnt[0];
m->encoderRight = pulse_cnt[1];
m->encoderLeft = pulse_cnt[ENCODER_LEFT];
m->encoderRight = pulse_cnt[ENCODER_RIGHT];
}

// clang-format off
static IRAM_ATTR ENCODER_CHANNEL_ISR(ENCODER_LEFT, ENCODER_LEFT_CHAN_A, ENCODER_LEFT_CHAN_B, ENCODER_CHANNEL_A)
static IRAM_ATTR ENCODER_CHANNEL_ISR(ENCODER_LEFT, ENCODER_LEFT_CHAN_A, ENCODER_LEFT_CHAN_B, ENCODER_CHANNEL_B)
static IRAM_ATTR ENCODER_CHANNEL_ISR(ENCODER_RIGHT, ENCODER_LEFT_CHAN_A, ENCODER_LEFT_CHAN_B, ENCODER_CHANNEL_A)
static IRAM_ATTR ENCODER_CHANNEL_ISR(ENCODER_RIGHT, ENCODER_LEFT_CHAN_A, ENCODER_LEFT_CHAN_B, ENCODER_CHANNEL_B)
Loading