DC Motor position control does not work below 0 angle

developement
,
1

Hi,

I am trying to position control 8 N20 DC motors with encoders using SimpleDCMotor library using STM32F722ZE.

I am using Generic sensor implementation to calculate angle. The position control works for positive angles only. i.e :

  1. If I ask the motor to go to 5rad with commander input as a5 it works.
  2. Then inputing a2 also works, moving the motor back
  3. If I then input a-2, then motor keeps rotating forever once it goes below 0rad.

You can see the serial monitor screenshots below. Upon noticing, what I see is, both encoder value, angle value are correct but the actual position reading(4th value) from Monitor does not go below 0rad. If i manually rotate the motor as well, the angle reading is fine but monitor reading is not.

  1. Moving to a5 (5 rad, motor1) stops at 5 rad

    image
    image297×168 9.18 KB

  2. Moving to a2, stops at 2.

    image
    image297×168 9.18 KB

  3. Moving to a-2 , does not stop at all

    image
    image371×383 16.5 KB

I remember, this code worked when I tested in Esp32 few months back (not sure of library versions).
Current Library versions installed are latest:

SimpleFoc: 2.3.4
SimpleDCMotor:1.0.3
SimpleFOCDrivers: 1.0.8

I am not good at coding embedded systems, please see if any mistakes in my Code below:

#include <Arduino.h>
#include "SimpleFOC.h"
#include "SimpleFOCDrivers.h"
#include "SimpleDCMotor.h"
#include <vector>
#include <sstream>
#include <iterator>

using namespace std;
const int numMotors = 1;
const int numMotorsToMove = 1;

struct MotorInfo {
  int encoderPinE1;
  int encoderPinE2;
  int driverPin1;
  int driverPin2;
  DCDriver2PWM driver;
  volatile int lastEncoded;
  volatile long encoderValue;
  volatile float angle;
  DCMotor motor;
  GenericSensor sensor;

   MotorInfo() 
    : encoderPinE1(0), encoderPinE2(0), driverPin1(0), driverPin2(0), driver(DCDriver2PWM(0, 0)), lastEncoded(0), encoderValue(0), angle(0.0f), motor(), sensor([](){ return 0.0f; }) {}

};

MotorInfo motors[numMotors];
Commander commander = Commander(Serial);


#define DEFINE_SENSOR_CALLBACK(idx) \
float readSensor##idx() { return motors[idx].angle; }

#define DEFINE_COMMANDER_CALLBACK(idx) \
void onMotor##idx(char* cmd) { commander.motor(&motors[idx].motor, cmd); }

#define DEFINE_UPDATE_ENCODER_CALLBACK(idx) \
void updateEncoder##idx() { updateEncoder(idx); }



DEFINE_SENSOR_CALLBACK(0)
DEFINE_SENSOR_CALLBACK(1)
DEFINE_SENSOR_CALLBACK(2)
DEFINE_SENSOR_CALLBACK(3)
DEFINE_SENSOR_CALLBACK(4)
DEFINE_SENSOR_CALLBACK(5)
DEFINE_SENSOR_CALLBACK(6)
DEFINE_SENSOR_CALLBACK(7)

DEFINE_COMMANDER_CALLBACK(0)
DEFINE_COMMANDER_CALLBACK(1)
DEFINE_COMMANDER_CALLBACK(2)
DEFINE_COMMANDER_CALLBACK(3)
DEFINE_COMMANDER_CALLBACK(4)
DEFINE_COMMANDER_CALLBACK(5)
DEFINE_COMMANDER_CALLBACK(6)
DEFINE_COMMANDER_CALLBACK(7)

DEFINE_UPDATE_ENCODER_CALLBACK(0)
DEFINE_UPDATE_ENCODER_CALLBACK(1)
DEFINE_UPDATE_ENCODER_CALLBACK(2)
DEFINE_UPDATE_ENCODER_CALLBACK(3)
DEFINE_UPDATE_ENCODER_CALLBACK(4)
DEFINE_UPDATE_ENCODER_CALLBACK(5)
DEFINE_UPDATE_ENCODER_CALLBACK(6)
DEFINE_UPDATE_ENCODER_CALLBACK(7)




typedef float (*FnPtr)();
const FnPtr sensorCallBackArrays[]{
  readSensor0,readSensor1,readSensor2,readSensor3,
  readSensor4,readSensor5,readSensor6,readSensor7
};

typedef void (*CommanderFnPtr)(char* cmd);
const CommanderFnPtr commanderFunctionArrays[]{
  onMotor0, onMotor1, onMotor2, onMotor3,
  onMotor4, onMotor5, onMotor6, onMotor7
};

typedef void (*InterruptFnPtr)();
const InterruptFnPtr interruptFunctionArrays[]{
  updateEncoder0, updateEncoder1,updateEncoder2,updateEncoder3,
  updateEncoder4, updateEncoder5,updateEncoder6,updateEncoder7
};

// Define encoder and motor pins for each motor
const int encoderPins[][8] = {
  {PC2, PC1}, // PF15, PE13
  {PC0, PC3}, //
  {PD4, PD5},
  {PD6, PD7},
  {PG8, PG9},
  {PG10,PG11},
  {PG12, PG13},
  {PG14, PG15}
};

const int driverPins[][8] = {

  {PA9,PA8},      // TIM1-CH1, TIM1-CH2 
  {PA11,PA10},    // TIM1-CH3, TIM1-CH4 

  {PA5,PB3},      // TIM2-CH1, TIM2-CH2
  {PB11,PB10},    // TIM2-CH3, TIM2-CH4

  {PA7,PA6},      // TIM3-CH1, TIM3-CH2
  {PC8,PC9},      // TIM3-CH3, TIM3-CH4

  {PD12,PD13},    // TIM4-CH1, TIM4-CH2
  {PD14,PD15},    // TIM4-CH3, TIM4-CH4
  
  // {PA0,PA1},      // TIM5-CH1, TIM5-CH2
  // {PA2,PA3},      // TIM5-CH3, TIM5-CH4


  // {PC6,PC7},      // TIM8-CH1, TIM8-CH2
  // {PC8,PC9},      // TIM8-CH3, TIM8-CH4

  // {PE5,PE6},      // TIM9-CH1, TIM9-CH2

  // {PB14,PB15},    // TIM12-CH1, TIM12-CH2

};


void updateEncoder(int motorIndex) {
  int MSB = digitalRead(motors[motorIndex].encoderPinE1);
  int LSB = digitalRead(motors[motorIndex].encoderPinE2);

  int encoded = (MSB << 1) | LSB;
  int sum = (motors[motorIndex].lastEncoded << 2) | encoded;

  if (sum == 0b1101 || sum == 0b0100 || sum == 0b0010 || sum == 0b1011) motors[motorIndex].encoderValue--;
  if (sum == 0b1110 || sum == 0b0111 || sum == 0b0001 || sum == 0b1000) motors[motorIndex].encoderValue++;

  motors[motorIndex].lastEncoded = encoded;
  motors[motorIndex].angle = (motors[motorIndex].encoderValue * 6.28319) /9048.0f;
}

void initMotor(int motorIndex, float P, float I, float D) {
  motors[motorIndex].driver = DCDriver2PWM(motors[motorIndex].driverPin1, motors[motorIndex].driverPin2);
  motors[motorIndex].driver.voltage_power_supply = 12.0f;
  motors[motorIndex].driver.voltage_limit = 12.0f;
  motors[motorIndex].driver.pwm_frequency = 100;
  motors[motorIndex].driver.init();

  motors[motorIndex].sensor = GenericSensor(sensorCallBackArrays[motorIndex]);
  motors[motorIndex].sensor.init();
  motors[motorIndex].motor.linkDriver(&(motors[motorIndex].driver));
  motors[motorIndex].motor.linkSensor(&(motors[motorIndex].sensor));

  motors[motorIndex].motor.voltage_limit = 12.0f;
  // motors[motorIndex].motor.velocity_limit = 20.0f;
  // if(motorIndex!=2)
  motors[motorIndex].motor.velocity_limit = 6.0f;
  motors[motorIndex].motor.controller = MotionControlType::angle;
  motors[motorIndex].motor.torque_controller = TorqueControlType::voltage;
  motors[motorIndex].motor.init();

  motors[motorIndex].motor.PID_velocity.P = P;
  motors[motorIndex].motor.PID_velocity.I = I;
  motors[motorIndex].motor.PID_velocity.D = D;
  motors[motorIndex].motor.PID_velocity.output_ramp = 400.0f;
  motors[motorIndex].motor.LPF_velocity.Tf = 0.001f;

  motors[motorIndex].motor.P_angle.P = 22.5f;
  motors[motorIndex].motor.enable();
  motors[motorIndex].motor.sensor_direction = Direction::CW;
  motors[motorIndex].motor.useMonitoring(Serial);
  //motors[motorIndex].motor.monitor_downsample = 500;
  motors[motorIndex].motor.monitor_start_char = 'a'+motorIndex;
}


void setup() {
  while(!Serial);
  Serial.begin(115200);

  Serial.print("Max Timers:");
  Serial.println(SIMPLEFOC_STM32_MAX_PINTIMERSUSED);
  
  for (int i = 0; i < numMotors; i++) {
    // Serial.println("motor init");
    motors[i].encoderPinE1 = encoderPins[i][0];
    motors[i].encoderPinE2 = encoderPins[i][1];
    motors[i].driverPin1 = driverPins[i][0];
    motors[i].driverPin2 = driverPins[i][1];

    motors[i].lastEncoded = 0;
    motors[i].encoderValue = 0;
    // motors[i].angle = 0.0f;
    // motors[i].motor.target=0.0f;                                                                                                      
    // Serial.println("motor set");
    pinMode(motors[i].encoderPinE1, INPUT_PULLUP);
    pinMode(motors[i].encoderPinE2, INPUT_PULLUP);
    pinMode(motors[i].driverPin1, OUTPUT);
    pinMode(motors[i].driverPin2, OUTPUT);

    // Serial.println("Pinmode set");
    digitalWrite(motors[i].encoderPinE1, HIGH);
    digitalWrite(motors[i].encoderPinE2, HIGH);

    attachInterrupt(digitalPinToInterrupt(motors[i].encoderPinE1), interruptFunctionArrays[i], CHANGE);
    attachInterrupt(digitalPinToInterrupt(motors[i].encoderPinE2), interruptFunctionArrays[i], CHANGE);

    float p = 1.0f;
    // if(i!=0&&i!=1){
    //   p=4.2f;
    // }
    initMotor(i, p, 0.02f, 0.001f);
    commander.add('a' + i, commanderFunctionArrays[i], "DC motor ");
  }

  Serial.println("Initialization complete.");
}

template <typename Out>
void split(const std::string &s, char delim, Out result) {
    std::istringstream iss(s);
    std::string item;
    while (std::getline(iss, item, delim)) {
        *result++ = item;
    }
    
}

vector<string> split(const string &s, char delim) {
    vector<string> elems;
    split(s, delim, back_inserter(elems));
    return elems;
}

void parseSerialInput(string input) {
  vector<string> motorCommands = split(input, ',');
  for (string command : motorCommands) {
    // Serial.println(const_cast<char*>(command.c_str()));
    commander.run(const_cast<char*>(command.c_str()));
  }
}


void loop() {
  if (Serial.available() > 0) {
    string input = Serial.readStringUntil('\n').c_str();
    parseSerialInput(input);
  }

  for (int i = 0; i < numMotorsToMove; i++) {
    if(i==0){
      motors[i].motor.move();
      motors[i].motor.monitor();

      Serial.print(motors[0].motor.target);
      Serial.print(": ");
      Serial.println(motors[i].angle);
    }
    
  }

}
2

Hi @Sanjeev-Hegde,

Don’t print each time through the loop, that isn’t good for performance. Most of the time will be spent waiting to send data on Serial…

Other than that I can’t at the moment see what is wrong with the code. Obviously DC Motor should also work for negative angles, and this problem hasn’t been reported before… please let me know if you find anything more…

3

Im not convinced the parseSerialInput() will work either. Commander tends to depend on the \n character so I’m not sure that splitting on commas will work if you don’t also add newlines… I also don’t get what the benefit would be. Are you trying to ensure the settings for several motors are applied within the same loop iteration?

Also, the way this code is written it will block forever if it receives only a partial command with no newline at the end… and the moment it receives the first byte of the command it will block until the newline arrives… that’s not ideal. Better would be to buffer bytes until you see the newline, and the process the buffer within one iteration…

And the string processing is mixing String, std:string and const char* in an unfortunate way resulting in what is a bunch of (presumably not intentional) memory allocations and data copying…

Another question I have is whether the PID values have been tuned?

4

Thanks for the reply @runger ,

  1. I only added Serial print to take the above screenshots. It would be removed when put in my robot.
  2. I need to refactor the code which mixes String, char * etc . But the commander logic does work for me. The way I use is, send commands for multiple motors in one go.
    Example input controlling 4 motors simultaneously: a2,b3,c1,d4\n
  3. Here is small video of same code working on my humanoid robot: https://www.reddit.com/r/robotics/comments/1ds8fy4/juggernaut_first_test_on_ground/
  4. In the above robot, I am controlling 6 dc motors (using SimpleFOC) and 4 steppers (accel library). The setup was complex because I could only control 4 dc motors using esp32 because of pin limitations. So I made central teensy send serial commands to 2 esp32s controlling 4 motors each.
  5. Now I am upgrading to stm32F7 (with same code) so that I can control 8 motors simultaneously(it does work, but with the limitation I mentioned above). This is required since I am now building 16DOF hand and want to fit 2 controller chips inside each hand(otherwise i need 4 esp32s). Also I am planning to add canbus for sending data.

I will try to rerun the same code in esp32. I suspect it might be because of some code change I might have done or because of latest version upgrades of libraries (unfortunately i didn’t keep track what was installed earlier before upgrading and also my code versioning anywhere earlier).