Using esp32, as5048a magnetic sensor, waveshare 1.28inch round LCD, gm3506 motor and button

developement
1

Hi! In need of urgent help :slight_smile:

I’m making a device for ACL rehabilitation which attaches a gimbal motor to your knee to provide resistance and an accurate measure of range of motion.

Have successfully coded (in Arduino) the magnetic sensor, motor and the LCD together in one file, using a tft_espi widget. This enables the user to push against the motor resistance and test their range of motion.

I now want to provide multiple levels of resistance (and for each level of resistance to change the colour of the LCD dial). I coded a simple program which (when button is pressed) goes through three settings below:


// Variables will change:
int counter = 1;
int lastState = HIGH; // the previous state from the input pin
int nowState;     // the current reading from the input pin

void setup() {
  Serial.begin(115200);
  // initialize the pushbutton pin as an pull-up input
  pinMode(BUTTON_PIN, INPUT_PULLUP);
}

void loop() {
  // read the state of the switch/button:
  nowState = digitalRead(BUTTON_PIN);
  if(nowState != lastState){
    delay(50);
    if(nowState == LOW){
      counter++;
      if (counter>3){
        counter=1;
      }
    }
  }    
  Serial.println(counter);

  // save the last state
  lastState = nowState;
  delay(10);
}
type or paste code here

When I try and incorporate this into the master code, I keep getting errors, even resorting to chat-gpt-4 which still can’t find a solution! I have successfully coded it so that the motor stops when the angle is 0 degrees (so that the user doesn’t break their knee!), but can’t seem to manage to have multiple settings via the press of a button.

This is the master code:

#define DRAW_DIGITS

#define BUTTON_PIN 21 // GIOP21 pin connected to button

//.frequency meter is updated
#define LOOP_DELAY 100 

// Hardware-specific libraries
#include <SPI.h>
#include <TFT_eSPI.h> 

// Open loop motor control example
 #include <SimpleFOC.h>

// BLDC motor & driver instance
// BLDCMotor motor = BLDCMotor(pole pair number);
BLDCMotor motor = BLDCMotor(11);

// BLDCDriver3PWM driver = BLDCDriver3PWM(pwmA, pwmB, pwmC, Enable(optional));
BLDCDriver3PWM driver = BLDCDriver3PWM(27, 26, 25, 33);

#ifdef DRAW_DIGITS
  #include "NotoSans_Bold.h"
  #include "Futura.h"
  #include "OpenFontRender.h"
  #define TTF_FONTA NotoSans_Bold
  #define TTF_FONTB Futura
#endif

TFT_eSPI tft = TFT_eSPI();            // Invoke custom library with default width and height
TFT_eSprite spr = TFT_eSprite(&tft);  // Declare Sprite object "spr" with pointer to "tft" object

#ifdef DRAW_DIGITS
OpenFontRender ofr;
#endif
MagneticSensorSPI sensor = MagneticSensorSPI(5, 14, 0x3FFF);

#define BLACK 0x18E3
#define GREEN 0x75C8
#define YELLOW 0xF760
#define RED 0xEAC7

// Jpeg image array attached to this sketch
#include "dial.h"

#include <TJpg_Decoder.h>

// =======================================================================================
// This function will be called during decoding of the jpeg file
// =======================================================================================
bool tft_output(int16_t x, int16_t y, uint16_t w, uint16_t h, uint16_t* bitmap)
{
  // Stop further decoding as image is running off bottom of screen
  if ( y >= tft.height() ) return 0;

  // This function will clip the image block rendering automatically at the TFT boundaries
  tft.pushImage(x, y, w, h, bitmap);

  // Return 1 to decode next block
  return 1;
}

uint32_t runTime = 0;       // time for next update

int reading = 0; // Value to be displayed
int d = 0; // Variable used for the sinewave test waveform
bool range_error = 0;
int8_t ramp = 1;

int counter = 1;
int lastState = HIGH; // the previous state from the input pin
int nowState;     // the current reading from the input pin
bool initMeter = true;

void setup(void) {
  // initialize encoder sensor hardware
  sensor.init();
  // link the motor to the sensor
  motor.linkSensor(&sensor);
  // driver config
  // power supply voltage [V]
  driver.voltage_power_supply = 12;
  // limit the maximal dc voltage the driver can set
  driver.voltage_limit = 12;
  driver.init();
  // link the motor and the driver
  motor.linkDriver(&driver);

  // limit the voltage to be set to the motor
  motor.voltage_limit = 12; 
  // choose FOC modulation
  motor.foc_modulation = FOCModulationType::SpaceVectorPWM;

  // set torque mode
  motor.torque_controller = TorqueControlType::voltage;
  // set motion control loop to be used
  motor.controller = MotionControlType::torque;

  // init motor hardware
  motor.init();
  // align sensor and start FOC
  motor.initFOC();

  Serial.begin(115200);

  // The byte order can be swapped (set true for TFT_eSPI)
  TJpgDec.setSwapBytes(true);

  // The jpeg decoder must be given the exact name of the rendering function above
  TJpgDec.setCallback(tft_output);

  // draw screen
  tft.fillScreen(TFT_NAVY);

  // Draw the dial
  TJpgDec.drawJpg(0, 0, dial, sizeof(dial));

  tft.begin();
  tft.setRotation(1);

  tft.setViewport(0, 0, 240, 240);

  // initialize the pushbutton pin as an pull-up input
  pinMode(BUTTON_PIN, INPUT_PULLUP);
}

float target_voltage = 7;

void loop() {

  // read the state of the switch/button:
  nowState = digitalRead(BUTTON_PIN);

  if(nowState != lastState){
    delay(50);
    if(nowState == LOW){
      counter++;
      if (counter>3){
        counter=1;
      }
    }
  }    

  // save the last state
  lastState = nowState;
  delay(10);

  static uint16_t maxRadius = 0;
  int8_t ramp = 1;
  static uint8_t radius = 0;
  static int16_t xpos = tft.width() / 2;
  static int16_t ypos = tft.height() / 2;
  bool newMeter = false;

  if (maxRadius == 0) {
    maxRadius = tft.width();
    if (tft.height() < maxRadius) maxRadius = tft.height();
    maxRadius = maxRadius / 2;
    radius = maxRadius*1.02;
  }

  tft.fillCircle(xpos, ypos, radius + 1, TFT_NAVY);
  initMeter = true;

#ifdef DRAW_DIGITS
  // Loading a font takes a few milliseconds, so for test purposes it is done outside the test loop
  if (ofr.loadFont(TTF_FONTA, sizeof(TTF_FONTA))) {
    Serial.println("Render initialize error");
    return;
  }
#endif

  initMeter = true;
  reading = 0;
  ramp = 1;
  while (!newMeter) {
    if (millis() - runTime >= LOOP_DELAY) {
      runTime = millis();

      // main FOC algorithm function
      motor.loopFOC();

      motor.move(target_voltage);

      sensor.update();
      int reading = (RAD_TO_DEG)*(sensor.getAngle());
      if (reading<=0) {
        reading = 0;
        target_voltage = 0;
      }
      if (reading>=180) reading = 180;
      if ((reading > 0) && (reading < 180)) target_voltage = 3;

      ringMeter(xpos, ypos, radius, reading, "Degrees"); // Draw analogue meter
    }
  }

#ifdef DRAW_DIGITS
  ofr.unloadFont(); // Recover space used by font metrics etc
#endif
}

// #########################################################################
//  Draw the meter on the screen, returns x coord of righthand side
// #########################################################################

// x,y is centre of meter, r the radius, val a number in range 0-100, units is the meter scale label
void ringMeter(int x, int y, int r, int val, const char *units){
  
  static uint16_t last_angle = 30;

  if (initMeter) {
    initMeter = false;
    last_angle = 30;
    tft.fillCircle(x, y, r, BLACK);
    tft.drawSmoothCircle(x, y, r, TFT_SILVER, BLACK);
    uint16_t tmp = r - 3;
    tft.drawArc(x, y, tmp, tmp - tmp / 5, last_angle, 330, BLACK, BLACK);
  }

  r -= 3;

  // Range here is 0-100 so value is scaled to an angle 30-330
  int val_angle = map(val, 0, 180, 30, 330);


  if (last_angle != val_angle) {
    // Could load the required font here
    //if (ofr.loadFont(TTF_FONT, sizeof(TTF_FONT))) {
    //  Serial.println("Render initialize error");
    //  return;
    //}
#ifdef DRAW_DIGITS
    ofr.setDrawer(spr); // Link renderer to sprite (font will be rendered in sprite spr)

    // Add value in centre if radius is a reasonable size
    if ( r >= 25 ) {
      // This code gets the font dimensions in pixels to determine the required the sprite size
      ofr.setFontSize((6 * r) / 4); // was 4
      ofr.setFontColor(TFT_WHITE, BLACK);


      // The OpenFontRender library only has simple print functions...
      // Digit jiggle for chaging values often happens with proportional fonts because
      // digit glyph width varies ( 1 narrower that 4 for example). This code prints up to
      // 3 digits with even spacing.
      // A few experiemntal fudge factors are used here to position the
      // digits in the sprite...
      // Create a sprite to draw the digits into
      uint8_t w = ofr.getTextWidth("444");
      uint8_t h = ofr.getTextHeight("4") + 4; // was +4
      spr.createSprite(w, h + 2);
      spr.fillSprite(BLACK); // (TFT_BLUE); // (BLACK);
      char str_buf[8];         // Buffed for string
      itoa (val, str_buf, 10); // Convert value to string (null terminated)
      uint8_t ptr = 0;         // Pointer to a digit character
      uint8_t dx = 4;          // x offfset for cursor position
      if (val < 100) dx = ofr.getTextWidth("4") / 2; // Adjust cursor x for 2 digits
      if (val < 10) dx = ofr.getTextWidth("4");      // Adjust cursor x for 1 digit
      while ((uint8_t)str_buf[ptr] != 0) ptr++;      // Count the characters
      while (ptr) {
        ofr.setCursor(w - dx - w / 20, -h / 2.5);    // Offset cursor position in sprtie
        ofr.rprintf(str_buf + ptr - 1);              // Draw a character
        str_buf[ptr - 1] = 0;                        // Replace character with a null
        dx += 1 + w / 3;                            // Adjust cursor for next character
        ptr--;                                     // Decrement character pointer
      }
      spr.pushSprite(x - w / 2, y - h / 2); // Push sprite containing the val number
      spr.deleteSprite();                   // Recover used memory

      // Make the TFT the print destination, print the units label direct to the TFT
      ofr.setDrawer(tft);
      ofr.setFontColor(TFT_WHITE, BLACK);
      ofr.setFontSize(r / 4.0);
      ofr.setCursor(x, y + (r * 0.4));
      ofr.cprintf("Degrees");
    }
#endif 

    // Allocate a value to the arc thickness dependant of radius
    uint8_t thickness = r / 5;
    if ( r < 25 ) thickness = r / 3;

    // Update the arc, only the zone between last_angle and new val_angle is updated
    if (val_angle > last_angle) {
      tft.drawArc(x, y, r, r - thickness, last_angle, val_angle, GREEN, BLACK); // drawing arc
    }
    else {
      tft.drawArc(x, y, r, r - thickness, val_angle, last_angle, BLACK, BLACK);
    }
    last_angle = val_angle; // Store meter arc position for next redraw
  }
}
type or paste code here

Thanks in advance, I know their is a lot of code to sift through - but any help in the next 24 hours would be invaluable <3

Callum

2

Just a heads up, all the hardware is working but the wiring is correct but messy (12v power supply for the gimbal motor, 5v power supply for the esp32 and other electronics, wiring for the button should be in the code). If anyone has any questions/wants me to take any photos I am happy to help! Thanks

3

That is a lot of code!

There are loads of different approaches you could try to reduce the resistance. You could limit/reduce motor voltage (in software) or reduce the P term in one of the pid controllers.

Have you configured any of the PIDs? I wonder if that means you are running whatever the defaults are for those. If you do change them, be careful and keep I term to zero! Don’t want to be integrating errors over time.

Also thought about switching to
motor.controller = MotionControlType::angle;
This would provide resistance in both directions, you’ll probably need to change the desired position to track the movement of the knee otherwise it’ll get harder to move the further they move from the desired position

4

By the look of it, you’re only calling loopFOC every 100 milliseconds (10 times per second). It needs to be much more frequent than that, thousands of times per second. The delay(50) in the input code will also block it from calling loopFOC() often enough. Try something like this to update the button every 50ms without blocking the rest of the code from running:

  if (millis() >= buttonTime + 50) {
    buttonTime = millis();
    lastState = nowState;
    nowState = digitalRead(BUTTON_PIN);
    if (nowState == LOW && lastState == HIGH){
      counter++;
      if (counter>3){
        counter=1;
      }
    }
  }

But the main problem is the while(!newMeter) loop. Nothing ever sets newMeter true, so that is an infinite loop. You should probably just remove that.

If ringMeter should only be called every 100ms, then you could keep just that inside the timer check, like this:

    if (millis() - runTime >= LOOP_DELAY) {
      runTime = millis();
      ringMeter(xpos, ypos, radius, reading, "Degrees"); // Draw analogue meter
   }

With the motor update being done outside so it’s called as often as possible. Or if ringMeter should be called all the time, then you could just remove the if(millis() - runTime >= LOOP_DELAY) check entirely.

There’s no need to call sensor.update() after motor.move(). It doesn’t hurt anything aside from wasting CPU time, but it’s called inside motor.loopFOC() so the reading will be up to date already.

You may have forgotten to set target_voltage = 0 inside the if(reading>=180) block.

I don’t think you should be setting initMeter true inside the update loop, though I’m not 100% sure.

5

Thanks for your help. I’ve implemented the following:

  • Not messing with PID controllers, system works smoothly now so not messing with it
  • Remaining with voltage to control the voltage motor.controller=MotionControlType::voltage; since this works with the project aim, consistently providing resistance against the users movement in the opposite direction until the magnetic sensor reads the motor is at 0 degrees (perfect knee range of motion). Above 180 degrees is also meant to stay on since that is when the user is bending their knee and that is difficult to achieve 180 degrees so this code target_voltage = 0 inside the if(reading>=180) is unnecessary.
  • The if statement code you wrote worked, yet I am unsure how to combine this with this
if (millis() - runTime >= LOOP_DELAY) {
      runTime = millis();
      ringMeter(xpos, ypos, radius, reading, "Degrees"); // Draw analogue meter
   }

to achieve a single if statement which enables the counter to increment and the ringMeter to redraw with the new colour (based on the counter) and the voltage to change. I did as you said and removed the sensor.update() after motor.move(), as well as the initMeter = true statements inside the update loop which were both unnecessary.

I think there’s something wrong with how often the ringMeter is being run, since before the changes it would constantly be on (since it was while (!initMeter) or whatever it was, but now it seems to update less often. The rest of the code is unchanged since before. Any more ideas?

6

Since you’re on ESP32, have you tried putting the display task on the other core from the motor control? This way you can have both the better (motor) loop speed and UI responsiveness. @Karl_Makes_Music has some experience with this.

7

Hi!
Just like @VIPQualityPost mentioned, id suggest separating Motor and Display tasks onto different cores with RTOS.
Millis approach would still delay the FOC loop if you want to keep everything on one core in one task instance.

Regarding the gradual resistance increase i would suggest link the angle with torque
eg the greather angle the more torque (voltage or current) is being applied. This can be done with relatively simple code.
Alternatively voltage can be constant and P value can increase incrementally as greather angle is being reached.

Really depends on circuit, as you want to avoid BEMF or transients spikes when you have your Proportional gain too high.

I’m happy to help you build such a device as this is basically what i’ve been doing for past year i would say so.