Skip to content

Commit

Permalink
Merge pull request #343 from aabadie/qdec_move_enh
Browse files Browse the repository at this point in the history 
bsp/qdec: drv/move: improve libraries
  • Loading branch information
@aabadie
aabadie authored Nov 14, 2024
2 parents b20fd77 + ffb4c67 commit b056377
Show file tree
Hide file tree
Showing 7 changed files with 160 additions and 54 deletions.
4 changes: 4 additions & 0 deletions .github/workflows/build.yml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -52,6 +52,10 @@ jobs:
steps:
- name: Checkout repo
uses: actions/checkout@v4
- name: Set up Python 3.12
uses: actions/setup-python@v5
with:
python-version: 3.12
- name: Install Doxygen
run: sudo apt-get install -y doxygen graphviz
- name: Install Sphinx
Expand Down
37 changes: 25 additions & 12 deletions bsp/nrf/qdec.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,19 +12,23 @@
#include <nrf_peripherals.h>
#include <assert.h>
#include <stdint.h>
#include <stdbool.h>
#include <stdio.h>
#include "gpio.h"
#include "qdec.h"

//=========================== defines ==========================================

typedef struct {
uint32_t overflow;
uint32_t underflow;
qdec_cb_t callback;
void *ctx;
} qdec_vars_t;

//=========================== variables ========================================

static qdec_vars_t _qdec_vars[QDEC_COUNT];
static qdec_vars_t _qdec_vars[QDEC_COUNT] = { 0 };
static NRF_QDEC_Type *_qdec_devs[QDEC_COUNT] = {
#if defined(NRF_TRUSTZONE_NONSECURE)
NRF_QDEC0_NS,
Expand Down Expand Up @@ -55,12 +59,10 @@ void db_qdec_init(qdec_t qdec, const qdec_conf_t *conf, qdec_cb_t callback, void
_qdec_vars[qdec].callback = callback;
_qdec_vars[qdec].ctx = ctx;

if (callback) {
// The driver is only interested in the accumulator overflow event
_qdec_devs[qdec]->INTENSET |= (QDEC_INTENSET_ACCOF_Enabled << QDEC_INTENSET_ACCOF_Pos);
NVIC_ClearPendingIRQ(QDEC0_IRQn + qdec);
NVIC_EnableIRQ(QDEC0_IRQn + qdec);
}
// The driver is only interested in the accumulator overflow event
_qdec_devs[qdec]->INTENSET |= (QDEC_INTENSET_ACCOF_Enabled << QDEC_INTENSET_ACCOF_Pos);
NVIC_ClearPendingIRQ(QDEC0_IRQn + qdec);
NVIC_EnableIRQ(QDEC0_IRQn + qdec);

// Enable debounce filter
_qdec_devs[qdec]->DBFEN = QDEC_DBFEN_DBFEN_Enabled << QDEC_DBFEN_DBFEN_Pos;
Expand All @@ -72,18 +74,29 @@ void db_qdec_init(qdec_t qdec, const qdec_conf_t *conf, qdec_cb_t callback, void
_qdec_devs[qdec]->TASKS_START = (QDEC_TASKS_START_TASKS_START_Trigger << QDEC_TASKS_START_TASKS_START_Pos);
}

int16_t db_qdec_read(qdec_t qdec) {
return (int16_t)_qdec_devs[qdec]->ACC;
int32_t db_qdec_read(qdec_t qdec) {
return (int32_t)_qdec_devs[qdec]->ACC + (1023 * _qdec_vars[qdec].overflow) - (1024 * _qdec_vars[qdec].underflow);
}

int16_t db_qdec_read_and_clear(qdec_t qdec) {
int32_t db_qdec_read_and_clear(qdec_t qdec) {
_qdec_devs[qdec]->TASKS_RDCLRACC = 1;
return (int16_t)_qdec_devs[qdec]->ACCREAD;
int32_t count = (int32_t)_qdec_devs[qdec]->ACCREAD + (1023 * _qdec_vars[qdec].overflow) - (1024 * _qdec_vars[qdec].underflow);
_qdec_vars[qdec].overflow = 0;
_qdec_vars[qdec].underflow = 0;
return count;
}

static void qdec_isr(qdec_t qdec) {
_qdec_devs[qdec]->EVENTS_ACCOF = 0;
_qdec_vars[qdec].callback(_qdec_vars[qdec].ctx);
if (_qdec_devs[qdec]->SAMPLE < 0) {
_qdec_vars[qdec].underflow += 1;
} else {
_qdec_vars[qdec].overflow += 1;
}
_qdec_devs[qdec]->TASKS_RDCLRACC = 1;
if (_qdec_vars[qdec].callback) {
_qdec_vars[qdec].callback(_qdec_vars[qdec].ctx);
}
};

void QDEC0_IRQHandler(void) {
Expand Down
4 changes: 2 additions & 2 deletions bsp/qdec.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -46,13 +46,13 @@ void db_qdec_init(qdec_t qdec, const qdec_conf_t *conf, qdec_cb_t callback, void
*
* @param[in] qdec Index of the QDEC peripheral to read
*/
int16_t db_qdec_read(qdec_t qdec);
int32_t db_qdec_read(qdec_t qdec);

/**
* @brief Read and clear the QDEC accumulator
*
* @param[in] qdec Index of the QDEC peripheral to read and reset
*/
int16_t db_qdec_read_and_clear(qdec_t qdec);
int32_t db_qdec_read_and_clear(qdec_t qdec);

#endif
127 changes: 104 additions & 23 deletions drv/move/move.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,6 +29,8 @@
#define DOTBOT_WHEEL_RADIUS (20) //< mm
#define DOTBOT_RADIUS (45) //< mm
#define QDEC_LEFT (0)
#define QDEC_RIGHT (1)
#define MOTOR_OFFSET (5)
#define MOVE_TIMER_DEV (0)

//=========================== variables ========================================
Expand All @@ -38,18 +40,25 @@ static const qdec_conf_t qdec_left = {
.pin_b = &db_qdec_left_b_pin,
};

//=========================== private ==========================================
static const qdec_conf_t qdec_right = {
.pin_a = &db_qdec_right_a_pin,
.pin_b = &db_qdec_right_b_pin,
};

static float _distance_from_accumulator(int16_t accumulator) {
return (((float)accumulator / DOTBOT_ENCODER_CPR) * 2 * M_PI * DOTBOT_WHEEL_RADIUS) / DOTBOT_MOTOR_REDUCTION;
}
static int32_t _previous_left_accumulator = 0;
static int32_t _previous_right_accumulator = 0;

//=========================== private ==========================================

static float _angle_from_accumulator(int16_t accumulator) {
return (180 * _distance_from_accumulator(accumulator)) / (M_PI * DOTBOT_RADIUS);
static float _distance_from_angle(uint16_t angle) {
return ((float)angle * DOTBOT_RADIUS * M_PI) / 180;
}

static void _move_reset(void) {
db_qdec_read_and_clear(QDEC_LEFT);
db_qdec_read_and_clear(QDEC_RIGHT);
_previous_left_accumulator = 0;
_previous_right_accumulator = 0;
}

//=========================== public ===========================================
Expand All @@ -61,23 +70,59 @@ void db_move_init(void) {
db_timer_init(MOVE_TIMER_DEV);

db_qdec_init(QDEC_LEFT, &qdec_left, NULL, NULL);
db_qdec_init(QDEC_RIGHT, &qdec_right, NULL, NULL);
}

void db_move_straight(uint16_t distance, int8_t speed) {
_move_reset();

if (distance == 0) {
if (distance == 0 || speed == 0) {
return;
}

int32_t accumulator = 0;
db_motors_set_speed(speed, speed);
while (1) {
accumulator += db_qdec_read_and_clear(QDEC_LEFT);
float current_distance = fabs(_distance_from_accumulator(accumulator));
if ((current_distance > (float)distance)) {
break;
int8_t left_power = speed;
int8_t right_power = speed;
uint32_t offset = 5;
int32_t expected_distance = distance;
if (speed < 0) {
offset *= -1;
expected_distance *= 1;
}

float correction = -1.0;
if (speed > 0) {
expected_distance += correction;
} else if (speed < 0) {
expected_distance -= correction;
}

int16_t left_count = 0;
int16_t right_count = 0;
int16_t prev_left_count = 0;
int16_t prev_right_count = 0;
int16_t left_diff, right_diff;
float counts_per_rev = DOTBOT_ENCODER_CPR * DOTBOT_MOTOR_REDUCTION;
float wheel_diameter = DOTBOT_WHEEL_RADIUS * 2;
float wheel_circumference = M_PI * wheel_diameter;
float rev_count = expected_distance / wheel_circumference;
float target_count = rev_count * counts_per_rev;
db_motors_set_speed(left_power, right_power);

while (abs(right_count) < fabs(target_count)) {
left_count = db_qdec_read(QDEC_LEFT);
right_count = db_qdec_read(QDEC_RIGHT);
left_diff = abs(left_count - prev_left_count);
right_diff = abs(right_count - prev_right_count);
prev_left_count = left_count;
prev_right_count = right_count;
if (left_diff > right_diff) {
left_power = left_power - offset;
right_power = right_power + offset;
} else if (left_diff < right_diff) {
left_power = left_power + offset;
right_power = right_power - offset;
}
db_motors_set_speed(left_power, right_power);
db_timer_delay_ms(MOVE_TIMER_DEV, REFRESH_DELAY_MS);
}

Expand All @@ -86,19 +131,55 @@ void db_move_straight(uint16_t distance, int8_t speed) {

void db_move_rotate(uint16_t angle, int8_t speed) {
_move_reset();
if (angle == 0) {

if (angle == 0 || speed == 0) {
return;
}

int32_t accumulator = 0;
db_motors_set_speed(speed, speed * -1);
while (1) {
accumulator += db_qdec_read_and_clear(QDEC_LEFT);
float current_angle = fabs(_angle_from_accumulator(accumulator));
if ((current_angle > (float)angle)) {
break;
}
int8_t left_power = speed;
int8_t right_power = speed * -1;
uint32_t offset = 5;
// Compute distance from angle
int32_t expected_distance = _distance_from_angle(angle);
if (speed < 0) {
offset *= -1;
expected_distance *= 1;
}

float correction = -5.0;
if (speed > 0) {
expected_distance += correction;
} else if (speed < 0) {
expected_distance -= correction;
}

int16_t left_count = 0;
int16_t right_count = 0;
int16_t prev_left_count = 0;
int16_t prev_right_count = 0;
int16_t left_diff, right_diff;
float counts_per_rev = DOTBOT_ENCODER_CPR * DOTBOT_MOTOR_REDUCTION;
float wheel_diameter = DOTBOT_WHEEL_RADIUS * 2;
float wheel_circumference = M_PI * wheel_diameter;
float rev_count = expected_distance / wheel_circumference;
float target_count = rev_count * counts_per_rev;
db_motors_set_speed(left_power, right_power);

while (abs(right_count) < fabs(target_count)) {
left_count = db_qdec_read(QDEC_LEFT);
right_count = db_qdec_read(QDEC_RIGHT);
left_diff = abs(left_count - prev_left_count);
right_diff = abs(right_count - prev_right_count);
prev_left_count = left_count;
prev_right_count = right_count;
if (left_diff > right_diff) {
left_power -= offset;
right_power -= offset;
} else if (left_diff < right_diff) {
left_power += offset;
right_power += offset;
}
db_motors_set_speed(left_power, right_power);
db_timer_delay_ms(MOVE_TIMER_DEV, REFRESH_DELAY_MS);
}

Expand Down
4 changes: 2 additions & 2 deletions drv/tdma_server.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,10 +25,10 @@
#define TDMA_SERVER_MAX_CLIENTS 100 ///< Max number of clients that can register with this server
#define TDMA_SERVER_TIME_SLOT_DURATION_US 2500 ///< default timeslot for a tdma slot in microseconds
#define TDMA_SERVER_MAX_GATEWAY_TX_DELAY_US 20000 ///< Max amount of microseconds that can elapse between gateway transmissions
/// Total amount of slots available in the tdma table, adds extra slots to MAX_CLIENTS to accomodate the gateway slots
#define TDMA_SERVER_MAX_TABLE_SLOTS \
TDMA_SERVER_MAX_CLIENTS + \
TDMA_SERVER_MAX_CLIENTS / (TDMA_SERVER_MAX_GATEWAY_TX_DELAY / TDMA_SERVER_TIME_SLOT_DURATION - 1) + 1 ///< Total amount of slots available in the tdma table, adds extra slots to MAX_CLIENTS
// to accomodate the gateway slots
TDMA_SERVER_MAX_CLIENTS / (TDMA_SERVER_MAX_GATEWAY_TX_DELAY / TDMA_SERVER_TIME_SLOT_DURATION - 1) + 1

//=========================== variables ========================================

Expand Down
16 changes: 8 additions & 8 deletions projects/01bsp_qdec/01bsp_qdec.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -57,20 +57,20 @@ int main(void) {
db_qdec_init(QDEC_RIGHT, &qdec_right, _callback, (void *)&_qdec_vars.right_overflow);

db_timer_init(TIMER_DEV);
printf("ACC left: %i\n", db_qdec_read_and_clear(QDEC_LEFT));
printf("ACC right: %i\n", db_qdec_read_and_clear(QDEC_RIGHT));

while (1) {
if (!_qdec_vars.left_overflow) {
printf("ACC left: %i\n", db_qdec_read(QDEC_LEFT));
} else {
printf("ACC left (overflow): %i\n", db_qdec_read_and_clear(QDEC_LEFT));
if (_qdec_vars.left_overflow) {
puts("ACC left overflow");
_qdec_vars.left_overflow = false;
}
if (!_qdec_vars.right_overflow) {
printf("ACC right: %i\n", db_qdec_read(QDEC_RIGHT));
} else {
printf("ACC right (overflow): %i\n", db_qdec_read_and_clear(QDEC_RIGHT));
if (_qdec_vars.right_overflow) {
puts("ACC right overflow");
_qdec_vars.right_overflow = false;
}
printf("ACC left: %i\n", db_qdec_read(QDEC_LEFT));
printf("ACC right: %i\n", db_qdec_read(QDEC_RIGHT));
db_timer_delay_s(TIMER_DEV, 1);
}
}
22 changes: 15 additions & 7 deletions projects/01drv_move/01drv_move.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,13 +17,21 @@
int main(void) {

db_move_init();
db_move_straight(200, 60);
db_move_rotate(90, 60);
db_move_straight(200, 60);
db_move_rotate(90, 60);
db_move_straight(200, 60);
db_move_rotate(90, 60);
db_move_straight(200, 60);
db_move_straight(500, 50);
db_move_rotate(90, 45);
db_move_straight(500, 50);
db_move_rotate(90, 45);
db_move_straight(500, 50);
db_move_rotate(90, 45);
db_move_straight(500, 50);

db_move_straight(500, -50);
db_move_rotate(90, -45);
db_move_straight(500, -50);
db_move_rotate(90, -45);
db_move_straight(500, -50);
db_move_rotate(90, -45);
db_move_straight(500, -50);

while (1) {
__WFE();
Expand Down

0 comments on commit b056377

Please sign in to comment.