craspberry-pi
Raspberry Pi 4 missing reverse ticks from AB encoder
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,¶m);
// 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
Solution
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.
- Why is variable sized object initialization not allowed using const
- C not operator applied to int?
- When to use restrict and when not to
- Why the first node of a linked list is declared as a pointer?
- Problem with not working gdk_test_simulate_button in GTK3
- Configuring the STM32F407VET6 ADC using CMSIS
- Allow double floating point comparisons with Unity unit testing framework from Throw The Switch
- CMake imported library behaviour
- libssh2 public key authentication only requiring passphrase if key request is accepted
- Are arrays treated as static data structures by gcc compiler or as dynamic?
- Is there a 128 bit integer in gcc?
- Loading c library from c# with pinvoke on dotnet core
- Why does my compiler not accept fork(), despite my inclusion of <unistd.h>?
- C/C++ warn or prohibit literal string concatenation
- Fastest way to write & compile a C/C++ program in Windows
- How to pass a 2D dynamically allocated array to a function?
- Behavior of scanset
- How to create an ada lib.a and link to C
- MISRA C 2012: Rule-10.5
- How does closing a socket affects other's side read
- Test result fails for my code of Inspect Bits function
- How can I populate an array with multiple copies of a small value with the fewest operations possible?
- C default arguments
- Default values on arguments in C functions and function overloading in C
- What is the purpose of uint64_t ps_pledge variable in OpenBSD kernel code?
- How can I get a string representation for the error return values of wordexp(3) in C?
- Task manager - c/c++ application - address space allocated vs used memory
- How to correctly determine at compile time that _Float16 is supported?
- Why do my integer and float value disappear when compiling and running my code?
- How can I set the MQTT keepalive in the Azure IoTHub C SDK?