Infineon Team
Published © MIT

Closed-Loop DC Motor Speed Control with XMC4700 and TLE5012B

Closed-loop PID speed control for a 12V DC motor using the XMC4700, TLE5012B magnetic angle sensor, and DC-SHIELD_BTN9970LV.

BeginnerProtip2 hours71
Closed-Loop DC Motor Speed Control with XMC4700 and TLE5012B

Things used in this project

Hardware components

Geared DC Motor, 12 V
Geared DC Motor, 12 V
×1
KIT XMC47 RELAX LITE V1
Infineon KIT XMC47 RELAX LITE V1
×1
TLE5012B E1000 MS2GO
Infineon TLE5012B E1000 MS2GO
×1
DC-SHIELD BTN9970LV
Infineon DC-SHIELD BTN9970LV
×1

Hand tools and fabrication machines

Soldering iron (generic)
Soldering iron (generic)

Story

Read more

Custom parts and enclosures

motor and sensor fixture

Code

receiver code

Arduino
/**
 * PID Speed Controller – XMC4700 Relax Kit
 *
 * Receives RPM measurements from the TLE5012B Angle Sensor 2GO
 * via UART and controls a 12V brushed DC motor using a
 * PID controller with feedforward compensation.
 *
 * Hardware connections:
 *   Sensor 2GO UART TX (P2.0 / Pin 6) -> XMC4700 RX (D0 / Pin 0)
 *   Sensor 2GO GND                    -> XMC4700 GND
 *   Motor lead A                      -> Shield OUT1
 *   Motor lead B                      -> Shield OUT2
 *   12V supply (+)                    -> Shield VS / VBAT
 *   12V supply (-)                    -> Shield GND
 *
 * If the motor spins in the wrong direction, swap OUT1 and OUT2,
 * or change the sign in processUART(): rpm_measured = +(rpm_i / 10.0f)
 */

#include <math.h>
#include "btn99x0_motor_control.hpp"
using namespace btn99x0;

// ── Motor Shield ──────────────────────────────────────────────
DCShield shield;
MotorControl motor(shield);

// ── Target Speed ──────────────────────────────────────────────
float rpm_setpoint = 600.0f;  // desired motor speed in RPM

// ── PID Gains ─────────────────────────────────────────────────
// These values are tuned for a 12V brushed DC motor (~1300 rpm no-load).
// See Step 5 of this article for tuning guidance.
float Kp = 0.2f;   // proportional: reacts to current error
float Ki = 0.02f;   // integral:     removes steady-state offset
float Kd = 0.00f;  // derivative:   dampens overshoot (applied to measurement)

// ── Feedforward Gain ──────────────────────────────────────────
// Provides a baseline drive signal so the PID only corrects small deviations.
// Approximate formula: Kff ≈ 255 / rpm_no_load
// Fine-tune until the I-term (Ierr) stays close to zero at steady state.
float Kff = 0.2194f;

// ── PID State Variables ───────────────────────────────────────
float pid_integral_err = 0.0f;
float pid_last_measured = 0.0f;
float pid_output = 0.0f;

// ── Output Limit ──────────────────────────────────────────────
const float OUTPUT_LIMIT = 255.0f;

// ── RPM Measurement ───────────────────────────────────────────
float rpm_measured = 0.0f;
float rpm_filtered = 0.0f;
bool rpm_init = false;

// ── Loop Timing ───────────────────────────────────────────────
uint32_t last_control_ms = 0;
uint32_t last_print_ms = 0;
const uint32_t CONTROL_INTERVAL_MS = 20;  // control loop: 50 Hz
const uint32_t PRINT_INTERVAL_MS = 100;   // serial output: 10 Hz

// ── CRC8 Checksum (must match sender) ────────────────────────
static uint8_t crc8(const uint8_t* data, uint8_t len) {
  uint8_t crc = 0xFF;
  for (uint8_t i = 0; i < len; i++) {
    crc ^= data[i];
    for (uint8_t b = 0; b < 8; b++)
      crc = (crc & 0x80) ? (uint8_t)((crc << 1) ^ 0x07)
                         : (uint8_t)(crc << 1);
  }
  return crc;
}

// ── Clamp Helper ──────────────────────────────────────────────
float clamp(float val, float minVal, float maxVal) {
  if (val < minVal) return minVal;
  if (val > maxVal) return maxVal;
  return val;
}

// ── PID Controller with Feedforward ──────────────────────────
float pid_calculate(float setpoint, float measured, float dt_s) {
  float error = setpoint - measured;

  // Integral term: accumulates error over time to remove steady-state offset
  // Clamped to prevent integrator windup when motor is at its limit
  pid_integral_err += error * dt_s;
  pid_integral_err = clamp(pid_integral_err, -500.0f, 500.0f);

  // Feedforward: estimates the drive signal needed for the target speed
  // This reduces the work the PID has to do and speeds up the response
  float u_ff = Kff * setpoint;

  // Derivative on measurement: dampens oscillations without reacting
  // to setpoint changes (avoids "derivative kick")
  float d = 0.0f;
  if (dt_s > 0.0f) {
    float d_meas = (measured - pid_last_measured) / dt_s;
    d = -Kd * d_meas;  // negative: rising speed reduces output
  }
  pid_last_measured = measured;

  // Total output: feedforward baseline + PID correction
  float u = u_ff + Kp * error + Ki * pid_integral_err + d;

  // Clamp to valid PWM range (forward direction only)
  return clamp(u, 0.0f, OUTPUT_LIMIT);
}

// ── UART Frame Receiver ───────────────────────────────────────
// Parses the binary frame sent by the Angle Sensor 2GO.
// Uses a state machine to synchronize on the two start bytes (0xAA, 0xBB).
void processUART() {
  static uint8_t buf[8];
  static uint8_t idx = 0;
  static uint8_t state = 0;

  while (Serial1.available()) {
    uint8_t b = (uint8_t)Serial1.read();

    switch (state) {
      case 0:  // wait for first start byte
        if (b == 0xAA) state = 1;
        break;
      case 1:  // wait for second start byte
        if (b == 0xBB) {
          idx = 0;
          state = 2;
        } else if (b == 0xAA) {
          state = 1;
        }                    // repeated 0xAA: stay in sync
        else { state = 0; }  // unexpected byte: reset
        break;
      case 2:  // collect payload bytes
        buf[idx++] = b;
        if (idx == 6) {
          state = 0;
          if (crc8(buf, 5) != buf[5]) break;  // discard frame on CRC error
          int16_t rpm_i = (int16_t)((uint16_t(buf[2]) << 8) | buf[3]);
          rpm_measured = -(rpm_i / 10.0f);  // sign: adjust if motor runs backwards
        }
        break;
    }
  }
}

void setup() {
  Serial.begin(115200);   // USB: serial monitor output
  Serial1.begin(115200);  // UART: receive data from Angle Sensor 2GO
  motor.begin();

  Serial.println("=== PID Speed Controller ===");
  Serial.print("Setpoint: ");
  Serial.print(rpm_setpoint, 0);
  Serial.println(" rpm");
  Serial.println("Setpoint | RPM | Error | Output | I-term");

  last_control_ms = millis();
  last_print_ms = millis();
}

void loop() {
  uint32_t now = millis();

  // 1) Receive and decode UART data from the sensor board
  processUART();

  // 2) Low-pass filter to smooth measurement noise
  //    Initialize with the first valid sample to avoid startup transient
  if (!rpm_init) {
    rpm_filtered = rpm_measured;
    rpm_init = true;
  } else {
    rpm_filtered = 0.8f * rpm_filtered + 0.2f * rpm_measured;
  }

  // 3) PID control loop running at 50 Hz
  if (now - last_control_ms >= CONTROL_INTERVAL_MS) {
    float dt_s = (now - last_control_ms) / 1000.0f;
    last_control_ms = now;

    pid_output = pid_calculate(rpm_setpoint, rpm_filtered, dt_s);
    motor.set_speed((int)lroundf(pid_output));
  }

  // 4) Serial monitor output at 10 Hz
  if (now - last_print_ms >= PRINT_INTERVAL_MS) {
    last_print_ms = now;

    float motor_value = (pid_output / OUTPUT_LIMIT) * 12.0f * 100;

    Serial.print("RPM:");
    Serial.print(rpm_filtered, 1);
    Serial.print(" Drive Signal:");
    Serial.print(motor_value, 2);
    Serial.print(" Setpoint:");
    Serial.println(rpm_setpoint, 0);
  }
}

TLx5012B sender code

Arduino
/**
 * Sender Sketch – TLE5012B Angle Sensor 2GO (XMC1100)
 *
 * Reads the shaft angle from the TLE5012B magnetic angle sensor,
 * computes the motor speed (RPM) from the angle change over time,
 * and streams the result to the XMC4700 via UART at ~100 Hz.
 *
 * UART frame format (8 bytes total):
 *   [0xAA][0xBB][angle_hi][angle_lo][rpm_hi][rpm_lo][status][crc8]
 *
 * Important: Set Tools -> Serial Output Selection -> Onboard
 * so that Serial routes to the UART pins (P2.0 TX / P2.6 RX)
 * instead of the USB debugger.
 */

#include <tlx5012-arduino.hpp>
using namespace tle5012;

Tle5012Ino sensor;
errorTypes  err = NO_ERROR;

// ── CRC8 checksum (polynomial 0x07) ──────────────────────────
static uint8_t crc8(const uint8_t* data, uint8_t len) {
  uint8_t crc = 0xFF;
  for (uint8_t i = 0; i < len; i++) {
    crc ^= data[i];
    for (uint8_t b = 0; b < 8; b++)
      crc = (crc & 0x80) ? (uint8_t)((crc << 1) ^ 0x07)
                         : (uint8_t)(crc << 1);
  }
  return crc;
}

// ── Handle 0°/360° rollover when computing angle difference ──
double unwrapDelta(double newAngle, double oldAngle) {
  double d = newAngle - oldAngle;
  if (d >  180.0) d -= 360.0;
  if (d < -180.0) d += 360.0;
  return d;
}

void setup() {
  // UART output to XMC4700
  // Make sure Tools -> Serial Output Selection is set to "Onboard"
  Serial.begin(115200);
  delay(200);
  err = sensor.begin();
}

void loop() {
  static bool     first      = true;
  static double   last_angle = 0.0;
  static uint32_t last_us    = 0;
  static float    rpm_filt   = 0.0f;
  static bool     filt_init  = false;

  // 1) Read sensor block with CRC verification
  err = sensor.readBlockCRC();
  if (err != NO_ERROR) {
    delay(5);
    return;
  }

  // 2) Read absolute shaft angle in degrees (0° to 360°)
  double angle_deg = 0.0;
  sensor.getAngleValue(angle_deg);

  // 3) Compute time delta using micros() for accurate RPM calculation
  uint32_t now_us = micros();
  if (first) {
    // First sample: just initialize, do not compute RPM yet
    first      = false;
    last_angle = angle_deg;
    last_us    = now_us;
    delay(10);
    return;
  }

  float dt = (now_us - last_us) * 1e-6f;  // convert to seconds
  last_us  = now_us;
  if (dt <= 0.0f) dt = 1e-3f;             // safety guard against division by zero

  // 4) Compute RPM from angle delta
  //    dAngle in degrees -> convert to full revolutions -> scale to per minute
  double dAngle  = unwrapDelta(angle_deg, last_angle);
  last_angle     = angle_deg;
  float rpm_raw  = (float)(dAngle / dt / 360.0 * 60.0);

  // 5) Low-pass filter to smooth out measurement noise
  //    Initialize filter with first valid sample to avoid startup transient
  if (!filt_init) {
    rpm_filt  = rpm_raw;
    filt_init = true;
  } else {
    rpm_filt = 0.8f * rpm_filt + 0.2f * rpm_raw;
  }

  // 6) Build UART frame and transmit
  uint16_t angle_u = (uint16_t)(angle_deg * 65535.0 / 360.0);
  int16_t  rpm_i   = (int16_t)(rpm_filt * 10.0f);  // 0.1 rpm resolution

  // Status byte: rotation direction
  uint8_t status = 0;
  if (rpm_filt >  0.5f) status = 0x01;  // clockwise
  if (rpm_filt < -0.5f) status = 0x02;  // counter-clockwise

  // Payload: angle (2 bytes) + RPM (2 bytes) + status (1 byte) + CRC (1 byte)
  uint8_t payload[6];
  payload[0] = (angle_u >> 8) & 0xFF;
  payload[1] =  angle_u       & 0xFF;
  payload[2] = (rpm_i   >> 8) & 0xFF;
  payload[3] =  rpm_i         & 0xFF;
  payload[4] = status;
  payload[5] = crc8(payload, 5);

  // Send frame: start bytes (0xAA, 0xBB) + payload
  Serial.write((uint8_t)0xAA);
  Serial.write((uint8_t)0xBB);
  Serial.write(payload, 6);

  delay(10);  // send rate: ~100 Hz
}

Credits

Infineon Team
143 projects • 429 followers
Hands-on projects, tutorials, and code for sensors, MCUs, connectivity, security, power, and IoT. Follow for new builds and ideas.

Comments

Related channels and tags
  • Sensors
  • Sensors