0

I posted this on stack exchange, but it isn't getting much traction so I'm posting here, hopefully someone has run into this before!

I am writing a program to control a robot in C using a raspberry pi, but for some reason, I am consistently missing reverse encoder steps. CW is positive, CCW is negative. When the wheel completes a cycle (1 rot CW, 1 rot CCW) the encoder ends up with hundreds of positive ticks left when it should be zero.

Setup:

  • Raspberry Pi 4 Model B
  • 2x Brushed DC motor with encoder. Amazon link
    • 5v power from regulator
    • Encoder Right A: GPIO 19
    • Encoder Right B: GPIO 21
    • Encoder Left A: GPIO 11
    • Encoder Left B: GPIO 13
  • Motor controller L298N to power the motor.
    • Left Enable (speed control): GPIO pin 12 (PWM 0)
    • Right Enable (speed control): GPIO pin 35 (PWM 1)
    • Left Forward: GPIO pin 24
    • Left Reverse: GPIO pin 22
    • Right Forward: GPIO pin 26
    • Right Reverse: GPIO pin 18

Here is the confusing part. I measured the top speed of the motor, and it is able to do 10 rotations in ~12 seconds (0.833 rot/sec). And based on documentation from a very similar motor by servo-city (which I tested and found to be applicable to this motor), there are ~3575.0855 ticks per rotation. This means I need to be able to read 2979.2379 ticks per second. ~3000Hz (335 us between edges, worst case) does not seem unreasonable.

I used the Realtime library provided here to measure the execution time. With optimizations, I am able to get the loop time below a max of 50 us, which should be more than enough to catch the edge changes. My question is, why might this not be working, and what should I try to fix it?

test.c

// g++ -o test test.c Realtime.cc -lm -lbcm2835

#include <bcm2835.h>
#include <stdio.h>
#include <ncurses.h>
#include <unistd.h>
#include <termios.h>
#include <fcntl.h>

#include "Realtime.h"

#define MOTOR_L_R RPI_V2_GPIO_P1_22
#define MOTOR_L_F RPI_V2_GPIO_P1_24
#define MOTOR_R_F RPI_V2_GPIO_P1_26
#define MOTOR_R_R RPI_V2_GPIO_P1_18

#define MOTOR_PWM_L RPI_V2_GPIO_P1_12
#define MOTOR_PWM_R RPI_V2_GPIO_P1_35

#define LIGHT_L RPI_V2_GPIO_P1_03
#define LIGHT_R RPI_V2_GPIO_P1_05

#define ENCODER_R_A RPI_V2_GPIO_P1_19
#define ENCODER_R_B RPI_V2_GPIO_P1_21
#define ENCODER_L_A RPI_V2_GPIO_P1_11
#define ENCODER_L_B RPI_V2_GPIO_P1_13

#define TICKS_PER_REV 3575.0855;
#define TICKS_PER_RAD 568.99252930116018242497;


void setup_terminal()
{
    struct termios term;
    tcgetattr(STDIN_FILENO, &term);
    term.c_lflag &= ~(ICANON | ECHO);
    tcsetattr(STDIN_FILENO, TCSANOW, &term);
}

char read_keyboard_input()
{
    char c;
    read(STDIN_FILENO, &c, 1);
    return c;
}

int main(int argc, char **argv)
{

    // Set the terminal to take inputs immediately, without waiting for [ENTER]
    setup_terminal();
    if (!bcm2835_init())
        return 1;

    int flags = fcntl(STDIN_FILENO, F_GETFL, 0);
    fcntl(STDIN_FILENO, F_SETFL, flags | O_NONBLOCK);

    // Configure the two PWM ports that control the speed of the wheels
    bcm2835_gpio_fsel(MOTOR_PWM_L, BCM2835_GPIO_FSEL_ALT5); // PWM0
    bcm2835_pwm_set_mode(0, 1, 1);                                // mark-space mode
    bcm2835_pwm_set_range(0, 1024);                               // 10-bit range
    bcm2835_pwm_set_clock(BCM2835_PWM_CLOCK_DIVIDER_16);          // 16 prescaler
    bcm2835_pwm_set_data(0, 1023);

    bcm2835_gpio_fsel(MOTOR_PWM_R, BCM2835_GPIO_FSEL_ALT5); // PWM0
    bcm2835_pwm_set_mode(1, 1, 1);                                // mark-space mode
    bcm2835_pwm_set_range(1, 1024);                               // 10-bit range
    bcm2835_pwm_set_clock(BCM2835_PWM_CLOCK_DIVIDER_16);          // 16 prescaler
    bcm2835_pwm_set_data(1, 1023);

    // Set the motors as outputs
    bcm2835_gpio_fsel(MOTOR_L_R, BCM2835_GPIO_FSEL_OUTP);
    bcm2835_gpio_fsel(MOTOR_L_F, BCM2835_GPIO_FSEL_OUTP);
    bcm2835_gpio_fsel(MOTOR_R_F, BCM2835_GPIO_FSEL_OUTP);
    bcm2835_gpio_fsel(MOTOR_R_R, BCM2835_GPIO_FSEL_OUTP);

    // Set the lights as outputs
    bcm2835_gpio_fsel(LIGHT_L, BCM2835_GPIO_FSEL_OUTP);
    bcm2835_gpio_fsel(LIGHT_R, BCM2835_GPIO_FSEL_OUTP);

    bcm2835_gpio_write(LIGHT_L, LOW);
    bcm2835_gpio_write(LIGHT_R, LOW);

    // Set the encoders as inputs
    bcm2835_gpio_fsel(ENCODER_L_A, BCM2835_GPIO_FSEL_INPT);
    bcm2835_gpio_fsel(ENCODER_L_B, BCM2835_GPIO_FSEL_INPT);
    bcm2835_gpio_set_pud(ENCODER_L_A, BCM2835_GPIO_PUD_UP);
    bcm2835_gpio_set_pud(ENCODER_L_B, BCM2835_GPIO_PUD_UP);

    uint8_t encoder_l_a = bcm2835_gpio_lev(ENCODER_L_A);
    uint8_t encoder_l_b = bcm2835_gpio_lev(ENCODER_L_B);
    int max_delay = 0;
    int last_change = -1;
    int c;
    int num_ticks = 0;
    Realtime::setup();
    uint32_t begin = Realtime::micros();
    uint32_t end = Realtime::micros();
    while (1)
    {
        // CW is positive ticks
        uint8_t encoder_l_a_new = bcm2835_gpio_lev(ENCODER_L_A);
        uint8_t encoder_l_b_new = bcm2835_gpio_lev(ENCODER_L_B);
        if(encoder_l_a_new != encoder_l_a)
        {
            if(encoder_l_a > encoder_l_a_new) // high to low
            {
                if(encoder_l_b)
                {
                    // printf("L\n");
                    num_ticks--;
                }
                else
                {
                    // printf("R\n");
                    num_ticks++;
                }
            }
            else
            {
                if(encoder_l_b)
                {
                    // printf("R\n");
                    num_ticks++;
                }
                else
                {
                    // printf("L\n");
                    num_ticks--;
                }
            }
            // printf("A\n");
            encoder_l_a = encoder_l_a_new;
            if(last_change == 1)
            {
                // num_ticks++;
            }
            last_change = 0;
            
            if(num_ticks ==0)
            {
                printf("%d\n",num_ticks);
            }
        }
        if(encoder_l_b_new != encoder_l_b)
        {
            if(encoder_l_b > encoder_l_b_new) // high to low
            {
                if(encoder_l_a)
                {
                    // printf("R\n");
                    num_ticks++;
                }
                else
                {
                    // printf("L\n");
                    num_ticks--;
                }
            }
            else
            {
                if(encoder_l_a)
                {
                    // printf("L\n");
                    num_ticks--;
                }
                else
                {
                    // printf("R\n");
                    num_ticks++;
                }
            }
            // printf("B\n");
            encoder_l_b = encoder_l_b_new;
            if(last_change == 0)
            {
                // num_ticks++;
            }
            last_change = 1;
            if(num_ticks ==0)
            {
                printf("%d\n",num_ticks);
            }
        }



        end = Realtime::micros();
        if(end-begin > max_delay)
        {
            max_delay = end-begin;
            printf("d:%d\n",max_delay);
        }
        begin = Realtime::micros();

    }
    bcm2835_close();
    return 0;
}

Realtime.cc

//  Realtime.cc
/*
    Copyright 2018 by Tom Roberts. All rights reserved.
    This code may be redistributed in accordance with the 
    Gnu Public License: https://www.gnu.org/licenses/gpl-3.0.en.html
*/

#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include <fcntl.h>
#include <sys/mman.h>
#ifndef _GNU_SOURCE
#define _GNU_SOURCE
#endif
#include <sched.h>

#include "Realtime.h"

volatile uint32_t *Realtime::systReg = 0;
int Realtime::fdMem = -1;
uint32_t Realtime::phys = 0;

void Realtime::setup()
{
    initMicros();
    disableTurbo();
    realTimeSched();
    cpu3();
}

void Realtime::initMicros()
{
    // based on pigpio source; simplified and re-arranged
    fdMem = open("/dev/mem",O_RDWR|O_SYNC);
    if(fdMem < 0) {
        fprintf(stderr,"Cannot map memory (need sudo?)\n");
        exit(1);
    }
    // figure out the address
    FILE *f = fopen("/proc/cpuinfo","r");
    char buf[1024];
    fgets(buf,sizeof(buf),f); // skip first line
    fgets(buf,sizeof(buf),f); // model name

    // if(strstr(buf,"ARMv6")) {
    //  phys = 0x20000000;
    // } else if(strstr(buf,"ARMv7")) {
    //  phys = 0x3F000000;
    // } else if(strstr(buf,"ARMv8")) {
    //  phys = 0x3F000000;
    // } else {
    //  fprintf(stderr,"Unknown CPU type\n");
    //  exit(1);
    // }
    phys = 0xFE000000; // For raspberry pi 4
    fclose(f);
    systReg = (uint32_t *)mmap(0,0x1000,PROT_READ|PROT_WRITE,
                MAP_SHARED|MAP_LOCKED,fdMem,phys+0x3000);
}

void Realtime::delay(int us)
{
    // The final microsecond can be short; don't let the delay be short.
    ++us;

    // usleep() on its own gives latencies 20-40 us; this combination
    // gives < 25 us.
    uint32_t start = micros();
    if(us >= 100)
        usleep(us - 50);
    while(micros()-start < us)
        ;
}

void Realtime::disableTurbo()
{
    // This fixes the CPU clock to its minimum value, so timing is
    // not screwed up by changing it; also keep the SPI clock correct.
    system("sudo cp /sys/devices/system/cpu/cpu0/cpufreq/scaling_min_freq "
        "/sys/devices/system/cpu/cpu0/cpufreq/scaling_max_freq");
}

void Realtime::enableTurbo()
{
    system("sudo cp /sys/devices/system/cpu/cpu0/cpufreq/cpuinfo_max_freq "
        "/sys/devices/system/cpu/cpu0/cpufreq/scaling_max_freq");
}

void Realtime::realTimeSched()
{
    int prio = sched_get_priority_max(SCHED_FIFO);
    struct sched_param param;
    param.sched_priority = prio;
    sched_setscheduler(0,SCHED_FIFO,&param);
    // This permits realtime processes to use 100% of a CPU, but on a
    // RPi that starves the kernel. Without this there are latencies
    // up to 50 MILLISECONDS.
    system("echo -1 >/proc/sys/kernel/sched_rt_runtime_us");
}

void Realtime::cpu3()
{
    // this does nothing if there are fewer than 4 CPUs
    cpu_set_t cpuset;
    CPU_ZERO(&cpuset);
    CPU_SET(3,&cpuset);
    sched_setaffinity(0,sizeof(cpu_set_t),&cpuset);
}

void Realtime::cpu2()
{
    // this does nothing if there are fewer than 4 CPUs
    cpu_set_t cpuset;
    CPU_ZERO(&cpuset);
    CPU_SET(2,&cpuset);
    sched_setaffinity(0,sizeof(cpu_set_t),&cpuset);
}

#ifdef TEST_REALTIME

int main(int argc, char *argv[])
{
    Realtime::setup();
    printf("Realtime mode setup, here's a shell on CPU3\n");
    system("/bin/bash -i");
    return 0;
}

#endif //TEST_REALTIME
2

1 Answer 1

0

I figured out the problem. It turns out the encoders on the motors I bought are bad and their a/b signals overlap such that I can't extract useful data about the rotations. I've attached some images from an oscilloscope to show what I mean.

To confirm my theory, I wrote some code to test the encoders using an Arduino nano and experienced the exact same behavior.Image showing 0.0us delay Image showing 100us delay

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.