AMT21 Absolute Encoder (RS485) not recognized

Hi all.

For a research project, I’m trying to use the AMT21 series absolute capacitive encoder with the ROB-20441 BLDC (the “SmartKnob” motor) powered by the recommended TMC6300 driver from Sparkfun. My code is running on a Pi Pico through the Arduino IDE.

I modified some code from CUI Devices (the manufacturer of the encoder) to read its position using the RS485 serial protocol, and I can confirm that this works quite well. Additionally, I know the motor and driver both work, as SimpleFOC open loop velocity control resulted in very clean movement. I even used the encoder (not connected to the motor) as a “speed knob” to do some manual real-time speed control and velocity tracking, both of which worked great.

The wrinkle seems to be in pairing the motor and encoder together to close the loop on feedback control. Since SimpleFOC lacks native RS485 support, I created a GenericSensor for my encoder and put the code to read position inside the readMySensorCallback() function. This works when I call it separately in the loop(), but I haven’t gotten the motor to move at all under voltage torque control (except for the initFOC sequence)–probing with a scope shows normal PWM during initFOC and absolutely nothing during torque control. Every time on startup, SimpleFOCDebug throws “MOT: Align sensor → MOT: Failed to notice movement → MOT: Init FOC failed”

Any help would be much appreciated, as I’m on a tight timeline to get this working and have no clue what to try next. Cheers!

P.S. apologies for the messy code–it’s been a long few days of troubleshooting. I tried to clean up some junk and remove commented code that’s no longer relevant from random other tests, but it’s still a bit of a bear. Most of the bulk is the RS485 code.

// Open loop motor control example
#include <SimpleFOC.h>

// BLDC motor & driver instance
// BLDCMotor motor = BLDCMotor(pole pair number);
BLDCMotor motor = BLDCMotor(8); //it looks like 6 pairs, but the data sheet says 8...? both run fine though... larger the number, smaller the cogs
// BLDCDriver3PWM driver = BLDCDriver3PWM(pwmA, pwmB, pwmC, Enable(optional));
BLDCDriver6PWM driver = BLDCDriver6PWM(21, 20, 19, 18, 17, 16);

double enc_position_buffer[4] = {-1.0,-1.0,-1.0,-1.0}; //keeps current position along with three past values

//target variable
float target_velocity = 10;
float target_position = 0;
int tick = 0;

// instantiate the commander
Commander command = Commander(Serial);
void doMotor(char* cmd) { command.motor(&motor, cmd); }
//void doTarget(char* cmd) { command.scalar(&target_velocity, cmd); }
//void doLimit(char* cmd) { command.scalar(&motor.voltage_limit, cmd); }

/* Serial rates for UART */
#define BAUDRATE             115200
#define RS485_BAUDRATE       2000000

/* We will use this define macro so we can write code once compatible with 12 or 14 bit encoders */
#define RESOLUTION            14

/* The AMT21 encoder is able to have a range of different values for its node address. This allows there
 * to be multiple encoders on on the RS485 bus. The encoder will listen for its node address, and that node
 * address doubles as the position request command. This simplifies the protocol so that all the host needs
 * to do is transmit the node address and the encoder will immediately respond with the position. The node address
 * can be any 8-bit number, where the bottom 2 bits are both 0. This means that there are 63 available addresses.
 * The bottom two bits are left as zero, because those bit slots are used to indicate extended commands. Taking
 * the node address and adding 0x01 changes the command to reading the turns counter (multi-turn only), and adding
 * 0x02 indicates that a second extended command will follow the first. We will define two encoder addresses below,
 * and then we will define the modifiers, but to reduce code complexity and the number of defines, we will not
 * define every single variation. 0x03 is unused and therefore reserved at this time.
#define RS485_RESET           0x75
#define RS485_ZERO            0x5E
#define RS485_ENC0            0x54
#define RS485_ENC1            0x58
#define RS485_POS             0x00 //this is unnecessary to use but it helps visualize the process for using the other modifiers
#define RS485_TURNS           0x01
#define RS485_EXT             0x02

#define RS485_T_RE_DE         2 //Receive enable and drive enable (tied together)
#define RS485_T_DI            0 //driver data input (UART0 TX)
#define RS485_T_RO            1 //receive data output (UART0 RX)

/* For ease of reading, we will create a helper function to set the mode of the transceiver. We will send that funciton
 * these arguments corresponding with the mode we want.
#define RS485_T_TX            1 //transmit: receiver off, driver on
#define RS485_T_RX            0 //receiver: driver off, transmit on

//create an array of encoder addresses so we can use them in a loop
uint8_t addresses[1] = {RS485_ENC0};

uint32_t *SIO = (uint32_t *)0xd0000000;
double enc_velocity = 0.0;
double SAVITZY_GOLAY_4[4] = {0.3, 0.1, -0.1, -0.3}; //filter weights for buffer size of 4
double spike_threshold = 5.0;

int clock_frequency = rp2040.f_cpu(); //returns clock frequency
uint32_t start_cycle, end_cycle, cycles_in_loop;
double computation_time_us; //time spent reading encoders
int delta_t_us = 100; //desired total loop time in microseconds

int angle_min = 0;
int angle_max = 360;

bool verifyChecksumRS485(uint16_t message)
  //using the equation on the datasheet we can calculate the checksums and then make sure they match what the encoder sent
  //checksum is invert of XOR of bits, so start with 0b11, so things end up inverted
  uint16_t checksum = 0x3;
  for(int i = 0; i < 14; i += 2)
    checksum ^= (message >> i) & 0x3;
  return checksum == (message >> 14);
 * This function sets the state of the RS485 transceiver. We send it that state we want. Recall above I mentioned how we need to do this as quickly
 * as possible. To be fast, we are not using the digitalWrite functions but instead will access the avr io directly. I have shown the direct access
 * method and left commented the digitalWrite method.
void setStateRS485(uint8_t state)
  //switch case to find the mode we want
  switch (state)
    case RS485_T_TX: //DE_RE high
      *(SIO + 0x014 / 4) = 1ul << RS485_T_RE_DE;
    case RS485_T_RX: //low
      *(SIO + 0x018 / 4) = 1ul << RS485_T_RE_DE;
  //Serial.println(*(SIO + 0x010 / 4) >> RS485_T_RE_DE);

// returning an angle in radians in between 0 and 2PI
float readMySensorCallback(){
  //uint8_t addresses[2] = {RS485_ENC0, RS485_ENC1};
  for(int encoder = 0; encoder < sizeof(addresses); ++encoder)
    //first we will read the position
    setStateRS485(RS485_T_TX); //put the transciver into transmit mode
    delayMicroseconds(2);   //IO operations take time, let's throw in an arbitrary 10 microsecond delay to make sure the transeiver is ready

    //send the command to get position. All we have to do is send the node address, but we can use the modifier for consistency
    Serial1.write(addresses[encoder] | RS485_POS);
    //Serial.println("flag"); //debug
    //We expect a response from the encoder to begin within 3 microseconds. Each byte sent has a start and stop bit, so each 8-bit byte transmits
    //10 bits total. So for the AMT21 operating at 2 Mbps, transmitting the full 20 bit response will take about 10 uS. We expect the response
    //to start after 3 uS totalling 13 microseconds from the time we've finished sending data.
    //So we need to put the transceiver into receive mode within 3 uS, but we want to make sure the data has been fully transmitted before we
    //do that or we could cut it off mid transmission. This code has been tested and optimized for this; porting this code to another device must
    //take all this timing into account.

    //Here we will make sure the data has been transmitted and then toggle the pins for the transceiver
    //Here we are accessing the avr library to make sure this happens very fast. We could use Serial.flush() which waits for the output to complete
    //but it takes about 2 microseconds, which gets pretty close to our 3 microsecond window. Instead we want to wait until the serial transmit flag USCR1A completes.
    delayMicroseconds(5); //manually tuned with oscilloscope so response is received as soon as possible (4us good... 5us was less error prone?? Find way to do this in hardware!)
    //while (!(UCSR1A & _BV(TXC1)));

    setStateRS485(RS485_T_RX); //set the transceiver back into receive mode for the encoder response

    //Response from encoder should be exactly 2 bytes
    //Serial.print(; //This debugging step was messing me up!!! Once I read, the first byte is lost!
    //We need to give the encoder enough time to respond, but not too long. In a tightly controlled application we would want to use a timeout counter
    //to make sure we don't have any issues, but for this demonstration we will just have an arbitrary delay before checking to see if we have data to read.
    delayMicroseconds(12); //5 is way too short, 8 includes a lot of errors, 10 is about right, 20 is no better than 10

    int bytes_received = Serial1.available();
    if (bytes_received == 2)
      uint16_t currentPosition =; //low byte comes first
      currentPosition |= << 8;    //high byte next, OR it into our 16 bit holder but get the high bit into the proper placeholder

      if (verifyChecksumRS485(currentPosition))
        //we got back a good position, so just mask away the checkbits
        currentPosition &= 0x3FFF;

        //If the resolution is 12-bits, then shift position
        if (RESOLUTION == 12)
          currentPosition = currentPosition >> 2;

        enc_position_buffer[0] = currentPosition*360/16384.0; //just take direct reading

        if(abs(enc_position_buffer[0]-enc_position_buffer[1])>360-(5+2*abs(enc_position_buffer[1]-enc_position_buffer[2]))) //if 0 line crossed, allowing a buffer for vel and offset
        if(enc_position_buffer[3]>-1 && abs(enc_position_buffer[0]-enc_position_buffer[1])>5+spike_threshold*abs(enc_position_buffer[1]-enc_position_buffer[2]))
            enc_position_buffer[0] = enc_position_buffer[1] + (enc_position_buffer[1]-enc_position_buffer[2]); //linear extrapolation

          //try lag compensation by guessing two more runge-kutta points and then applying filtering?
          enc_velocity = SAVITZY_GOLAY_4[0]*enc_position_buffer[0]+SAVITZY_GOLAY_4[1]*enc_position_buffer[1]+SAVITZY_GOLAY_4[2]*enc_position_buffer[2]+SAVITZY_GOLAY_4[3]*enc_position_buffer[3];
        //Serial.print("Encoder #");
        //Serial.print(encoder, DEC);
        //Serial.print(" position: ");
        //Serial.println(currentPosition, DEC); //print the position in decimal format
        //Serial.print(" ");
        //Serial.print(" ");
        //Serial.print(enc_position_buffer[0]); //print position in degrees
        //Serial.print(" ");
        //Serial.print(20*(enc_position_buffer[0]-enc_position_buffer[1])); //simple velocity
        //Serial.print(" ");
        //Serial.println(20*enc_velocity); //print velocity (S-G filtered)
        //Serial.println(enc_position_buffer[0]); //PRINT POSITION IN DEGREES
        //Serial.println(100*((enc_position-enc_position_buffer[2]))); //approx velocity
        //Serial.print("Encoder #");
        //Serial.print(encoder, DEC);
        //Serial.println(" position error: Invalid checksum.");
      //Serial.print("Encoder #");
      //Serial.print(encoder, DEC);
      //Serial.print(" error reading position: Expected to receive 2 bytes. Actually received ");
      //Serial.print(bytes_received, DEC);
      //Serial.println(" bytes.");

    //wait briefly before reading the turns counter
    //flush the received serial buffer just in case anything extra got in there
    while (Serial1.available());
  return (((int)(5*enc_position_buffer[0]))%360)*_2PI/360.0; //multiple of 5 accounts for pulley ratio between motor and encoder

// sensor intialising function
void initMySensorCallback(){

// sensor instance
GenericSensor encoder1 = GenericSensor(readMySensorCallback, initMySensorCallback);

void setup()

  //Initialize the UART serial connection to the PC

  //Initialize the UART link to the RS485 transceiver


  // initialize sensor hardware
  // link the motor to the sensor

  // driver config
  // power supply voltage [V]
  driver.voltage_power_supply = 5;
  // limit the maximal dc voltage the driver can set
  // as a protection measure for the low-resistance motors
  // this value is fixed on startup
  driver.voltage_limit = 5;
  driver.pwm_frequency = 32000;
  // link the motor and the driver

  // limiting motor movements
  // limit the voltage to be set to the motor
  // start very low for high resistance motors
  // current = voltage / resistance, so try to be well under 1Amp
  motor.voltage_limit = 3;   // [V]

   // aligning voltage 
  motor.voltage_sensor_align = 5;
  // choose FOC modulation (optional)
  //motor.foc_modulation = FOCModulationType::SpaceVectorPWM;
  // set motion control loop to be used
  motor.torque_controller = TorqueControlType::voltage;
  motor.controller = MotionControlType::torque;

  // open loop control config
  //motor.controller = MotionControlType::velocity_openloop;
  //Serial.println("Motor control loop configured");

  // init motor hardware

  //Serial.println("FOC init complete");

  //Set the modes for the RS485 transceiver
  pinMode(RS485_T_RE_DE, OUTPUT);
  //pinMode(RS485_T_RE, OUTPUT);
  //pinMode(RS485_T_DE, OUTPUT);
  //pinMode(RS485_T_DI, OUTPUT);
  //pinMode(RS485_T_RO, OUTPUT);

void loop()
  start_cycle = rp2040.getCycleCount(); //gets cycle at beginning of loop

    //"dumb" setpoint control with OL velocity
    //target_velocity = 50.0*sin((enc_position_buffer[0]-320.0-90)*PI/180.0);
    //target_position = 260+60*sin(tick/200.0);
    //double error = target_position-enc_position_buffer[0];
    //target_velocity = -0.5*(error);
    //Serial.print(" ");
    //Serial.print(" ");
    //Serial.print(" ");

    //velocity matching test:

    //sin calibration
    //target_velocity = 1.0*sin(tick/1000.0);

    //Serial.print("Error :: ");
    //Serial.print("Angle :: ");
    //target_velocity = -0.2*(target_position-(enc_position_buffer[0]));
    // user communication --do I need this???;

    //Serial.println(readMySensorCallback()); //this works and reports angles from 0 to 2pi
    //DEBUG GENERIC SENSOR (encoder1)

    end_cycle = rp2040.getCycleCount();
      cycles_in_loop = 4,294,967,295-start_cycle+end_cycle; //accounts for wrapping
      cycles_in_loop = end_cycle - start_cycle;
    computation_time_us = 1000000*cycles_in_loop*1.0/(1.0*clock_frequency);
    //Serial.print("Time : ");

Hi @A_Daetz , welcome to SimpleFOC!

First off, your approach is valid and the way you’re using the GenericSensor class is why we provide it - although we try, we simply can’t support all the sensors out there ourselves.

Generally speaking, sensors should be as fast as possible, so an asynchronous RS485 based protocol is less ideal than say, SPI or sensors with ABZ interfaces. But in principle it should work as long as you can get loop times faster than 1kHz or so. It will just limit the system’s performance compared to a faster sensor.

Have you checked if you can achieve your desired loop time of 100us? It would be a good speed!

So why is it not working - good question! At first glance the code looks ok - is it possible something is configured differently in this sketch from the other, working sketches?

I am concerned a bit about all the angle manipulations…

The sensor seems to return a 14 bit value, so why not just store that, as a uint16_t instead of converting to a double value in degrees?
(Note also the code won’t work if RESOLUTION==12, the shift needs to be << not >>)
Or if you want to convert it, why not convert it to a float value, in radians, which is what SimpleFOC needs?

And then there is this:

return (((int)(5*enc_position_buffer[0]))%360)*_2PI/360.0; //multiple of 5 accounts for pulley ratio between motor and encoder

%360 is an integer modulo operation, you’re sacrificing accuracy here since 360 is less than the 14 bits you originally read in. You need to use fmod() to take a floating point modulo, but see my note above - by sticking to an integer representation up to this point (uint32_t in this case to allow for the 5x multiplication) you could use the fact the sensor resolution and hence the full rotation is a power of two to do the modulo as value & 0x3FFF, which is orders of magnitude faster.

But the real question is, why is there a pulley between motor and encoder? We generally don’t recommend that… in this case the gear-down implies that 5 motor rotations happen for 1 sensor rotation, and you have effectively reduced your resolution by a factor of 5 - its now a 11 bit sensor instead of a 14 bit one…
And any kind of pulley system introduces the possibility of backlash, or much worse, slippage. If it is a gear-based system, then the teeth should prevent slippage, but the backlash will introduce inaccuracies. For belt based systems, acceleration could cause slippage, which would introduce an angle offset error - which will eventually kill the FOC performance.
We recommend fixing the encoder/sensor rigidly and directly to the motor axis.

How confident are you of the Pole Pair number?

8 is not the most usual number, although certainly possible. 7, 11 and 15 PP are the most usual numbers for the outrunner type motors people usually use, depending on the motor size.

Open loop mode will work fine no matter what number you use for the PP.

However closed loop mode will totally fail unless the PP number is set correctly.

We have a test-sketch that uses open-loop mode to check the number of PP for you (its in our examples) but this does require the sensor to be working.

You can try commenting out the motor.move() and motor.loopFOC(), and replacing them with a sensor.update(), and then print the sensor.getAngle() value every 10 iterations. Then see if you get proper sensor values as you turn the motor by hand…

Hi @runger , thank you so much for the reply!
Yes, a 100us loop time is what I’ve been running at. The model of encoder I have transmits at 2Mbit/s which results in a measurement time of around 20us.
As far as sketch configuration, I’ve left the RS485 code and SimpleFOC code as untouched as possible, so hopefully I haven’t introduced any weirdness there. Testing last night seemed to indicate that encoder1.getAngle() works as desired and calls my RS485 code, but motor.shaftAngle() always returns 0.0 and the initFOC function fails. Do you have any idea why shaftAngle() isn’t calling my encoder code? That seems to be the “broken wire” so to speak.
I appreciate the deep dive on the encoder math as well! I agree that my hastily coded return is horribly inefficient and loses precision–I’m rewriting that to use the native 14 bit output tonight.
As far as the separation between the motor and encoder, the motor actuates the robot joint via a capstan drive where the cables are tied off, so slippage is impossible and backlash is extremely minimal. It’s a bit of a long story as to why I wanted the motor and encoder separated in the first place, but that’s not easy to change at this point in the design unfortunately. The only wrinkle is obtaining the precise pulley ratio (accounting for cable thickness), but that shouldn’t be terribly difficult.
Back to the “broken wire” of the shaftAngle() → getAngle() problem, is there a way to bypass the loopFOC() function and simply tell the motor directly what its shaft angle is without that function call being abstracted away? Or, relatedly, could I directly feed it a desired voltage and shaft angle together and call setPhaseVoltage from main?

Alas, the datasheet is not the most edifying overall, but it does say 8 poles on page 4.

The sensor.getAngle() does work as per my above post–that’s why I’m wondering if there’s maybe a way to bypass the faulty loopFOC and give the motor a shaft angle value directly?

Ok, that’s strange.

Normally its like this:

motor.loopFOC() calls sensor.update(), and then motor.electricalAngle() to get the current electrical angle. This calls sensor.getMechanicalAngle() to get the current angle value.

sensor.update() calls sensor.getSensorAngle(), which reads a new value from the hardware.
So if sensor.getAngle() is working then sensor.getMechanicalAngle() should be working too and so should motor.electricalAngle().

The sensor has a min_elapsed_time field, defaulting to 100us, which not relevant for torque-voltage mode but relevant if you want to use the velocity, so something to keep in mind.

The GenericSensor is really just a trivial wrapper allowing you to substitute the getSensorAngle() function with your callback, and thereby relieving you of the need to create a subclass of Sensor.

Maybe print the angle value once every 10 iterations and make sure it is really changing…

If its 8 poles that normally means 4 PP… although that would be a very weird number for an outrunner.

To clarify regarding the “broken link”:

encoder1.getSensorAngle() will call your code and read a fresh value.
encoder1.update() will do the above, store the value, and also track the full rotations and the timestamp for the velocity calculation.

encoder1.getAngle(), encoder1.getMechanicalAngle(), encoder1.getPreciseAngle() - these will all just return the last value set by encoder1.update(), no matter how often you call them. Calling these won’t call your RS485 code.

Gotcha, thanks for the clarification! Indeed, running encoder1.update() and encoder1.getAngle() results in correct values when I move the encoder/motor by hand. The issue is that when I run motor.loopFOC() and Serial.println(motor.shaftAngle()), I get all zeros. This is what I meant by the “broken link”–I don’t understand why I’m able to set up my encoder as a GenericSensor and get good data, yet the motor seems unable to link to it and get its position via shaftAngle().

All motor.shaftAngle() is doing is adding the sensor offset and the direction parameters to the sensor.getAngle(), as well as applying the low pass filter.

This is not stateless, as the filter keeps a timestamp each time it is called.

So you should not call motor.shaftAngle() yourself, it is called for you in motor.move(). Its value is stored in motor.shaft_angle, so you can read it from there if you need it.

Calling motor.shaftAngle() could result in weird values due to the very short timestamps resulting in consecutive calls.

The values from motor.shaft_angle / motor.shaftAngle() only make sense if the motor.sensor_direction was correctly set during initFOC(). If the motor.sensor_direction is UNKNOWN, then its value is zero and the shaft_angle will remain at zero. So this could be the problem in your case.

Gotcha. Probing with the oscilloscope, I can verify that loopFOC() is in fact correctly running the RS485 code and getting readings from the encoder once the code gets to the main loop. The issue seems to be in the initFOC() process–I don’t see anything on the scope while it’s initializing, and I think I should see two RS485 calls to get the shaft angle before and after movement, right? Why might initFOC() not be correctly querying the encoder position?

In fact, during initFOC() it will call sensor.update() 1006 times if you’re not using current sensing, if I counted that right.

And unlike your main loop, it will leave only a 2us delay between calls to sensor.update()… could it be a problem somehow?

Maybe this one:

//Set the modes for the RS485 transceiver
  pinMode(RS485_T_RE_DE, OUTPUT);

has to happen before the call to initFOC() ?

1 Like

Funny story–I just found this (profoundly silly) mistake, only to come back to this page and find that you spotted it a few minutes before me! IT FREAKING WORKS NOW!!! Thank you so much for your help and patience–I am excited beyond belief that this is actually working.

1 Like