Problem with foc_moduation and torque_controller


I am currently trying to implement current sensing into my closed loop angle control project.
The current sensing part itself is working I think but as soon as I am trying to either change the foc_modulation or the torque_controller the system does not work when I enter a target angle via Serial.

Here is the code that I am using.

Code for adaptive Steering Wheel EMC
-Get string from Serialport with desired angle command of BLDC inside the steering wheel / Format: “”
-closed loop control of BLDC with SimpleFOC library
-current sensing not implemented yet
#define B_G431
#include <SimpleFOC.h>
for SimpleFOC command
char* testangle;
float maxv;

// BLDC motor & driver instance
BLDCMotor motor = BLDCMotor(4,0.35,601);
LowsideCurrentSense currentSense = LowsideCurrentSense(0.003f, -64.0f/7.0f, A_OP1_OUT, A_OP2_OUT, A_OP3_OUT);
// encoder instance
Encoder encoder = Encoder(PB6, PB7, 2048);
// Interrupt routine intialisation
// channel A and B callbacks
void doA(){encoder.handleA();}
void doB(){encoder.handleB();}
// angle set point variable
float target_angle = 0;
// instantiate the commander
Commander command = Commander();
void doTarget(char* cmd) { command.scalar(&target_angle, cmd); }

//variables for Serial Communication(get Angle command as string)
const byte numChars = 32;
char receivedChars[numChars];
boolean newData = false;
char* cangle; //Angle of clockspring

void setup() {

// initialize encoder sensor hardware
encoder.enableInterrupts(doA, doB);
// link the motor to the sensor
// driver config
// power supply voltage [V]
driver.voltage_power_supply = 12;
// link the motor and the driver

//Here is the problem
motor.foc_modulation = FOCModulationType::SinePWM;
motor.torque_controller = TorqueControlType::foc_current;

// aligning voltage [V]
motor.voltage_sensor_align = 3;
// set motion control loop to be used
motor.controller = MotionControlType::angle;

// contoller configuration
// default parameters in defaults.h

// velocity PI controller parameters
motor.PID_velocity.P = 0.2f;
motor.PID_velocity.I = 20;
motor.PID_velocity.D = 0;
// default voltage_power_supply
//motor.voltage_limit = 11;
motor.current_limit = 6;
// jerk control using voltage voltage ramp
// default value is 300 volts per sec ~ 0.3V per millisecond
motor.PID_velocity.output_ramp = 1000;

// velocity low pass filtering time constant
motor.LPF_velocity.Tf = 0.01f;

// angle P controller
motor.P_angle.P = 20;
//maximal velocity of the position control
motor.velocity_limit = 500;

// use monitoring with serial
// comment out if not needed
// motor.useMonitoring(Serial);

// initialize motor

if (currentSense.init()) Serial.println(“Current sense init success!”);
Serial.println(“Current sense init failed!”);
//currentSense.skip_align = true;
// align encoder and start FOC

// add target command T
command.add(‘T’, doTarget, “target angle”);

//Serial.println(F(“Motor ready.”));
//Serial.println(F(“Set the target angle using serial terminal:”));

maxv = 0;

void loop() {
// main FOC algorithm function
// the faster you run this function the better
// Arduino UNO loop ~1kHz
// Bluepill loop ~10kHz

// Motion control function
// velocity, position or voltage (defined in motor.controller)
// this function can be run at much lower frequency than loopFOC() function
// You can also use motor.move() and set the in the code

// function intended to be used with serial plotter to monitor motor variables
// significantly slowing the execution down!!!
// motor.monitor();

// user communication
//Recieve angle over Serial
cangle = receivedChars;

maxv = abs(encoder.getVelocity());


//function for recieving string
void recvWithStartEndMarkers() {
static boolean recvInProgress = false;
static byte ndx = 0;
char startMarker = ‘<’;
char endMarker = ‘>’;
char rc;

while (Serial.available() > 0 && newData == false) {
    rc =;

    if (recvInProgress == true) {
        if (rc != endMarker) {
            receivedChars[ndx] = rc;
            if (ndx >= numChars) {
                ndx = numChars - 1;
        else {
            receivedChars[ndx] = '\0'; // terminate the string
            recvInProgress = false;
            ndx = 0;
            newData = true;

    else if (rc == startMarker) {
        recvInProgress = true;

//complimentary function for void recvWithStartEndMarkers()
void showNewData() {
if (newData == true) {
//Serial.print("This just in … ");
newData = false;



Am I missing some configurations or is the code in the wrong place ?
I already tried to look into some threads in the community but I can’t seem to find where my error is comming from.

Hi @karlleit ,

Sorry to hear you’re having trouble! Have you managed to sort it out?

Its a bit hard from this description what’s going on…

You should be able to change the modulation type without problems, valid types are SinePWM or SpaceVectorPWM. Don’t try to use the trapezoidal modes when using current sensing.

Changing the torque control mode also requires changing other parameters of the system - at the moment your code uses foc_current - is this the mode that is working? If you change from foc_current to voltage, for example, then it will no longer use the current PIDs to set the voltage… so the limits may need to be adjusted…

In general, we recommend to start with torque-voltage mode and once this is working well go to the more complicated modes involving the PIDs.