MD25 Encoder Registers

[Filippo Pardini]

According to the documentation:

"Each motor has its encoder count stored in an array of four bytes, together the bytes form a signed 32 bit number, the encoder count is captured on a read of the highest byte (registers 2, 6) and the subsequent lower bytes will be held until another read of the highest byte takes place. The count is stored with the highest byte in the lowest numbered register. The registers can be zeroed at any time by writing 32 (0x20) to the command register."

I understand that:

1. The encoder count can be represented by a signed long int variable (-2147483648 to 2147483647).
2. For the motor turning in one direction the count is a positive number and in the other direction is a negative number.
3. Registers 2,3,4,5 are used for encoder1 and 6,7,8,9 for encoder2.
4. The four bytes (32bits) of a signed long int variable correspond to registers 2,3,4,5 of encoder1 count from the MSB to the LSB.
5. The four bytes (32bits) of a signed long int variable correspond to registers 6,7,8,9 of encoder2 count from the MSB to the LSB.

Using C (CVAVR), I can do this to calculate the encoder count:

1. Read the four registers as unsigned char variables.
2. Cast the unsigned char variables to unsigned long int variables and shift left 24,16,8,0 bits each one in correspondence to the 2,3,4,5 or 6,7,8,9 registers.
3. Compose an unsigned long int variable by adding the four unsigned long int variables or ORing them.
4. Cast the unsigned long int variable obtained to a signed long int variable.

So, I write this code:

long int read_encoder1_count(void)
{
unsigned long int data;
data = 0;

i2c_start();
i2c_write(0xB0);
i2c_write(2);
i2c_start();
i2c_write(0xB1);
data |= (unsigned long int)i2c_read(0) << 24;
i2c_stop();

i2c_start();
i2c_write(0xB0);
i2c_write(3);
i2c_start();
i2c_write(0xB1);
data |= (unsigned long int)i2c_read(0) << 16;
i2c_stop();

i2c_start();
i2c_write(0xB0);
i2c_write(4);
i2c_start();
i2c_write(0xB1);
data |= (unsigned long int)i2c_read(0) << 8;
i2c_stop();

i2c_start();
i2c_write(0xB0);
i2c_write(5);
i2c_start();
i2c_write(0xB1);
data |= (unsigned long int)i2c_read(0);
i2c_stop();

return (long int)data;
}

long int read_encoder2_count(void)
{
unsigned long int data;
data = 0;

i2c_start();
i2c_write(0xB0);
i2c_write(6);
i2c_start();
i2c_write(0xB1);
data |= (unsigned long int)i2c_read(0) << 24;
i2c_stop();

i2c_start();
i2c_write(0xB0);
i2c_write(7);
i2c_start();
i2c_write(0xB1);
data |= (unsigned long int)i2c_read(0) << 16;
i2c_stop();

i2c_start();
i2c_write(0xB0);
i2c_write(8);
i2c_start();
i2c_write(0xB1);
data |= (unsigned long int)i2c_read(0) << 8;
i2c_stop();

i2c_start();
i2c_write(0xB0);
i2c_write(9);
i2c_start();
i2c_write(0xB1);
data |= (unsigned long int)i2c_read(0);
i2c_stop();

return (long int)data;
}

I made a bench test run (the wheels not touching the floor) using mode 3 with turn 0 (speed regulation enabled and resetting the encoders count at start) in a two-wheel robot at speed1 60 and the physical result was OK. But the reading (every 5 seconds) of the encoders count is strange:
1. One reading is positive and the other is negative. OK, the two motors run in opposite directions.
2. The two readings start from zero and increase continuosly in absolute value. OK.
2. The two readings differ in absolute value something as some dozens.
3. At some time (with the count greater than 30000 (maybe 32767 ???)) the encoders count change sign and start to decrease in absolute value (???).

In conclusion, there must be a mistake at some point. I welcome your help. thank you.

[chris]

We have tested the MD25 and the encoder registers work ok. There does not look anything wrong with the code, how are you displaying the data? your problem could be down to the print function.

[Filippo Pardini]

Chris, thank you for your reply.

I use printf to display the data. See the complete program:

/*****************************************************
Chip type : ATmega128
Program type : Application
AVR Core Clock frequency: 14,745600 MHz
Memory model : Small
External RAM size : 0
Data Stack size : 1024
*****************************************************/

#include <mega128.h>
#include <stdio.h>
#include <delay.h>

unsigned char read_register(unsigned char reg);
void set_mode(unsigned char mode);
void set_speed1(char velocity);
void set_speed2(char velocity);
void set_turn(char turn);
void set_equal_motor_velocity(char velocity);
void set_acceleration(unsigned char accel);
void reset_encoders_count(void);
void enable_speed_regulation(void);
void disable_speed_regulation(void);
void enable_timeout(void);
void disable_timeout(void);
unsigned char read_battery_voltage(void);
unsigned char read_motor1_current(void);
unsigned char read_motor2_current(void);
long int read_encoder1_count(void);
long int read_encoder2_count(void);

// I2C Bus functions
#asm
.equ __i2c_port=0x15 ;PORTC
.equ __sda_bit=2
.equ __scl_bit=3
#endasm
#include <i2c.h>

void main(void)
{
unsigned int curr1;
unsigned int curr2;
long int enc1;
long int enc2;
// USART0 initialization
// Communication Parameters: 8 Data, 1 Stop, No Parity
// USART0 Receiver: Off
// USART0 Transmitter: On
// USART0 Mode: Asynchronous
// USART0 Baud Rate: 115200
UCSR0A=0x00;
UCSR0B=0x08;
UCSR0C=0x06;
UBRR0H=0x00;
UBRR0L=0x07;

// I2C Bus initialization (1K8 resistors for SDA and SCL)
i2c_init();

set_acceleration(5);
enable_speed_regulation();
disable_timeout();
reset_encoders_count();
set_equal_motor_velocity(60);

while (1)
{
enc1 = read_encoder1_count();
enc2 = read_encoder2_count();
printf ("encoder1 = % 10d encoder2 = % 10d \n\r",enc1,enc2);
delay_ms(5000);
};
}

unsigned char read_register(unsigned char reg)
{
unsigned char data;
i2c_start();
i2c_write(0xB0);
i2c_write(reg);
i2c_start();
i2c_write(0xB1);
data = i2c_read(0);
i2c_stop();
return data;
}

void set_mode(unsigned char mode)
{
i2c_start();
i2c_write(0xB0);
i2c_write(15);
i2c_write(mode);
i2c_stop();
}

void set_speed1(char velocity)
{
i2c_start();
i2c_write(0xB0);
i2c_write(0);
i2c_write(velocity);
i2c_stop();
}

void set_speed2(char velocity)
{
i2c_start();
i2c_write(0xB0);
i2c_write(1);
i2c_write(velocity);
i2c_stop();
}

void set_turn(char turn)
{
set_speed2(turn);
}

void set_equal_motor_velocity(char velocity)
{
set_mode(3);
set_speed1(velocity);
set_turn(0);
}

void set_acceleration(unsigned char accel)
{
i2c_start();
i2c_write(0xB0);
i2c_write(14);
i2c_write(accel);
i2c_stop();
}

void reset_encoders_count(void)
{
i2c_start();
i2c_write(0xB0);
i2c_write(16);
i2c_write(32);
i2c_stop();
}

void enable_speed_regulation(void)
{
i2c_start();
i2c_write(0xB0);
i2c_write(16);
i2c_write(49);
i2c_stop();
}

void disable_speed_regulation(void)
{
i2c_start();
i2c_write(0xB0);
i2c_write(16);
i2c_write(48);
i2c_stop();
}

void enable_timeout(void)
{
i2c_start();
i2c_write(0xB0);
i2c_write(16);
i2c_write(51);
i2c_stop();
}

void disable_timeout(void)
{
i2c_start();
i2c_write(0xB0);
i2c_write(16);
i2c_write(50);
i2c_stop();
}

unsigned char read_battery_voltage(void)
{
unsigned char data;
i2c_start();
i2c_write(0xB0);
i2c_write(10);
i2c_start();
i2c_write(0xB1);
data = i2c_read(0);
i2c_stop();
return data;
}

unsigned char read_motor1_current(void)
{
unsigned int data;
i2c_start();
i2c_write(0xB0);
i2c_write(11);
i2c_start();
i2c_write(0xB1);
data = (int)i2c_read(0) * 100; // Em miliamperes
i2c_stop();
return data;
}

unsigned char read_motor2_current(void)
{
unsigned int data;
i2c_start();
i2c_write(0xB0);
i2c_write(12);
i2c_start();
i2c_write(0xB1);
data = (int)i2c_read(0) * 100; // Em miliamperes
i2c_stop();
return data;
}

long int read_encoder1_count(void)
{
unsigned long int data;

data = 0;

i2c_start();
i2c_write(0xB0);
i2c_write(2);
i2c_start();
i2c_write(0xB1);
data |= (unsigned long int)i2c_read(0) << 24;
i2c_stop();

i2c_start();
i2c_write(0xB0);
i2c_write(3);
i2c_start();
i2c_write(0xB1);
data |= (unsigned long int)i2c_read(0) << 16;
i2c_stop();

i2c_start();
i2c_write(0xB0);
i2c_write(4);
i2c_start();
i2c_write(0xB1);
data |= (unsigned long int)i2c_read(0) << 8;
i2c_stop();

i2c_start();
i2c_write(0xB0);
i2c_write(5);
i2c_start();
i2c_write(0xB1);
data |= (unsigned long int)i2c_read(0);
i2c_stop();

return (long int)data;
}

long int read_encoder2_count(void)
{
unsigned long int data;

data = 0;

i2c_start();
i2c_write(0xB0);
i2c_write(6);
i2c_start();
i2c_write(0xB1);
data |= (unsigned long int)i2c_read(0) << 24;
i2c_stop();

i2c_start();
i2c_write(0xB0);
i2c_write(7);
i2c_start();
i2c_write(0xB1);
data |= (unsigned long int)i2c_read(0) << 16;
i2c_stop();

i2c_start();
i2c_write(0xB0);
i2c_write(8);
i2c_start();
i2c_write(0xB1);
data |= (unsigned long int)i2c_read(0) << 8;
i2c_stop();

i2c_start();
i2c_write(0xB0);
i2c_write(9);
i2c_start();
i2c_write(0xB1);
data |= (unsigned long int)i2c_read(0);
i2c_stop();

return (long int)data;
}

See the data printed:

encoder1 = 0 encoder2 = 0
encoder1 = 2712 encoder2 = -2709
encoder1 = 5482 encoder2 = -5477
encoder1 = 8254 encoder2 = -8246
encoder1 = 11027 encoder2 = -11014
encoder1 = 13798 encoder2 = -13785
encoder1 = 16567 encoder2 = -16556
encoder1 = 19338 encoder2 = -19325
encoder1 = 22111 encoder2 = -22095
encoder1 = 24882 encoder2 = -24863
encoder1 = 27651 encoder2 = -27631
encoder1 = 30422 encoder2 = -30402
encoder1 = -32343 encoder2 = 32365
encoder1 = -29570 encoder2 = 29596
encoder1 = -26800 encoder2 = 26828
encoder1 = -24032 encoder2 = 24059
encoder1 = -21263 encoder2 = 21288
encoder1 = -18492 encoder2 = 18518
encoder1 = -15723 encoder2 = 15751
encoder1 = -12953 encoder2 = 12982
encoder1 = -10183 encoder2 = 10211
encoder1 = -7412 encoder2 = 7440
encoder1 = -4642 encoder2 = 4671
encoder1 = -1871 encoder2 = 1902
encoder1 = 899 encoder2 = -870
encoder1 = 3669 encoder2 = -3640
encoder1 = 6439 encoder2 = -6409
encoder1 = 9209 encoder2 = -9179
encoder1 = 11978 encoder2 = -11947

[chris]

I suspect this is down to the compiler one way or another, in our compilers there is a build option for sprintf that defines what variables are able to be handled. I would print up the four individual encoder bytes as hex. This will then tell you if the number received is correct. Alternatively load enc1 with a constant of -123456 and see wether it prints correctly.

[chris]

I found this from a quick google, and thought that it might be pertinent:

These functions now support the precision specifier and displaying longs. Which version of these functions will be linked, can now be specified in the Project|Configure|C Compiler|(s)printf features option

[Filippo Pardini]

Yes, I know that and I use the correct configuration. But although I used the correct configuration for (s)printf (long,width,precision) it prints wrongly the numbers greater than 32767 and smaller than -32768. So, it uses the integer configuration (int,width) in place of the long int configuration. This is a bug in the compiler.

Now I will write to Pavel Haiduc from HP InfoTech to report the bug.

Chris, thank you for your help and support which was very meaningful to me.

[Filippo Pardini]

I received from Pavel the information that I must use the 'l' modifier before the 'd' to print long int numbers:

printf ("encoder1 = % 10ld encoder2 = % 10ld \n\r",enc1,enc2);

Now the printing is OK and the program is working well. Here is the last version using CodeVisionAVR 2.04.4

/*****************************************************
Chip type : ATmega128
Program type : Application
AVR Core Clock frequency: 14,745600 MHz
Memory model : Small
External RAM size : 0
Data Stack size : 1024
*****************************************************/

#include <mega128.h>
#include <stdio.h>
#include <delay.h>

unsigned char read_register(unsigned char reg);
void write_unsigned_register(unsigned char reg,unsigned char val);
void write_signed_register(unsigned char reg,char val);
void set_mode(unsigned char mode);
void set_speed1(char velocity);
void set_speed2(char velocity);
void set_turn(char turn);
void set_equal_motor_velocity(char velocity);
void set_acceleration(unsigned char accel);
void reset_encoders_count(void);
void enable_speed_regulation(void);
void disable_speed_regulation(void);
void enable_timeout(void);
void disable_timeout(void);
unsigned char read_battery_voltage(void);
unsigned char read_motor1_current(void);
unsigned char read_motor2_current(void);
long int read_encoder1_count(void);
long int read_encoder2_count(void);

// I2C Bus functions
#asm
.equ __i2c_port=0x15 ;PORTC
.equ __sda_bit=2
.equ __scl_bit=3
#endasm
#include <i2c.h>

void main(void)
{
long int enc1;
long int enc2;
// USART0 initialization
// Communication Parameters: 8 Data, 1 Stop, No Parity
// USART0 Receiver: Off
// USART0 Transmitter: On
// USART0 Mode: Asynchronous
// USART0 Baud Rate: 115200
UCSR0A=0x00;
UCSR0B=0x08;
UCSR0C=0x06;
UBRR0H=0x00;
UBRR0L=0x07;

// I2C Bus initialization (1K8 resistores para SDA e SCL)
i2c_init();

set_acceleration(5);
enable_speed_regulation();
disable_timeout();
reset_encoders_count();
set_equal_motor_velocity(60);

while (1)
{
enc1 = read_encoder1_count();
enc2 = read_encoder2_count();
printf ("encoder1 = % 10ld encoder2 = % 10ld \n\r",enc1,enc2);
delay_ms(5000);
};
}

unsigned char read_register(unsigned char reg)
{
unsigned char data;
i2c_start();
i2c_write(0xB0);
i2c_write(reg);
i2c_start();
i2c_write(0xB1);
data = i2c_read(0);
i2c_stop();
return data;
}

void write_unsigned_register(unsigned char reg,unsigned char val)
{
i2c_start();
i2c_write(0xB0);
i2c_write(reg);
i2c_write(val);
i2c_stop();
}

void write_signed_register(unsigned char reg,char val)
{
i2c_start();
i2c_write(0xB0);
i2c_write(reg);
i2c_write(val);
i2c_stop();
}

void set_mode(unsigned char mode)
{
write_unsigned_register(15,mode);
}

void set_speed1(char velocity)
{
write_signed_register(0,velocity);
}

void set_speed2(char velocity)
{
write_signed_register(1,velocity);
}

void set_turn(char turn)
{
set_speed2(turn);
}

void set_equal_motor_velocity(char velocity)
{
set_mode(3);
set_speed1(velocity);
set_turn(0);
}

void set_acceleration(unsigned char accel)
{
write_unsigned_register(14,accel);
}

void reset_encoders_count(void)
{
write_unsigned_register(16,32);
}

void enable_speed_regulation(void)
{
write_unsigned_register(16,49);
}

void disable_speed_regulation(void)
{
write_unsigned_register(16,48);
}

void enable_timeout(void)
{
write_unsigned_register(16,51);
}

void disable_timeout(void)
{
write_unsigned_register(16,50);
}

unsigned char read_battery_voltage(void)
{
return read_register(10);
}

unsigned char read_motor1_current(void)
{
return (int)read_register(11) * 100; // Em miliamperes
}

unsigned char read_motor2_current(void)
{
return (int)read_register(12) * 100; // Em miliamperes
}

long int read_encoder1_count(void)
{
unsigned long int data;

data = 0;
data |= (unsigned long int)read_register(2) << 24;
data |= (unsigned long int)read_register(3) << 16;
data |= (unsigned long int)read_register(4) << 8;
data |= (unsigned long int)read_register(5);
return (long int)data;
}

long int read_encoder2_count(void)
{
unsigned long int data;

data = 0;
data |= (unsigned long int)read_register(6) << 24;
data |= (unsigned long int)read_register(7) << 16;
data |= (unsigned long int)read_register(8) << 8;
data |= (unsigned long int)read_register(9);
return (long int)data;
}