Motor only operates at 3 distinct speeds with TorqueControlType::foc_current

Hi guys, I have been following the getting started guide and am now on the last step which is this:

I have changed the motor.torque_controller variable to TorqueControlType::foc_current and set the motor.controller variable to MotionControlType::torque

I have a ~20R phase winding resistance and 24V supply.

I am finding that my motor only seems to have 3 discrete speeds. If I type MTX where 0 <= X < 1.0 it goes one fixed rotational speed, then if 1.0 <= X < 2.0 it goes faster at a single rotation speed, and finally for X >= 2.0 it goes fastest.

There is no smooth increase in speed as I would expect. So what is going wrong?

My code is simply as below, very similar to the example:

#include <SimpleFOC.h>

// BLDC motor & driver instance
BLDCMotor motor = BLDCMotor(7);
BLDCDriver3PWM driver = BLDCDriver3PWM(4, 5, 6, 7);

// magnetic sensor instance - SPI
MagneticSensorSPI sensor = MagneticSensorSPI(19, 14, 0x3FFF);
SPIClass SPI_AS5047P;

// inline current sensor instance
// ACS712-05B has the resolution of 0.185mV per Amp
InlineCurrentSense current_sense = InlineCurrentSense(185.0f, 1, 2, _NC);

float target_position = 0.0;

// commander communication instance
Commander command = Commander(Serial);
void doMotor(char* cmd) {
  command.motor(&motor, cmd);
}

void setup() {

  // use monitoring with serial
  Serial.begin(115200);
  // enable more verbose output for debugging
  // comment out if not needed
  SimpleFOCDebug::enable(&Serial);

  // Wait for something to be typed at the other end of the serial connection (there is no other way to detect a serial port ready to go).
  while (!Serial.available()) yield();

  // void begin(int8_t sck = -1, int8_t miso = -1, int8_t mosi = -1, int8_t ss = -1);
  SPI_AS5047P.begin(47, 20, 21, 19);

  // spi mode (phase/polarity of read/writes) i.e one of SPI_MODE0 | SPI_MODE1 (default) | SPI_MODE2 | SPI_MODE3
  sensor.spi_mode = SPI_MODE1;
  // speed of the SPI clock signal - default 1MHz - AS5047P can do up to 10MHz
  sensor.clock_speed = 1000000;

  // initialise magnetic sensor hardware
  sensor.init(&SPI_AS5047P);

  // link the motor to the sensor
  motor.linkSensor(&sensor);

  // driver config
  // power supply voltage [V]
  driver.voltage_power_supply = 24;
  // 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 = 12;
  if (!driver.init()) {
    Serial.println("Driver init failed!");
    return;
  }

  // link driver
  motor.linkDriver(&driver);

  // link driver to cs
  current_sense.linkDriver(&driver);

  // current sense init
  if (!current_sense.init()) {
    Serial.println("Current sense init failed!");
    return;
  }
  // link the current sense to the motor
  motor.linkCurrentSense(&current_sense);

  // set control loop type to be used
  motor.torque_controller = TorqueControlType::foc_current;
  motor.controller = MotionControlType::torque;

  // use monitoring with serial
  // comment out if not needed
  motor.useMonitoring(Serial);
  motor.monitor_downsample = 1000;                      // set downsampling can be even more > 100
  motor.monitor_variables = _MON_CURR_Q | _MON_CURR_D;  // set monitoring of d and q currents

  // initialise motor
  if (!motor.init()) {
    Serial.println("Motor init failed!");
    return;
  }

  // align encoder and start FOC
  if (!motor.initFOC()) {
    Serial.println("FOC init failed!");
    return;
  }

  // set the inital target value
  motor.target = 1.0;

  // subscribe motor to the commander
  command.add('M', doMotor, "Motor");

  // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com)
  Serial.println("Motor ready.");
  Serial.println(F("Set the target using serial terminal and command M:"));

  _delay(1000);
}


void loop() {
  // iterative setting FOC phase voltage
  motor.loopFOC();

  // iterative function setting the outer loop target
  motor.move();

  // display the currents
  motor.monitor();

  // user communication
  command.run();
}

Thanks for looking.