ESP32 keeps rebooting due to Guru Meditation Error

Hi,

I'm trying to build a Self Balancing Robot based on an ESP and initial copy of this project.

I got the sketch compiling after going back to board library ESP v2.0.17.

After loading the sketch to the board and rebooting the ESP it I get following error on the serial monitor where I see a reboot. This again leads to the error.

Guru Meditation Error: Core  1 panic'ed (LoadProhibited). Exception was unhandled.

Core  1 register dump:
PC      : 0x40169b9b  PS      : 0x00060d30  A0      : 0x800d8884  A1      : 0x3ffb2170  
A2      : 0x00ff0000  A3      : 0x3ffb219e  A4      : 0x00000001  A5      : 0x3ffb2205  
A6      : 0x00000002  A7      : 0x00000000  A8      : 0x800e3af5  A9      : 0x3ffb2170  
A10     : 0x00000001  A11     : 0x00000000  A12     : 0x00000000  A13     : 0x00000000  
A14     : 0x3ffb833c  A15     : 0x00000000  SAR     : 0x0000000a  EXCCAUSE: 0x0000001c  
EXCVADDR: 0x00ff0010  LBEG    : 0x40084a41  LEND    : 0x40084a49  LCOUNT  : 0x00000027  

Backtrace: 0x40169b98:0x3ffb2170 0x400d8881:0x3ffb2190 0x400d8894:0x3ffb21c0 0x400d85e2:0x3ffb21e0 0x400d86d1:0x3ffb2230 0x400d3e89:0x3ffb2250 0x400e4135:0x3ffb2290


ELF file SHA256: 20ba309ab822accc

How to identify what is going wrong?
Does anyone understand what this is telling me?

The ESP has following identifiers:

Chip is ESP32-D0WD-V3 (revision v3.1)
Features: WiFi, BT, Dual Core, 240MHz, VRef calibration in efuse, Coding Scheme None
Crystal is 40MHz

Use the Exception decoder tool in the IDE but first you need to install it

See this (and other) topics ESP ExceptionDecoder with Arduino IDE 2.3.4

and post your sketchere, using code tags when you do

1 Like

Ok. That might help indeed.

ESP Exception Decoder
Sketch: SelfBalancingRobot FQBN: esp32:esp32:esp32

Guru Meditation Error: Core  1 panic'ed (LoadProhibited). Exception was unhandled.

Core  1 register dump:
PC      : 0x4016b517  PS      : 0x00060d30  A0      : 0x800d9120  A1      : 0x3ffb2170  
A2      : 0x00ff0000  A3      : 0x3ffb219e  A4      : 0x00000001  A5      : 0x3ffb2205  
A6      : 0x00000002  A7      : 0x00000000  A8      : 0x800e4da5  A9      : 0x3ffb2170  
A10     : 0x00000001  A11     : 0x00000000  A12     : 0x00000000  A13     : 0x00000000  
A14     : 0x3ffb833c  A15     : 0x00000000  SAR     : 0x0000000a  EXCCAUSE: 0x0000001c  
EXCVADDR: 0x00ff0010  LBEG    : 0x40084a45  LEND    : 0x40084a4d  LCOUNT  : 0x00000027  



1 | LoadProhibited: A load referenced a page mapped with an attribute that does not permit loads | 28

PC -> 0x4016b517: Adafruit_GenericDevice::readRegister (unsigned char*, unsigned char, unsigned char*, unsigned short) at d:\Users\Ron\Documents\Arduino\libraries\Adafruit_BusIO\Adafruit_GenericDevice.cpp:73
Fault -> 0x00ff0010: ??

Not aware I was using that library. Must be part of some other library.
Any indication?

Yes, use Adafruit_INA219 that includes Adafruit_I2CDevice that is included in Adafruit_BusIO just like Adafruit_GenericDevice which is part of the library. :sweat_smile:

Not unless you post your sketch here as requested

1 Like

The Adafruit_INA219 is included.

As requested the sketch. I commented some code out of the sketch to make it run beyond the setup (otherwise the Guru error would not show and we see only a lock since no data coming to the serial monitor).

Also the inits of the INA219 and the MPU6050 are commented out.

/*
A high speed balancing robot, running on an ESP32.

Chip Model: ESP32-D0WD-V3
Chip revision: 3
Board: ESP32 Dev Module
FCC ID: 2BB77-ESP32-32X

Use board library ESP32 v2.0.17 
*/

#include <Arduino.h>
#include <WiFi.h>
#include <MPU6050.h>
#include <Streaming.h>
#include <PID.h>
#include <fastStepper.h>
#include <par.h>
#include <Preferences.h> // for storing settings
#include <Adafruit_INA219.h>
#include <ESPAsyncWebServer.h>
#include <WebSocketsServer.h> 
#include <FS.h>
#include <LittleFS.h>
#include <ESPmDNS.h>
#include "defines.h"


// ----- Type definitions
typedef union
{
  struct
  {
    float val; // Float (4 bytes) comes first, as otherwise padding will be applied
    uint8_t cmd;
    uint8_t checksum;
  };
  uint8_t array[6];
} command;

// ----- Function prototypes
bool connectWifi();
void parseSerial();
void parseCommand(char *data, uint8_t length);
void calculateGyroOffset(uint8_t nSample);
void webSocketEvent(uint8_t num, WStype_t type, uint8_t *payload, size_t length);
void readSensor();
void initSensor(uint8_t n);
void sendConfigurationData(uint8_t num);
bool InitPIDs();

void IRAM_ATTR motLeftTimerFunction();
void IRAM_ATTR motRightTimerFunction();

// ----- Definitions and variables
// -- Server
const char *http_username = "admin";
const char *http_password = "admin";
AsyncWebServer server(80);
WebSocketsServer wsServer = WebSocketsServer(81);

// -- EEPROM
Preferences preferences;

// -- Stepper motors
fastStepper motLeft(PIN_MOTOR_LEFT_STEP, PIN_MOTOR_LEFT_DIR, 0, REVERSE_LEFT_MOTOR, motLeftTimerFunction);
fastStepper motRight(PIN_MOTOR_RIGHT_STEP, PIN_MOTOR_RIGHT_DIR, 1, REVERSE_RIGHT_MOTOR, motRightTimerFunction);

uint8_t motorCurrent = 35;
float maxStepSpeed = MAX_STEP_SPEED;
float absSpeedChange = 0;

// -- PID control
// go (default)
PID pidAngle(cPD, dT, PID_ANGLE_MAX, -PID_ANGLE_MAX);
PID pidPos(cPD, dT, PID_POS_MAX, -PID_POS_MAX);
PID pidSpeed(cPID, dT, PID_SPEED_MAX, -PID_SPEED_MAX);
// stay
PID pidAngleStay(cPD, dT, PID_ANGLE_MAX, -PID_ANGLE_MAX);
PID pidPosStay(cPD, dT, PID_POS_MAX, -PID_POS_MAX);

// to save intial values
PID intialPidAngle(cPD, dT, PID_ANGLE_MAX, -PID_ANGLE_MAX);
PID intialPidPos(cPD, dT, PID_POS_MAX, -PID_POS_MAX);

uint8_t controlMode = 1; // 0 = only angle, 1 = angle+position, 2 = angle+speed

// -- IMU
MPU6050 imu;

// current and voltage
Adafruit_INA219 ina219(0x40);

int16_t gyroOffset[3];
float accAngle = 0;
float filterAngle = 0;
float accAngle_X = 0;
float filterAngle_X = 0;
float angleOffset = 0.0;
float gyroFilterConstant = GYRO_FILTER_CONSTANT;
float gyroGain = GYRO_GAIN;

// -- Others
float steerFilterConstant = 0.9;
float speedFilterConstant = 0.9;

// -- WiFi
const char host[] = "BalancingRobot";

// Noise source (for system identification)
boolean noiseSourceEnable = 0;
float noiseSourceAmplitude = 1;

float steerInput, speedInput;

uint32_t lastCntInput = 0;

boolean enableControl = 0;

boolean inOTA = false;

uint8_t pidTypeApply = 1; // PID to use 0=auto, 1= go PIDs, 2= stay PIDs

float batVoltageArray[16];
float batVoltage = 0;
float batVoltageLowest = 26;
float current_mA = 0;
float power_mW = 0;
float mA_h_count = 0;

uint32_t energySave_ready = 0;

boolean calibrationAutoDone = false;


// ----- Interrupt functions -----
portMUX_TYPE timerMux = portMUX_INITIALIZER_UNLOCKED;


void IRAM_ATTR motLeftTimerFunction()
{
  portENTER_CRITICAL_ISR(&timerMux);
  motLeft.timerFunction();
  portEXIT_CRITICAL_ISR(&timerMux);
}


void IRAM_ATTR motRightTimerFunction()
{
  portENTER_CRITICAL_ISR(&timerMux);
  motRight.timerFunction();
  portEXIT_CRITICAL_ISR(&timerMux);
}

void sendData(uint8_t *b, uint8_t l)
{
  wsServer.sendBIN(0, b, l);
}

void wirelessTask(void *parameters)
{
  while (1)
  {
    //IBus.loop();
    wsServer.loop();
    //ArduinoOTA.handle();
    delay(2);
  }
}


// compare PIDs
bool InitPIDs()
{
  if (pidAngle.K == intialPidAngle.K && pidAngle.Ti == intialPidAngle.Ti && pidAngle.Td == intialPidAngle.Td && pidAngle.N == intialPidAngle.N)
  {
    if (pidPos.K == intialPidPos.K && pidPos.Ti == intialPidPos.Ti && pidPos.Td == intialPidPos.Td && pidPos.N == intialPidPos.N)
      return true;
  }
  return false;
}


// ----- Main code
void setup()
{
  Serial.begin(115200);
  preferences.begin("settings", false); // false = RW-mode
  // preferences.clear();  // Remove all preferences under the opened namespace

  pinMode(PIN_MOTOR_ENABLE, OUTPUT);
  digitalWrite(PIN_MOTOR_ENABLE, 1); // Disable steppers during startup

  pinMode(PIN_LED, OUTPUT);
  digitalWrite(PIN_LED, 0);

  motLeft.init();
  motRight.init();
  motLeft.microStep = MICRO_STEP;
  motRight.microStep = MICRO_STEP;

  // Connect to Wifi 
  Serial.println("Init WiFi...");
  boolean wifiConnected = connectWifi();

  // Start DNS server
  if (MDNS.begin(host))
  {
    Serial.print("MDNS responder started, name: ");
    Serial.println(host);
  }
  else
  {
    Serial.println("Could not start MDNS responder");
  }

  // LittleFS setup
  if (!LittleFS.begin(FORMAT_LITTLEFS_IF_FAILED))
  {
    Serial.println("LittleFS mount failed");
    return;
  }
  else
  {
    Serial.println("LittleFS mount success !");
  }

  // Test file 
  // File file = LittleFS.open("/index.htm", "r");
  // if (!file) 
  // {
  //   Serial.println("Failed to open file '/index.htm' for reading");
  //   return;
  // }
  // Serial.println("File Content:");
  // while (file.available()) {
  //   Serial.write(file.read());
  // }
  // file.close();
  // Serial.println();

  // Define a route to serve the HTML page
  server.on("/", HTTP_GET, [](AsyncWebServerRequest* request) {
    request->send(200, "text/html", "<html><body><h1>Hello, ESP32!</h1></body></html>");
    });
  server.on("/", HTTP_GET, [](AsyncWebServerRequest *request) {
    request->send(LittleFS, "/index.htm"); 
    });
  server.serveStatic("/", LittleFS, "/");
  server.onNotFound([](AsyncWebServerRequest *request) { 
    request->send(404, "text/plain", "FileNotFound"); 
    });

  // Start the server
  server.begin();

  wsServer.begin();
  wsServer.onEvent(webSocketEvent);

  MDNS.addService("http", "tcp", 80);
  MDNS.addService("ws", "tcp", 81);

  // Gyro setup
  delay(200);
  Wire.begin(21, 22, 400000);

  // ============== find all i2c adresses
  Serial.println("===============================");
  Serial.println(F("Finding all i2c adresses !"));

  byte error, address;
  int nDevices;
  nDevices = 0;
  for (address = 1; address < 127; address++)
  {
    // The i2c_scanner uses the return value of
    // the Write.endTransmisstion to see if
    // a device did acknowledge to the address.
    Wire.beginTransmission(address);
    error = Wire.endTransmission();

    if (error == 0)
    {
      Serial.println("-");
      Serial.print("I2C device found at address 0x");
      if (address < 16)
        Serial.print("0");
      Serial.print(address, HEX);
      Serial.println("  !");

      nDevices++;
    }
    else if (error == 4)
    {
      Serial.println("-");
      Serial.print("Unknow error at address 0x");
      if (address < 16)
        Serial.print("0");
      Serial.println(address, HEX);
    }
  } // for loop
  if (nDevices == 0)
  {
    Serial.println("No I2C devices found");
  }
  Serial.println("===============================");

  // ============== i2c test end.

  Serial.println("Init Gyro...");
//  imu.initialize();
  Serial.println("Calculate Gyro range and store offsets...");
//  imu.setFullScaleGyroRange(MPU6050_GYRO_FS_500);
  // Calculate and store gyro offsets
  delay(50);

  // sensor start
  Serial.println("Init ina219...");
//  ina219.begin();

  // Init EEPROM, if not done before
  Serial.println("Init EEPROM...");
  if (preferences.getUInt("pref_version", 0) != PREF_VERSION)
  {
    preferences.clear(); // Remove all preferences under the opened namespace
    preferences.putUInt("pref_version", PREF_VERSION);
    Serial << "EEPROM init complete, all preferences deleted, new pref_version: " << PREF_VERSION << "\n";
  }

  // Read gyro offsets
  Serial.println("Gyro calibration values...");
  Serial << "Gyro calibration values: ";
  for (uint8_t i = 0; i < 3; i++)
  {
    char buf[16];
    sprintf(buf, "gyro_offset_%u", i);
    gyroOffset[i] = preferences.getShort(buf, 0);
    Serial << gyroOffset[i] << "\t";
  }
  Serial << endl;

  // Read angle offset
  angleOffset = preferences.getFloat("angle_offset", 0.0);

  // Perform initial gyro measurements
  Serial.println("Initial Gyro measurements...");
//  initSensor(50);

  // disable robot
  enableControl = 0;
  inOTA = true;
  // motLeft.speed = 0;
  // motRight.speed = 0;
  digitalWrite(PIN_MOTOR_ENABLE, 1);

  // PID (go)
  Serial.println("Init PID...");
  pidAngle.setParameters(KP_ANGLE_GO, KI_ANGLE_GO, KD_ANGLE_GO, KN_ANGLE_GO);
  pidPos.setParameters(KP_POS_GO, KI_POS_GO, KD_POS_GO, KN_POS_GO);
  pidSpeed.setParameters(KP_SPEED_GO, KI_SPEED_GO, KD_SPEED_GO, KN_SPEED_GO);

  // PID (stay)
  pidAngleStay.setParameters(KP_ANGLE_STAY, KI_ANGLE_STAY, KD_ANGLE_STAY, KN_ANGLE_STAY);
  pidPosStay.setParameters(KP_POS_STAY, KI_POS_STAY, KD_POS_STAY, KN_POS_STAY);

  intialPidAngle = pidAngle; // copy intial PIDs
  intialPidPos = pidPos;

  // Run wireless related tasks on core 0
  xTaskCreatePinnedToCore(
                     wirelessTask,   /* Function to implement the task */
                     "wirelessTask", /* Name of the task */
                     10000,      /* Stack size in words */
                     NULL,       /* Task input parameter */
                     0,          /* Priority of the task */
                     NULL,       /* Task handle. */
                     0);  /* Core where the task should run */

  // ready beep
  Serial.println("Init Stepper motors...");
  dacWrite(PIN_MOTOR_CURRENT, 7);
  digitalWrite(PIN_MOTOR_ENABLE, 0); // enable
  motLeft.speed = -1000;
  motRight.speed = -1000;
  Serial.println("Init Stepper motors: speed update");
  // motLeft.update();
  // motRight.update();
  // delay(360);
  // Serial.println("Init Stepper motors: speed set");
  // motLeft.speed = 650;
  // motRight.speed = 650;
  // Serial.println("Init Stepper motors: speed update");
  // motLeft.update();
  // motRight.update();
  // delay(170);
  // digitalWrite(PIN_MOTOR_ENABLE, 1); // disable
  // dacWrite(PIN_MOTOR_CURRENT, motorCurrent);

  Serial.println("End of setup!");
}


void loop()
{
  static unsigned long tLast = 0;
  float pidAngleOutput = 0;
  float avgMotSpeed;
  float steer = 0;
  static float avgSteer;
  static float avgSpeed;
  static float avgMotSpeedSum = 0;
  int32_t avgMotStep;
  float pidPosOutput = 0, pidSpeedOutput = 0;
  static uint8_t k = 0;
  static uint32_t lastInputTime = 0;
  uint32_t tNowMs;
  float noiseValue = 0;

  static uint32_t sendLaslWs = 0;

  static uint32_t maxCurHold = 0;

  static uint32_t measurment_last = 0;
  static boolean measurment_switch = true; // just to minimize i2c use time

  static uint32_t maxSpeedCount = 0;

  static float absSpeed = 0;

  unsigned long tNow = micros();
  tNowMs = millis();

  // test
  Serial.println("looping...");

  // check voltage every xx ms.
  if (millis() - measurment_last > 100)
  {
    measurment_last = millis();
    if (measurment_switch)
    { // true every 200ms
      for (int i = 0; i != 15; i++)
      { // move
        batVoltageArray[i] = batVoltageArray[i + 1];
      }
      batVoltageArray[15] = ina219.getBusVoltage_V() + (ina219.getShuntVoltage_mV() / 1000) + 0.1; // write to last
      float batVoltageTemp = 0;
      for (int i = 0; i != 15; i++)
      { // add all
        batVoltageTemp += batVoltageArray[i];
      }
      if (batVoltageArray[0] != 0)
      {                                    // if array full, calculate avg
        batVoltage = batVoltageTemp / 15;  // avg
        if (batVoltage < batVoltageLowest) // updt lowest voltage
        {
          batVoltageLowest = batVoltage;
        }
      }
    }

    if (!measurment_switch)
    {
      current_mA = (current_mA + ina219.getCurrent_mA()) / 2;
      power_mW = (power_mW + ina219.getPower_mW()) / 2;
      if (current_mA < 0)
      {
        mA_h_count += -((current_mA / 3600) / 5);
      }
    }

    measurment_switch = !measurment_switch; // invert
  }

  // blink led depending on battery level
  if (batVoltageLowest < DEAD_BATTERY_VOLT - 0.2)
  { // dead battery
    digitalWrite(PIN_LED, millis() >> 6 & 1);
    enableControl = 0;
    motLeft.speed = 0.003; // very slow led blinking, dead battery
    motRight.speed = 0.003;
    digitalWrite(PIN_MOTOR_ENABLE, 1); // disable
  }
  else if (batVoltageLowest < DEAD_BATTERY_VOLT)
  {
    digitalWrite(PIN_LED, millis() >> 8 & 1);
  }
  else if (batVoltageLowest < DEAD_BATTERY_VOLT + 0.5)
  {
    digitalWrite(PIN_LED, millis() >> 10 & 1);
  } // 1 Hz blinking
  else
  {
    digitalWrite(PIN_LED, 0);
  }

  if (tNow - tLast > dT_MICROSECONDS && !inOTA)
  {
    readSensor();

    if (enableControl)
    {
      // to 0, if not updated x ms, in case ws control suddently goes offline with not 0 as the last value
      if (lastCntInput < millis() - 1300)
      {
        speedInput = 0;
        steerInput = 0;
      }

      avgSpeed = speedFilterConstant * avgSpeed + (1 - speedFilterConstant) * speedInput / 12;
      avgSteer = steerFilterConstant * avgSteer + (1 - steerFilterConstant) * steerInput / 2.0;

      // turn off on dead battery
      if (batVoltageLowest < DEAD_BATTERY_VOLT)
      {
        avgSpeed = 0;
        avgSteer = 0;
      }

      if (abs(avgSpeed) < 0.2)
      {
        // speedInput = 0;
      }
      else
      {
        lastInputTime = tNowMs;
        if (controlMode == 1)
        {
          controlMode = 2;
          motLeft.setStep(0);
          motRight.setStep(0);
          pidSpeed.reset();
        }
      }

      steer = avgSteer;

      if (tNowMs - lastInputTime > 300 && controlMode == 2)
      {
        controlMode = 1;
        motLeft.setStep(0);
        motRight.setStep(0);
        pidPos.reset();
      }

      if (controlMode == 0)
      { // angle
        pidAngle.setpoint = avgSpeed * 2;
      }
      else if (controlMode == 1)
      { // angle+position
        avgMotStep = (motLeft.getStep() + motRight.getStep()) / 2;
        pidPos.setpoint = avgSpeed;
        pidPos.input = -((float)avgMotStep) / 1000.0;
        pidPosOutput = pidPos.calculate();
        pidAngle.setpoint = pidPosOutput;
      }
      else if (controlMode == 2)
      {                                          // angle+speed
        pidSpeed.setpoint = avgSpeed;            // pid set target
        pidSpeed.input = -avgMotSpeedSum / 35.0; // pid speed now (input)
        pidSpeedOutput = pidSpeed.calculate();   // pid out
        pidAngle.setpoint = pidSpeedOutput;      // apply to angle
      }

      pidAngle.input = filterAngle;
      pidAngleOutput = pidAngle.calculate();

      if (noiseSourceEnable)
      {
        noiseValue = noiseSourceAmplitude * ((random(1000) / 1000.0) - 0.5);
        pidAngleOutput += noiseValue;
      }

      avgMotSpeedSum += pidAngleOutput / 2;
      if (avgMotSpeedSum > maxStepSpeed)
      {
        avgMotSpeedSum = maxStepSpeed;
      }
      else if (avgMotSpeedSum < -maxStepSpeed)
      {
        avgMotSpeedSum = -maxStepSpeed;
      }
      avgMotSpeed = avgMotSpeedSum;

      float absSpeed_Before = absSpeed;
      absSpeed = abs(avgMotSpeed);

      absSpeedChange = abs(absSpeed_Before - absSpeed);

      // default control
      motLeft.speed = avgMotSpeed + steer;
      motRight.speed = avgMotSpeed - steer;

      // count how many cycles robot is stable & not moving.
      if (absSpeed < 20 && abs(filterAngle) < 5 && controlMode == 1 && abs(avgSteer) < 0.01) // true if robot stable
      {
        energySave_ready++;
      }
      else
      {
        energySave_ready = 0;
      }
      //=============================================

      // count how many cycles motors at max speed without input.
      if (maxStepSpeed == absSpeed && abs(avgSpeed) < 0.2)
      {
        maxSpeedCount++;
      }
      else
      {
        maxSpeedCount = 0;
      }
      //=============================================

      //==================current control
      int setCur = 0;
      if (absSpeed < 15)
      {
        setCur = map(absSpeed, 0, 15, 42, 70);
        setCur = constrain(setCur, 42, 70);
      }

      // lower current when robot stays for a long period of time
      if (energySave_ready >= 2500 && absSpeed < 5 && abs(filterAngle) < 4)
      {
        setCur = map(absSpeed, 0, 3, 28, 42);
      }

      if (absSpeed >= 15)
      {
        setCur = map(absSpeed, 15, 120, 70, 110);
        setCur = constrain(setCur, 70, 110);
      }

      // add current on angle
      if (setCur < 100)
      {
        int addCur = map(abs(filterAngle), 0, 8, 0, 25);
        addCur = constrain(addCur, 0, 25);
        setCur = setCur + addCur;
      }

      // on fast change max current for atleast XX ms
      if (absSpeedChange > 2 || millis() - maxCurHold < 250)
      {
        setCur = 180;
        if (absSpeedChange > 2)
        {
          maxCurHold = millis();
        }
      }
      motorCurrent = setCur;
      dacWrite(PIN_MOTOR_CURRENT, setCur);
      //====

      // apply spetial stay PIDs if robot stable
      if ((energySave_ready == 400 && abs(filterAngle) < 1.3) && InitPIDs() && pidTypeApply == 0)
      {
        pidAngle = pidAngleStay; // apply stay (no swing pids), good for staying, bad for going
        pidPos = pidPosStay;     // apply stay (no swing pids), good for staying, bad for going
        // Serial.println("Stay PIDs applied automatically");
      }
      if ((abs(filterAngle) >= 2.2 || energySave_ready < 400) && !InitPIDs() && pidTypeApply == 0) // robot going, so revert PIDs
      {
        pidAngle = intialPidAngle; // revert to initial pids
        pidPos = intialPidPos;
        // Serial.println("Go PIDs applied automatically");
      }

      if (pidTypeApply == 2) // only stay PIDs
      {
        pidTypeApply = 20;       // it means applied
        pidAngle = pidAngleStay; // apply stay (no swing pids), good for staying, bad for going
        pidPos = pidPosStay;     // apply stay (no swing pids), good for staying, bad for going
        // Serial.println("Stay PIDs applied manually");
      }

      if (pidTypeApply == 1) // only go PIDs
      {
        pidTypeApply = 10;         // applied
        pidAngle = intialPidAngle; // revert to initial pids
        pidPos = intialPidPos;
        // Serial.println("Go PIDs applied manually");
      }
      //======================
      // auto micro calibratation (once per boot when robot stable)
      if (abs(filterAngle) >= 1.8 && energySave_ready == 4000 && !calibrationAutoDone)
      { // auto calibration
        angleOffset = angleOffset + (filterAngle * 0.5);
        preferences.putFloat("angle_offset", angleOffset);
        calibrationAutoDone = true;
      }
      //======

      // Disable control if robot is almost horizontal. Re-enable if upright.
      if (abs(filterAngle) > 50 || maxSpeedCount > 300)
      { // (if motors at max spped for a long time, something happened, disable robot)
        enableControl = 0;
        motLeft.speed = 0.01; // slow led blinking, ready state
        motRight.speed = 0.01;
        digitalWrite(PIN_MOTOR_ENABLE, 1); // Inverted action on enable pin
      }
    }
    else
    {
      if (abs(filterAngle) < 0.5 && abs(filterAngle_X) < 13 && batVoltageLowest > DEAD_BATTERY_VOLT)
      { // (re-)enable and reset stuff
        enableControl = 1;
        controlMode = 1;
        avgMotSpeedSum = 0;
        motLeft.setStep(0);
        motRight.setStep(0);
        pidAngle.reset();
        pidPos.reset();
        pidSpeed.reset();
        digitalWrite(PIN_MOTOR_ENABLE, 0); // Inverted action on enable pin
        // delay(1);
      }
    }

    motLeft.update();
    motRight.update();

    parseSerial();

    // Serial << micros()-tNow << "\t";
    tLast = tNow;

    // Run other tasks
    // eyes of robot?
    // internet requests?

    // Serial << micros()-tNow << endl;
  }
}


bool connectWifi() 
{
  if (WiFi.status() == WL_CONNECTED) return false;
  //Manual Wifi
  Serial.printf("Connecting to WiFi %s/%s", WIFI_SSID.c_str(), WIFI_PASS.c_str());
  WiFi.disconnect();
  WiFi.mode(WIFI_STA);
  WiFi.hostname(WIFI_HOSTNAME);
  WiFi.begin(WIFI_SSID.c_str(), WIFI_PASS.c_str());
  int i = 0;
  while ((WiFi.status() != WL_CONNECTED) || (i > 5) )
  {
    delay(500);
    i += 1;
    Serial.print(".");
  }
  if (WiFi.status() == WL_CONNECTED)
  {
    Serial.printf("Connected to WiFi '%s'\n", WIFI_SSID.c_str());
    Serial.printf("Connected, IP address: %s/%s\n", WiFi.localIP().toString().c_str(), WiFi.subnetMask().toString().c_str()); //Get ip and subnet mask
    Serial.printf("Connected, MAC address: %s\n", WiFi.macAddress().c_str());  //Get the local mac address
    return true;
  }
  else
  {
    Serial.printf("Could not connected to WiFi '%s'\n", WIFI_SSID.c_str());
    return false;
  }
}


void parseSerial()
{
  static char serialBuf[10];
  static uint8_t pos = 0;
  char currentChar;

  while (Serial.available())
  {
    currentChar = Serial.read();
    serialBuf[pos++] = currentChar;
    if (currentChar == 'x')
    {
      parseCommand(serialBuf, pos);
      pos = 0;
    }
  }
}


void parseCommand(char *data, uint8_t length)
{
  float val2;
  if ((data[length - 1] == 'x') && length >= 3)
  {
    switch (data[0])
    {

      //=== other
    case 'c':
    { // Change controller parameter
      uint8_t controllerNumber = data[1] - '0';
      char cmd2 = data[2];
      float val = atof(data + 3);

      // Make a temporary pid object, in which parameters are updated
      PID pidTemp = pidAngle;
      switch (controllerNumber)
      {
      case 1:
        pidTemp = pidAngle;
        break;
      case 2:
        pidTemp = pidPos;
        break;
      case 3:
        pidTemp = pidSpeed;
        break;
      }

      switch (cmd2)
      {
      case 'p':
        pidTemp.K = val;
        break;
      case 'i':
        pidTemp.Ti = val;
        break;
      case 'd':
        pidTemp.Td = val;
        break;
      case 'n':
        pidTemp.N = val;
        break;
      case 't':
        pidTemp.controllerType = (uint8_t)val;
        break;
      case 'm':
        pidTemp.maxOutput = val;
        break;
      case 'o':
        pidTemp.minOutput = -val;
        break;
      }
      pidTemp.updateParameters();

      // Store temporary pid object in correct pid object
      switch (controllerNumber)
      {
      case 1:
        pidAngle = pidTemp;
        break;
      case 2:
        pidPos = pidTemp;
        break;
      case 3:
        pidSpeed = pidTemp;
        break;
      }

      Serial << controllerNumber << "\t" << pidTemp.K << "\t" << pidTemp.Ti << "\t" << pidTemp.Td << "\t" << pidTemp.N << "\t" << pidTemp.controllerType << endl;
      break;
    }

    case 'y': // PID to use 0=auto 1= go PIDs 2= stay PIDs
      pidTypeApply = atoi(&data[1]);
      Serial << pidTypeApply << endl;
      break;

    case 'a': // Change angle offset
      angleOffset = atof(data + 1);
      Serial << angleOffset << endl;
      break;
    case 'f':
      gyroFilterConstant = atof(data + 1);
      Serial << gyroFilterConstant << endl;
      break;
    case 'v':
      motorCurrent = atof(data + 1);
      Serial << motorCurrent << endl;
      dacWrite(PIN_MOTOR_CURRENT, motorCurrent);
      break;
    case 'm':
      val2 = atof(data + 1);
      Serial << val2 << endl;
      controlMode = val2;
      break;
    case 'u':
      //
      break;
    case 'g':
      gyroGain = atof(data + 1);
      break;
    case 'p':
    {
      switch (data[1])
      {
      case 'n': // Noise source enable
        noiseSourceEnable = atoi(data + 2);
        break;
      case 'a': // Noise source amplitude
        noiseSourceAmplitude = atof(data + 2);
        break;
      }
      break;
    }
    case 'j':
      gyroGain = atof(data + 1);
      break;
    case 'k':
    {
      uint8_t cmd2 = atoi(data + 1);
      if (cmd2 == 1)
      { // calibrate gyro
        calculateGyroOffset(100);
      }
      else if (cmd2 == 2)
      { // calibrate acc
        Serial << "Updating angle offset from " << angleOffset;
        // angleOffset = filterAngle;
        angleOffset = angleOffset + (filterAngle * 0.5);
        Serial << " to " << angleOffset << endl;
        preferences.putFloat("angle_offset", angleOffset);
      }
      break;
    }
    case 'l':
      maxStepSpeed = atof(&data[1]);
      break;
    case 'n':
      gyroFilterConstant = atof(&data[1]);
      break;
    }
  }
}


void calculateGyroOffset(uint8_t nSample)
{
  int32_t sumX = 0, sumY = 0, sumZ = 0;
  int16_t x, y, z;

  for (uint8_t i = 0; i < nSample; i++)
  {
    imu.getRotation(&x, &y, &z);
    sumX += x;
    sumY += y;
    sumZ += z;
    delay(5);
  }

  gyroOffset[0] = sumX / nSample;
  gyroOffset[1] = sumY / nSample;
  gyroOffset[2] = sumZ / nSample;

  for (uint8_t i = 0; i < 3; i++)
  {
    char buf[16];
    sprintf(buf, "gyro_offset_%u", i);
    preferences.putShort(buf, gyroOffset[i]);
  }

  Serial << "New gyro calibration values: " << gyroOffset[0] << "\t" << gyroOffset[1] << "\t" << gyroOffset[2] << endl;
}


void readSensor()
{
  int16_t ax, ay, az, gx, gy, gz;
  float deltaGyroAngle;
  float deltaGyroAngle_X;

  imu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

  accAngle = atan2f((float)ay, (float)az) * 180.0 / M_PI - angleOffset;
  deltaGyroAngle = ((float)((gx - gyroOffset[0])) / GYRO_SENSITIVITY) * dT * gyroGain;
  filterAngle = gyroFilterConstant * (filterAngle + deltaGyroAngle) + (1 - gyroFilterConstant) * (accAngle);
  // Serial << ay/1000.0 << "\t" << az/1000.0 << "\t" << accAngle << "\t" << filterAngle << endl;

  accAngle_X = atan2f((float)ax, (float)az) * 180.0 / M_PI - 5.68;
  deltaGyroAngle_X = -((float)((gx - gyroOffset[1])) / GYRO_SENSITIVITY) * dT * 1;
  filterAngle_X = 0.98 * (filterAngle_X + deltaGyroAngle_X) + (1 - 0.98) * (accAngle_X);
  // Serial << ax/1000.0 << "\t" << az/1000.0 << "\t" << accAngle_X << "\t" << filterAngle_X << endl;
}


void initSensor(uint8_t n)
{
  float gyroFilterConstantBackup = gyroFilterConstant;
  gyroFilterConstant = 0.8;
  for (uint8_t i = 0; i < n; i++)
  {
    readSensor();
  }
  gyroFilterConstant = gyroFilterConstantBackup;
}


void webSocketEvent(uint8_t num, WStype_t type, uint8_t *payload, size_t length)
{
  Serial.println("webSocketEvent");
  switch (type)
  {
  case WStype_DISCONNECTED:
    Serial.printf("[%u] Disconnected!\n", num);
    break;
  case WStype_CONNECTED:
  {
    IPAddress ip = wsServer.remoteIP(num);
    Serial.printf("[%u] Connected from %d.%d.%d.%d url: %s\n", num, ip[0], ip[1], ip[2], ip[3], payload);
    sendConfigurationData(num);
  }
  break;
  case WStype_TEXT:
    Serial.printf("[%u] get Text: %s\n", num, payload);
    parseCommand((char *)payload, length);
    break;
  case WStype_BIN:
  {
    // Serial.printf("[%u] get binary length: %u\n", num, length);

    if (length == 6)
    {
      cmd c;
      memcpy(c.arr, payload, 6);
      // Serial << "Binary: " << c.grp << "\t" << c.cmd << "\t" << c.val << "\t" << sizeof(cmd) << endl;

      if (c.grp < parList::groupCounter)
      {
        // if (c.grp==0 && c.cmd<100) {
        //   pidParList.set(c.cmd,c.val);

        //   // pidPar[c.cmd].setFloat(c.val);
        // }
        // if (c.cmd==253) {
        //   pidParList.sendList(&wsServer);
        // }
        // if (c.cmd==254) {
        //   pidParList.read();
        // }
        // if (c.cmd==255) {
        //   pidParList.write();
        // }
      }
      else if (c.grp == 100)
      {
        if (c.cmd == 0)
        {
          speedInput = c.val;
        }
        else if (c.cmd == 1)
        {
          steerInput = -c.val;
        }
      }
      else if (c.grp == 105)
      {
        lastCntInput = millis();
        String msg = "TELEMETRY: ";
        msg += String(batVoltageLowest);
        msg += "V (L) | ";
        msg += String(batVoltage);
        msg += "V (N) | ";
        msg += String(current_mA);
        msg += "mA | ";
        msg += String(power_mW / 1000);
        msg += "W | Ang: ";
        msg += String(filterAngle);
        if (enableControl == 1) // enabled
        {
          msg += " | Speed: ";
          // msg += String(-(motRight.speed + motLeft.speed) / 2);
          msg += " | Step: ";
          // msg += String(motRight.getStep());
          msg += ":";
          // msg += String(motLeft.getStep());

          if (InitPIDs())
          {

            msg += " [GO PIDs]";
          }
          else
          {
            msg += " [STAY PIDs]";
          }

          if (energySave_ready)
          {
            msg += " ES: ";
            msg += String(energySave_ready);
          }
        }
        else // disabled
        {
          msg += "<font color=\"#0caded\"> [LAYING DOWN]</font>";
        }

        if (calibrationAutoDone)
        {
          msg += " [CLBR DONE]";
        }

        if (batVoltageLowest < DEAD_BATTERY_VOLT) // dead battery
        {
          msg += "<font color=\"#fe3437\"> [LOW BATTERY]</font>";
        }
        if (current_mA < 0) // charging
        {
          msg += "<font color=\"#08e18e\"> [CHARGING]</font>";
          msg += " Capacity: ";
          msg += String(mA_h_count);
          msg += "mA-h";
        }
        wsServer.sendTXT(0, msg);
      }
    }

    break;
  }
  case WStype_ERROR:
  case WStype_FRAGMENT_TEXT_START:
  case WStype_FRAGMENT_BIN_START:
  case WStype_FRAGMENT:
  case WStype_FRAGMENT_FIN:
    break;
  }
}


void sendConfigurationData(uint8_t num)
{
  // save configured data
  char wBuf[63];
  char buf[63];
  sprintf(wBuf, "c%dp%.4f", 1, pidAngle.K);
  wsServer.sendTXT(num, wBuf);
  sprintf(wBuf, "c%di%.4f", 1, pidAngle.Ti);
  wsServer.sendTXT(num, wBuf);
  sprintf(wBuf, "c%dd%.4f", 1, pidAngle.Td);
  wsServer.sendTXT(num, wBuf);
  sprintf(wBuf, "c%dn%.4f", 1, pidAngle.N);
  wsServer.sendTXT(num, wBuf);
  sprintf(wBuf, "c%dr%.4f", 1, pidAngle.R);
  wsServer.sendTXT(num, wBuf);
  sprintf(wBuf, "c%dm%.4f", 1, pidAngle.maxOutput);
  wsServer.sendTXT(num, wBuf);
  sprintf(wBuf, "c%do%.4f", 1, -pidAngle.minOutput);
  wsServer.sendTXT(num, wBuf);
  sprintf(wBuf, "c%dp%.4f", 2, pidPos.K);
  wsServer.sendTXT(num, wBuf);
  sprintf(wBuf, "c%di%.4f", 2, pidPos.Ti);
  wsServer.sendTXT(num, wBuf);
  sprintf(wBuf, "c%dd%.4f", 2, pidPos.Td);
  wsServer.sendTXT(num, wBuf);
  sprintf(wBuf, "c%dn%.4f", 2, pidPos.N);
  wsServer.sendTXT(num, wBuf);
  sprintf(wBuf, "c%dr%.4f", 2, pidPos.R);
  wsServer.sendTXT(num, wBuf);
  sprintf(wBuf, "c%dm%.4f", 2, pidPos.maxOutput);
  wsServer.sendTXT(num, wBuf);
  sprintf(wBuf, "c%do%.4f", 2, -pidPos.minOutput);
  wsServer.sendTXT(num, wBuf);
  sprintf(wBuf, "c%dp%.4f", 3, pidSpeed.K);
  wsServer.sendTXT(num, wBuf);
  sprintf(wBuf, "c%di%.4f", 3, pidSpeed.Ti);
  wsServer.sendTXT(num, wBuf);
  sprintf(wBuf, "c%dd%.4f", 3, pidSpeed.Td);
  wsServer.sendTXT(num, wBuf);
  sprintf(wBuf, "c%dn%.4f", 3, pidSpeed.N);
  wsServer.sendTXT(num, wBuf);
  sprintf(wBuf, "c%dr%.4f", 3, pidSpeed.R);
  wsServer.sendTXT(num, wBuf);
  sprintf(wBuf, "c%dm%.4f", 3, pidSpeed.maxOutput);
  wsServer.sendTXT(num, wBuf);
  sprintf(wBuf, "c%do%.4f", 3, -pidSpeed.minOutput);
  wsServer.sendTXT(num, wBuf);
  sprintf(wBuf, "h%.4f", speedFilterConstant);
  wsServer.sendTXT(num, wBuf);
  sprintf(wBuf, "i%.4f", steerFilterConstant);
  wsServer.sendTXT(num, wBuf);
  sprintf(wBuf, "v%d", motorCurrent);
  wsServer.sendTXT(num, wBuf);
  sprintf(wBuf, "j%.4f", gyroGain);
  wsServer.sendTXT(num, wBuf);
  sprintf(wBuf, "n%.4f", gyroFilterConstant);
  wsServer.sendTXT(num, wBuf);
  sprintf(wBuf, "a%.4f", angleOffset);
  wsServer.sendTXT(num, wBuf);
  sprintf(wBuf, "l%.4f", maxStepSpeed);
  wsServer.sendTXT(num, wBuf);
}

What is in teh defines.h file and which partition scheme is selected for the board in the IDE ?

1 Like

The defines.h file hold some constants (I changed the WIFI parts to "*"-s, this connection works fine):

#ifndef DEFINES_H_
#define DEFINES_H_

String WIFI_SSID = "****************";
String WIFI_PASS = "****************";
#define WIFI_HOSTNAME "ESP32 Balancing Robot"

#define PREF_VERSION 1 // if setting structure has been changed, count this number up to delete all settings
#define FORMAT_LITTLEFS_IF_FAILED true

// -- Stepper motors
#define PIN_MOTOR_ENABLE 27
#define PIN_MOTOR_LEFT_STEP 5
#define PIN_MOTOR_LEFT_DIR 4
#define PIN_MOTOR_RIGHT_STEP 18
#define PIN_MOTOR_RIGHT_DIR 15
#define PIN_MOTOR_CURRENT 25

#define REVERSE_LEFT_MOTOR true
#define REVERSE_RIGHT_MOTOR false

#define MICRO_STEP 32
#define MAX_STEP_SPEED 360 // small wheels : 410, large wheels : 360

// -- PID control
#define dT_MICROSECONDS 5000
#define dT dT_MICROSECONDS / 1000000.0

#define PID_ANGLE_MAX 15
#define PID_POS_MAX 32
#define PID_SPEED_MAX 20

#define GYRO_FILTER_CONSTANT 0.996
#define GYRO_GAIN 1.0

// go
#define KP_ANGLE_GO 0.65
#define KI_ANGLE_GO 0
#define KD_ANGLE_GO 0.2
#define KN_ANGLE_GO 15

#define KP_POS_GO 1
#define KI_POS_GO 0
#define KD_POS_GO 1.2
#define KN_POS_GO 20

#define KP_SPEED_GO 6
#define KI_SPEED_GO 5
#define KD_SPEED_GO 0
#define KN_SPEED_GO 20

// stay (no swing pids, good for staying, bad for going)
#define KP_ANGLE_STAY 1.9
#define KI_ANGLE_STAY 0.57
#define KD_ANGLE_STAY 0.64
#define KN_ANGLE_STAY 8.2

#define KP_POS_STAY 1.0
#define KI_POS_STAY 1.85
#define KD_POS_STAY 0.3
#define KN_POS_STAY 10

// -- IMU
#define GYRO_SENSITIVITY 65.5

// -- Others
#define PIN_LED 2

#define DEAD_BATTERY_VOLT 11.1

#endif /* DEFINES_H_ */

I don't know what the partition scheme is: I just uploaded the html used (data folder) with the "Upload LittleFS to ...." add-in.

In a separate project I striped all down to just the web based elements and the webserver and socketsserver worked fine. So I don't think the flash partition for these files are the issue.

How can I check on the partition scheme?

Check it in the IDE Tools menu

which version of ESPAsyncWebServer are you using?
I have a note in one of my programs which usses ESPAsyncWebServer

// works with AsyncTCP 1.1.4 ESPAsyncWebServer 3.1.0 and ESP32 core 2.0.17

It is the first option in the list: Default 4MB with spiffs (1.2MB APP/1,5 MB SPIFFS).

I use AsyncTCP 1.1.4 and ESPAsyncWebServer 3.1.0 (both latest according to IDE).
But a solution where I stripped down all but the server and websockets part works without any issue. So I guess the error might not be caused by the server parts.

I just commented out all INA219 related stuff (I had a variable defined and some commands were still in use on that variable) and that made the webserver going.

But upon uncommenting the last part (stepper control) in Setup made it stop there again with no error message. On the point where the comments start: motLeft.update(). I figured that out using line by line Serial.println's.

  Serial.println("Init Stepper motors...");
  dacWrite(PIN_MOTOR_CURRENT, 7);
  digitalWrite(PIN_MOTOR_ENABLE, 0); // enable
  motLeft.speed = -1000;
  motRight.speed = -1000;
  Serial.println("Init Stepper motors: speed update");
  // motLeft.update();
  // motRight.update();
  // delay(360);
  // Serial.println("Init Stepper motors: speed set");
  // motLeft.speed = 650;
  // motRight.speed = 650;
  // Serial.println("Init Stepper motors: speed update");
  // motLeft.update();
  // motRight.update();
  // delay(170);
  digitalWrite(PIN_MOTOR_ENABLE, 1); // disable
  dacWrite(PIN_MOTOR_CURRENT, motorCurrent);

  Serial.println("End of setup!");
}

Are there any other Serial messages from setup() being printed ?
or does it fail here already >

  motLeft.init();
  motRight.init();
  motLeft.microStep = MICRO_STEP;
  motRight.microStep = MICRO_STEP;

No, the last serial message "Init Stepper motors: speed update" is shown and then it stops. I see the ESP32 also disconnecting on my router. So no reboor there.

Playing arround in that part of the setup I now got a message:

Init PID...
Init Stepper motors...
Init Stepper motors: speed update
E (32673) task_wdt: Task watchdog got triggered. The following tasks did not reset the watchdog in time:
E (32673) task_wdt:  - async_tcp (CPU 0/1)
E (32673) task_wdt: Tasks currently running:
E (32673) task_wdt: CPU 0: IDLE0
E (32673) task_wdt: CPU 1: loopTask
E (32673) task_wdt: Aborting.

abort() was called at PC 0x400ef061 on core 0


Backtrace: 0x40083c3d:0x3ffbed0c |<-CORRUPTED


So a slow task on async_tcp where the watchdog terminates processing.
There are several parts causing issues: ina219, fastStepper using timers, .....

Strange, since I have seen different users building this balancing robot based on this code.

can you print the linker output showing memory usage? e.g.

Sketch uses 294610 bytes (22%) of program storage space. Maximum is 1310720 bytes.
Global variables use 20680 bytes (6%) of dynamic memory, leaving 307000 bytes for local variables. Maximum is 327680 bytes.

maybe worth trying Partition Scheme . Huge App
maybe worth disabling the watchdog timer

Hi all, thanks for helping me out.

@Horace: what trying on partition scheme? How to disable watchdog? This working on the core of the ESP is new for me :slight_smile:

Sketch uses 931813 bytes (71%) of program storage space. Maximum is 1310720 bytes.
Global variables use 49840 bytes (15%) of dynamic memory, leaving 277840 bytes for local variables. Maximum is 327680 bytes.

The watchdog message does not always appear. Upon narrowing down parts using commenting and serail messages it sometimes appears.

My ESP is a China one. Maybe it is not that compatible?
But then again that rises the question: which one would match the sketch I'm trying to get working.

IDE Tools menu
Choose the Huge App partition scheme

I set it to: Huge APP (3MB no OTA/1MB SPIFFS).

After compiling and loading the sketch again I had to upload the htm files again and things seem to work (stepper part still excluded).
In trying out before I sometimes had to upload the htm files also again suspecting the code is skipping into that partition and corrupting the htm files.

Next step was to uncomment the stepper parts as mentioned above but then all came the same. The update() function on the stepper is at least causing an issue.
Website is reponding and websockets working fine.

So still a Stepper issue and probably a INA219 issue.
The last one I will try to add again when the webpart is not crashing anymore.

So you are saying that it crashes more or less here ? (i am saying more or less because you have just started a task on the other core)

So my suspicion is that something there has not been handled properly.

The websocket runs on core 0 from this task

void wirelessTask(void *parameters)
{
  while (1)
  {
    //IBus.loop();
    wsServer.loop();
    //ArduinoOTA.handle();
    delay(2);
  }
}

but is that really ok ?
shouldn't these 2 lines be a part of that task ?

  wsServer.begin();
  wsServer.onEvent(webSocketEvent);

like this

void wirelessTask(void *parameters)
{
  wsServer.begin();
  wsServer.onEvent(webSocketEvent);
  while (1)
  {
    //IBus.loop();
    wsServer.loop();
    //ArduinoOTA.handle();
    delay(2);
  }
}

I am no specialist on this 2 core stuff, but if you let the WiFi run on Core 0, initialize the websocket on Core 1 and then run a task on core 0 to handle the events, that may not work efficiently to be gine with and it may cause memory read and write conflicts.

I really don't think this is going to make any difference. These settings are meant for situations where the App size (the code) is to big to fit into the default size reserved for the App. That is not the case here