Encoder reading using SPI port (Biss-C)

Hello Community,

I am working on a system that requires communication with a sensor that outputs data in BISS-C format. The communication between the microcontroller and the sensor is established through the SPI interface, and the sensor data is transmitted in 16-bit frames. I have previously run bit bang to get the data out but it is too slow and would like to use SPI but this bit shifting thing is driving me crazy, would appreciate if someone could help me :slight_smile:

SPI Reading

SPISettings settingsSPI(1000000, MSBFIRST, SPI_MODE1);

void setup()
{
  SPI.setSCLK(PC10);
  SPI.setMISO(PC11);
  SPI.setMOSI(PC12);
  SPI.begin();
}

byte buffer[4];

void readData_SPI(void)
{
  SPI.beginTransaction(settingsSPI);

  for (int i = 0; i < 3; i++)
  {
    buffer[i] = SPI.transfer(0x00); //8*3 = 24
  }

  SPI.endTransaction();
}

float calculateAngle(uint32_t singleTurn)
{
  return (singleTurn * 360UL) / 4096.0;
}

void loop()
{
  readData_SPI();

  // Process buffer to extract 24-bit data
  uint64_t position = ((unsigned long)buffer[1] << 16) | ((unsigned long)buffer[2] << 8) | buffer[3];
  float angle = calculateAngle(position & 0x0FFF); //angle is wrong probably that I bitshift wrong because Im stupid and cannot figure it out.
}

bit bang parsing (working angle)

float encoder_position = 0;

#define BIT_COUNT 16
#define GPIO_1_Pin GPIO_PIN_11
#define GPIO_1_GPIO_Port GPIOA
#define GPIO_2_Pin GPIO_PIN_12
#define GPIO_2_GPIO_Port GPIOA

unsigned long shiftIn(const int bit_count)
{
  unsigned long data = 0;

  for (int i=0; i<bit_count; i++)
  {
    data <<= 1;
    HAL_GPIO_WritePin(GPIO_1_GPIO_Port, GPIO_1_Pin, GPIO_PIN_RESET);
    delayMicroseconds(1);
    HAL_GPIO_WritePin(GPIO_1_GPIO_Port, GPIO_1_Pin, GPIO_PIN_SET);
    delayMicroseconds(1);

    data |= HAL_GPIO_ReadPin(GPIO_2_GPIO_Port, GPIO_2_Pin);
  }

  return data;
}

float read_position()
{
	unsigned long sample1 = shiftIn(BIT_COUNT);
	unsigned long sample2 = shiftIn(BIT_COUNT);
	delayMicroseconds(20);  // Clock mus be high for 20 microseconds before a new sample can be taken

	if (sample1 != sample2) return encoder_position;

	return ((sample1 & 0x0FFF) * 360UL) / 4096.0;
}

float readMySensorCallback()
{
  encoder_position = read_position();
  return (encoder_position * PI) / 180; //rads
}

Thank you in advance for your time!

I got it working on Teency 4.0 but it does not work on STM32F4 with SimpleFoc Generic Sensor. Anyone has a clue? I suppose it has to do with some timings, maybe it calls to fast? When I debug sensor update method to read angle it jumps randomly and motor is shaking.

TEENCY 4.0

#define CLOCK_SPEED                 1'000'000

const int miso_pin = 12;
const int clock_pin = 13;


void setup()
{
  pinMode(clock_pin,OUTPUT);
  pinMode(miso_pin,INPUT);
  
  SPI.begin(); // Initialize SPI
  
  Serial.begin(9600); // Initialize Serial communication at 9600 baud rate

  delay(1000);
}

uint8_t encoderBuf[4] = {0,0,0,0};

uint32_t decodeEncoderFrame()
{
      uint32_t data = static_cast<uint32_t>(encoderBuf[0] << 24) |
                      static_cast<uint32_t>(encoderBuf[1] << 16) |
                      static_cast<uint32_t>(encoderBuf[2] << 8)  |
                      static_cast<uint32_t>(encoderBuf[3]);           // here transfer8 was made
                   
    data = data >> 14;
    
    return data;
}

void calculateAndPrintPosition(uint32_t& encoderData)
{
  float angularAbsolutePosition = ((encoderData & 0x0FFF) * 360UL) / 4096.0;

  SerialUSB.println("Encoder Positon = " + String(encoderData) + " Angle = " + String(angularAbsolutePosition, 4));
}

void loop()
{
  // Start SPI transaction
  SPI.beginTransaction(SPISettings(CLOCK_SPEED, MSBFIRST, SPI_MODE0));

  // Send dummy bytes and read data
  for (int i = 0; i < 4; i++)
  {
    encoderBuf[i] = SPI.transfer(0xAA);
  }

  SPI.endTransaction();

  uint32_t encoderData = decodeEncoderFrame();
  calculateAndPrintPosition(encoderData);

  delay(100); // Delay for demonstration purposes
}

SIMPLE FOC

#define CLOCK_SPEED 1'000'000       // 1.0 MHz

SPISettings settingsSPI(CLOCK_SPEED, MSBFIRST, SPI_MODE0);

uint8_t encoderBuf[4] = {0,0,0,0};

uint32_t decodeEncoderFrame()
{
      uint32_t data = static_cast<uint32_t>(encoderBuf[0] << 24) |
                      static_cast<uint32_t>(encoderBuf[1] << 16) |
                      static_cast<uint32_t>(encoderBuf[2] << 8)  |
                      static_cast<uint32_t>(encoderBuf[3]);           // here transfer8 was made
                   
    data = data >> 14;
    
    return data;
}

float calculatePosition(uint32_t& encoderData)
{
  return ((encoderData & 0x0FFF) * 360UL) / 4096.0;
}

float readMySensorCallback()
{
  // Start SPI transaction
  SPI.beginTransaction(settingsSPI);

  // Send dummy bytes and read data (24)
  for (int i = 0; i < 4; i++)
  {
    encoderBuf[i] = SPI.transfer(0xAA);
  }

  SPI.endTransaction();

  uint32_t encoderData = decodeEncoderFrame();

  return (calculatePosition(encoderData) * PI) / 180; //rads
}

void initMySensorCallback()
{
    SPI.setSCLK(PC10);
    SPI.setMISO(PC11);
    SPI.setMOSI(PC12);
    SPI.begin();
}

Hey,

I think you’re approaching this too complicated…

You can do a 24bit transfer using SPI:

uint8_t buff[3] = { 0x00, 0x00, 0x00 };
SPI.beginTransaction(settingsSPI);
digitialWrite(PIN_nCS, LOW);
SPI.transfer(buff, 3)
digitialWrite(PIN_nCS, HIGH);
SPI.endTransaction();

uint16_t rawval16 = (buff[0]<<8) | buff[1];          // raw value, 16bit
float angle = (float)rawval16 / 0x10000 * _2PI;      // angle in radians
uint8_t crc = buff[2] &0x3F;                         // crc - checking it is a bit more complicated
bool err = ((buff[2]&0x80)==0);                      // error flag
bool warn = ((buff[2]&0x40)==0);                     // warning flag

Thanks runger! I probably do indeed, thanks for your info I’ll test it out :slight_smile:

I tested this and I get same value all the time, what can be wrong?

  uint8_t buff[3] = { 0x00, 0x00, 0x00 };
  SPI.beginTransaction(settings);
  digitalWrite(clock_pin, LOW);
  SPI.transfer(buff, 3);
  digitalWrite(clock_pin, HIGH);
  SPI.endTransaction();
  
  uint16_t rawval16 = (buff[0]<<8) | buff[1];          // raw value, 16bit
  float angle_ = (float)rawval16 / 0x10000 * _2PI;      // angle in radians
  uint8_t crc = buff[2] &0x3F;                         // crc - checking it is a bit more complicated
  bool err = ((buff[2]&0x80)==0);                      // error flag
  bool warn = ((buff[2]&0x40)==0);                     // warning flag

  Serial.print("angle: ");
  Serial.println(angle_);

angle: 5.78
angle: 5.86
angle: 5.86
angle: 5.78
angle: 5.78
angle: 5.78
angle: 5.78
angle: 5.78
angle: 5.86
angle: 5.78
angle: 5.78
angle: 5.78
angle: 5.86

It looks like its not the same angle, but jumping between two values, 5.86 and 5.78?

Are you turning the motor by hand to test the angle?

I don’t know about this BISS-C sensor, but are you using the right SPI mode and clock speed? how does the “settings” structure look?

Hi,

Yes the values jump little but wrong values and I turn motor by hand. I tried both MODE0 and MODE1 with same result.

#define CLOCK_SPEED 1’000’000 // 1.0 MHz

SPISettings settings = SPISettings(CLOCK_SPEED, MSBFIRST, SPI_MODE0);

I appreciate your help :slight_smile:

Result with 1.5 MHz

angle: 6.28 - rawval16: 65535 - err: 0 - warn: 0
angle: 6.28 - rawval16: 65535 - err: 0 - warn: 0
angle: 6.28 - rawval16: 65535 - err: 0 - warn: 0
angle: 6.28 - rawval16: 65535 - err: 0 - warn: 0
angle: 6.28 - rawval16: 65535 - err: 0 - warn: 0
angle: 6.28 - rawval16: 65535 - err: 0 - warn: 0
angle: 6.28 - rawval16: 65535 - err: 0 - warn: 0
angle: 6.28 - rawval16: 65535 - err: 0 - warn: 0
angle: 6.28 - rawval16: 65535 - err: 0 - warn: 1
angle: 6.28 - rawval16: 65535 - err: 0 - warn: 1
angle: 6.28 - rawval16: 65535 - err: 0 - warn: 0
angle: 6.28 - rawval16: 65535 - err: 0 - warn: 0
angle: 6.28 - rawval16: 65535 - err: 0 - warn: 0
angle: 6.28 - rawval16: 65535 - err: 0 - warn: 0
angle: 6.28 - rawval16: 65535 - err: 0 - warn: 0
angle: 6.28 - rawval16: 65535 - err: 0 - warn: 0
angle: 6.28 - rawval16: 65535 - err: 0 - warn: 0
angle: 6.28 - rawval16: 65535 - err: 0 - warn: 0
angle: 6.28 - rawval16: 65535 - err: 0 - warn: 0
angle: 6.28 - rawval16: 65535 - err: 0 - warn: 0
angle: 6.28 - rawval16: 65535 - err: 0 - warn: 0
angle: 6.28 - rawval16: 65535 - err: 0 - warn: 0

When I try use last two bytes the result is this, does it look fine as encoder values?

min: 0
max: 6.28

uint16_t rawval16 = (buff[1]<<8) | buff[2];

One “fast turn”

angle: 0.26 - rawval16: 2748
angle: 1.10 - rawval16: 11439
angle: 1.57 - rawval16: 16367
angle: 2.00 - rawval16: 20911
angle: 2.29 - rawval16: 23846
angle: 2.72 - rawval16: 28398
angle: 3.01 - rawval16: 31358
angle: 3.34 - rawval16: 34797
angle: 3.47 - rawval16: 36159
angle: 3.81 - rawval16: 39742
angle: 4.06 - rawval16: 42399
angle: 4.34 - rawval16: 45279
angle: 4.50 - rawval16: 46903
angle: 4.67 - rawval16: 48685
angle: 4.79 - rawval16: 49951
angle: 5.01 - rawval16: 52222
angle: 5.23 - rawval16: 54502
angle: 5.50 - rawval16: 57318
angle: 5.80 - rawval16: 60447
angle: 6.14 - rawval16: 64046

That looks good :slight_smile:

So the byte order is the opposite of what I expected - its very unclear from the datasheet screenshot…

So then probably:

  uint16_t rawval16 = (buff[1]<<8) | buff[2];          // raw value, 16bit
  float angle_ = (float)rawval16 / 0x10000 * _2PI;      // angle in radians
  uint8_t crc = buff[0] &0x3F;                         // crc - checking it is a bit more complicated
  bool err = ((buff[0]&0x80)==0);                      // error flag
  bool warn = ((buff[0]&0x40)==0);                     // warning flag

Thanks!

Seems to be the correct values now and the sensor check goes well, but when implementing the generic sensor the motor just shakes, motor runs fine during the sensor calibration.

But once it gets to the loop it start shake:

motor.loopFOC();
motor.move(1);

float readMySensorCallback()
{
  uint8_t buff[3] = { 0x00, 0x00, 0x00 };
  SPI.beginTransaction(settings);
  SPI.transfer(buff, 3);
  SPI.endTransaction();
  
  uint16_t rawval16 = (buff[1]<<8) | buff[2];          // raw value, 16bit
  float angle_ = (float)rawval16 / 0x10000 * _2PI;      // angle in radians
  return angle_;
}

Is there something i’m missing?

Which mode are you using?

First you should try torque-voltage mode, this is the simplest closed loop mode, and only needs a well-working sensor…

 // power supply voltage [V]
driver.pwm_frequency = 25000;
driver.voltage_power_supply = 24;
driver.voltage_limit = 3;
driver.init();

motor.linkDriver(&driver);
motor.controller = MotionControlType::velocity;
motor.PID_velocity.P = 0.2;
motor.PID_velocity.I = 20;
motor.PID_velocity.output_ramp = 1000;
motor.LPF_velocity.Tf = 0.01;
motor.voltage_limit = 1;

motor.init();
motor.initFOC();

Ok I can try torque mode and see.

Yes, the velocity mode depends on torque mode, and you need to tune the PID to match your motor. 20.0 is probably too high for the I term.

The voltage limit of 3/24 is not ideal: this is a low utilisation of the PWM range, all the PWM duty cycles will be very short. It would be better to use a 5V power supply if you only need 3V on the motor.

Which type of motor is it, what’s the winding resistance?

And how many pole pairs has the motor, did you configure it correctly? Its important to get the pole-pair number correct…

Hi,

The motor is in this topic (25A): Harmonic Drive - motors

Pole pairs is 6 and phase resistance 3.7

Ok, this seems ok. With 3.7Ω phase resistance you can probably go a little higher with the voltage… but 1V motor voltage limit should be enough to move the motor.

What about torque-voltage mode, is it working?

Yes it moves fine during initialization so voltage seems fine during testing.

I have tried torque mode as well with same results, not sure what I’m missing here. Encoder provide 0 - 6.28 values and PID ia set low.

Question; should one full rotation go from 0 - 6.28?

Yes, that’s 2PI radians, or one full turn…

How is the behaviour in torque mode? Is it “just shaking”?

Would you mind sharing the full code that you are testing at the moment?

Hi,

I’m new to motor driving but I it will probably be easier when all start to make sense, yes it’s still shake on same place. :slight_smile: I appreciate your help!

That’s interesting, encoder is mounted on motor shaft but when I turn motor by hand it goes from 0 to 6.28 quite fast not a full motor turn. Can it be that encoder is calibrated somehow with gearing? Motor gearing is 30x for output shaft, but encoder is not mounted on output shaft…

I dont have access to code right now but it’s same as for torque example but with voltage limit of 3.

I will make a test to see how many times it reaches 2PI radians when made one full motor turn.

This is a strange sensor, but then yes, it sounds like it is somehow calibrated for maybe the output shaft position? Have you checked if the 0-2PI corresponds to one full turn of the output shaft?
→ if yes, then you could divide the value by the gear-down, i.e. 1/30…

But the datasheet you pasted says “Absolute position / revolution (motor side)” - which I would understand to mean before the gear-down…

The SimpleFOC code absolutely cannot work if the sensor does not return 0-2PI corresponding to the rotor position.

Perhaps another explanation is that the byte order is still wrong: maybe we’re using the low byte as the high byte? Then you would seem to get many 0-2PI outputs per revolution…
Is it better if you do:
uint16_t rawval16 = (buff[2]<<8) | buff[1];
or does that make it even worse?

Hi,

uint16_t rawval16 = (buff[2]<<8) | buff[1];

This makes it worse if switch so it seems fine, I notice when I turn maybe 5-10 degree of motor shaft it goes from 0 - 2PI, and values looks fine each time.

If I divide the value by the gear-down it jumps only from 0 - 0.21

I made a test with some counter that check each full turn of motor and when it reach 2PI

angle_rad: 0.80 - count: 10

So it tool 10 times to rotate 1 full motor turn and each range from 0-2PI