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;
break;
case RS485_T_RX: //low
*(SIO + 0x018 / 4) = 1ul << RS485_T_RE_DE;
break;
default:
break;
}
//Serial.println(*(SIO + 0x010 / 4) >> RS485_T_RE_DE);
//Serial.println(digitalRead(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!)
//Serial.flush();
//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(Serial1.read()); //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 = Serial1.read(); //low byte comes first
currentPosition |= Serial1.read() << 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[3]=enc_position_buffer[2];
enc_position_buffer[2]=enc_position_buffer[1];
enc_position_buffer[1]=enc_position_buffer[0];
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[0]<180)
{
enc_position_buffer[1]-=360;
enc_position_buffer[2]-=360;
enc_position_buffer[3]-=360;
}else{
enc_position_buffer[1]+=360;
enc_position_buffer[2]+=360;
enc_position_buffer[3]+=360;
}
}
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
}
if(enc_position_buffer[3]>-1)
{
//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(angle_min);
//Serial.print(" ");
//Serial.print(angle_max);
//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
}
else
{
//Serial.print("Encoder #");
//Serial.print(encoder, DEC);
//Serial.println(" position error: Invalid checksum.");
}
}
else
{
//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
//delayMicroseconds(100);
//flush the received serial buffer just in case anything extra got in there
while (Serial1.available()) Serial1.read();
}
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(){
//nothing
}
// sensor instance
GenericSensor encoder1 = GenericSensor(readMySensorCallback, initMySensorCallback);
void setup()
{
//Initialize the UART serial connection to the PC
Serial.begin(BAUDRATE);
//Initialize the UART link to the RS485 transceiver
Serial1.begin(RS485_BAUDRATE);
delay(1000);
SimpleFOCDebug::enable();
// initialize sensor hardware
encoder1.init();
// link the motor to the sensor
motor.linkSensor(&encoder1);
// 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;
driver.init();
// link the motor and the driver
motor.linkDriver(&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
motor.init();
motor.initFOC();
//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()
{
tick++;
if(tick>2*PI*1000)
tick=0;
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(angle_min);
//Serial.print(" ");
//Serial.print(angle_max);
//Serial.print(" ");
//Serial.print(target_position);
//Serial.print(" ");
//Serial.println(enc_position_buffer[0]);
//velocity matching test:
//target_velocity=12*enc_velocity;
//sin calibration
//target_velocity = 1.0*sin(tick/1000.0);
//Serial.print("Error :: ");
//Serial.println(target_position-(enc_position_buffer[0]));
//Serial.print("Angle :: ");
//Serial.println(readMySensorCallback());
//motor.move(2*(target_position-(enc_position_buffer[0])));
//target_velocity = -0.2*(target_position-(enc_position_buffer[0]));
//motor.move(target_velocity);
//SIMPLEFOC CODE
motor.loopFOC();
//Serial.println(motor.shaftAngle());
motor.move(1);
// user communication --do I need this???
command.run();
//END SIMPLEFOC CODE
//Serial.println(readMySensorCallback()); //this works and reports angles from 0 to 2pi
//DEBUG GENERIC SENSOR (encoder1)
//encoder1.update();
//Serial.print(encoder1.getAngle());
//Serial.print("\t");
//Serial.println(encoder1.getVelocity());
end_cycle = rp2040.getCycleCount();
if(end_cycle<start_cycle)
{
cycles_in_loop = 4,294,967,295-start_cycle+end_cycle; //accounts for wrapping
}else
{
cycles_in_loop = end_cycle - start_cycle;
}
computation_time_us = 1000000*cycles_in_loop*1.0/(1.0*clock_frequency);
//Serial.println(cycles_in_loop);
//Serial.print("Time : ");
//Serial.println(computation_time_us);
delayMicroseconds(delta_t_us-computation_time_us);
}