What do i do to optimize this code? Also how do i measure how much mah it is using

Other Hardware Sensors
1

Hardware:
Esp32 s2 mini (wemos clone board)
GY-521 gyro
2x buttons
1 rgb led
thats it

I know this code is unoptimized cause i wrote it (mostly copy and paste anyway). I was wondering how i could optimize it, basically all this does is read a mpu6050 and send the data through udp to a udp server i have running on my pc. It works execpt it overheats pretty fast, its not efficent and I'd like to fix that. Please help :slight_smile:

#include <Wire.h>
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"

#include <WiFi.h>
#include <WiFiUdp.h>

MPU6050 mpu;

#define LED_PIN 15
#define BUTTON1 14
#define BUTTON2 10

// Wifi
int status = WL_IDLE_STATUS;
char ssid[] = "myWifiSsd";
char pass[] = "superSecretPassword";
int port = 5468;
IPAddress ip(im,not,getting,doxxed);

WiFiUDP Udp;

// Gyro stuff
float RateRoll, RatePitch, RateYaw;
float AccX, AccY, AccZ;
float AngleRoll, AnglePitch;
float LoopTimer;
uint8_t devStatus; 
uint16_t packetSize; 
uint16_t fifoCount;     // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; 

Quaternion q;           // [w, x, y, z]         quaternion container
VectorInt16 aa;         // [x, y, z]            accel sensor measurements
VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements
VectorFloat gravity;    
float ypr[3];   

float axZero = 0;
float ayZero = 0;
float azZero = 0;

// Offset stuff
int16_t ax, ay, az, gx, gy, gz;

int buffersize=1000;
int acel_deadzone=8;     
int giro_deadzone=1;  

int mean_ax,mean_ay,mean_az,mean_gx,mean_gy,mean_gz,state=0;
int ax_offset,ay_offset,az_offset,gx_offset,gy_offset,gz_offset;

void setup() {
  Serial.begin(57600);

  status = WiFi.begin(ssid, pass);

  delay(10000);

  Udp.begin(port);

  //Buttons
  pinMode(12, OUTPUT);
  pinMode(11, OUTPUT);
  pinMode(15, OUTPUT);
  pinMode(14, INPUT_PULLUP);
  pinMode(10, INPUT_PULLUP);

  // LED
  pinMode(1, OUTPUT);
  pinMode(3, OUTPUT);
  pinMode(5, OUTPUT);
  pinMode(7, OUTPUT);

  digitalWrite(5,HIGH);

  digitalWrite(1,HIGH);
  digitalWrite(5,HIGH);
  digitalWrite(7,HIGH);

  digitalWrite(11,LOW);
  digitalWrite(15, HIGH);
  digitalWrite(15, LOW);


  Wire.begin(33,35);
  Wire.setClock(400000);

  mpu.initialize();

  Serial.println(F("Testing device connections..."));
  devStatus = mpu.dmpInitialize();
  Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

  while(digitalRead(14)!=LOW){
    digitalWrite(3,LOW);
    digitalWrite(7,LOW);
    delay(1000);
    digitalWrite(3,HIGH);
    digitalWrite(7,HIGH);
    delay(1000);
  }
  digitalWrite(3,HIGH);
  digitalWrite(7,HIGH);

  delay(200);

  digitalWrite(3,LOW);

  Serial.println("\nStarting offset calibration...\n");
  
  // Reset offsets
  mpu.setXAccelOffset(0);
  mpu.setYAccelOffset(0);
  mpu.setZAccelOffset(0);
  mpu.setXGyroOffset(0);
  mpu.setYGyroOffset(0);
  mpu.setZGyroOffset(0);

  Serial.println("Reading sensors for first time...");
  meansensors();
  delay(200);

  Serial.println("\nCalculating offsets...");
  calibration();
  delay(200);

  meansensors();
  Serial.println("\nFinished calibration!\n");

  Serial.print("\nOffsets:\t");
    Serial.print(ax_offset); 
    Serial.print("\t");
    Serial.print(ay_offset); 
    Serial.print("\t");
    Serial.print(az_offset); 
    Serial.print("\t");
    Serial.print(gx_offset); 
    Serial.print("\t");
    Serial.print(gy_offset); 
    Serial.print("\t");
    Serial.println(gz_offset); 

  mpu.setXAccelOffset(ax_offset);
  mpu.setYAccelOffset(ay_offset);
  mpu.setZAccelOffset(az_offset);
  mpu.setXGyroOffset(gx_offset);
  mpu.setYGyroOffset(gy_offset);
  mpu.setZGyroOffset(gz_offset);

  digitalWrite(3,HIGH);

  while(!digitalRead(10)==LOW){
    digitalWrite(1,LOW);
    delay(1000);
    digitalWrite(1,HIGH);
    delay(1000);
  }
  if (devStatus == 0) {
    // Calibration Time: generate offsets and calibrate our MPU6050
    mpu.CalibrateAccel(6);
    mpu.CalibrateGyro(6);
    mpu.PrintActiveOffsets();
    // turn on the DMP, now that it's ready
    Serial.println(F("Enabling DMP..."));
    mpu.setDMPEnabled(true);

    // get expected DMP packet size for later comparison
    packetSize = mpu.dmpGetFIFOPacketSize();
  } else {
        // 1 = initial memory load failed
        // 2 = DMP configuration updates failed
    Serial.print(F("DMP Initialization failed (code "));
    Serial.print(devStatus);
    Serial.println(F(")"));
  }
  Serial.println(F("DMP Enabled"));

  delay(100);
  digitalWrite(1,LOW);
  digitalWrite(3,LOW);
  digitalWrite(7,LOW);
}
void loop() {
  if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) {
    mpu.dmpGetQuaternion(&q, fifoBuffer);
    mpu.dmpGetGravity(&gravity, &q);
    mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
    Serial.print("ypr\t");
    Serial.print((ypr[0] * 180/M_PI)-axZero);
    Serial.print("\t");
    Serial.print(((ypr[1] * 180/M_PI)-7)-ayZero);
    Serial.print("\t");
    Serial.println((ypr[2] * 180/M_PI)-azZero);

    // Wifi data transmit (UDP)
    String message = "1," + String((ypr[0] * 180 / M_PI) - axZero) + "," + String(((ypr[1] * 180 / M_PI) - 7) - ayZero) + "," + String((ypr[2] * 180 / M_PI) - azZero);

    // Convert the String to a char array
    char charBuf[message.length() + 1]; // +1 for the null terminator
    message.toCharArray(charBuf, message.length() + 1);

    Udp.beginPacket(ip, port);
    int i = 0;
    while (message[i] != 0) 
    Udp.write((uint8_t)charBuf[i++]);
    Udp.endPacket();

    delay(15);
  }
  if(digitalRead(14)==LOW){
    digitalWrite(3,LOW);
    mpu.setXAccelOffset(0);
    mpu.setYAccelOffset(0);
    mpu.setZAccelOffset(0);
    mpu.setXGyroOffset(0);
    mpu.setYGyroOffset(0);
    mpu.setZGyroOffset(0);

    Serial.println("Recalibrating");
    meansensors();
    delay(200);

    Serial.println("\nCalculating offsets...");
    calibration();
    delay(200);

    meansensors();
    Serial.println("\nFinished calibration!\n");

    Serial.print("\nOffsets:\t");
      Serial.print(ax_offset); 
      Serial.print("\t");
      Serial.print(ay_offset); 
      Serial.print("\t");
      Serial.print(az_offset); 
      Serial.print("\t");
      Serial.print(gx_offset); 
      Serial.print("\t");
      Serial.print(gy_offset); 
      Serial.print("\t");
      Serial.println(gz_offset); 

    mpu.setXAccelOffset(ax_offset);
    mpu.setYAccelOffset(ay_offset);
    mpu.setZAccelOffset(az_offset);
    mpu.setXGyroOffset(gx_offset);
    mpu.setYGyroOffset(gy_offset);
    mpu.setZGyroOffset(gz_offset);
    digitalWrite(3,HIGH);
  }
  if (digitalRead(10)==LOW) {
    digitalWrite(1,LOW);
    axZero -= 0-((ypr[0] * 180/M_PI)-axZero);
    ayZero -= 0-(((ypr[1] * 180/M_PI)-7)-ayZero);
    azZero -= 0-((ypr[2] * 180/M_PI)-azZero);
    delay(200);
    digitalWrite(1,HIGH);
  }
  delay(10);
}

void meansensors(){
  long i=0,buff_ax=0,buff_ay=0,buff_az=0,buff_gx=0,buff_gy=0,buff_gz=0;

  while (i<(buffersize+101)){
    // read raw accel/gyro measurements from device
    mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
    
    if (i>100 && i<=(buffersize+100)){ //First 100 measures are discarded
      buff_ax=buff_ax+ax;
      buff_ay=buff_ay+ay;
      buff_az=buff_az+az;
      buff_gx=buff_gx+gx;
      buff_gy=buff_gy+gy;
      buff_gz=buff_gz+gz;
    }
    if (i==(buffersize+100)){
      mean_ax=buff_ax/buffersize;
      mean_ay=buff_ay/buffersize;
      mean_az=buff_az/buffersize;
      mean_gx=buff_gx/buffersize;
      mean_gy=buff_gy/buffersize;
      mean_gz=buff_gz/buffersize;
    }
    i++;
    delay(2); //Needed so we don't get repeated measures
  }
}

void calibration(){
  ax_offset=-mean_ax/8;
  ay_offset=-mean_ay/8;
  az_offset=(16384-mean_az)/8;

  gx_offset=-mean_gx/4;
  gy_offset=-mean_gy/4;
  gz_offset=-mean_gz/4;
  while (1){
    int ready=0;
    mpu.setXAccelOffset(ax_offset);
    mpu.setYAccelOffset(ay_offset);
    mpu.setZAccelOffset(az_offset);

    mpu.setXGyroOffset(gx_offset);
    mpu.setYGyroOffset(gy_offset);
    mpu.setZGyroOffset(gz_offset);

    meansensors();

    if (abs(mean_ax)<=acel_deadzone) ready++;
    else ax_offset=ax_offset-mean_ax/acel_deadzone;

    if (abs(mean_ay)<=acel_deadzone) ready++;
    else ay_offset=ay_offset-mean_ay/acel_deadzone;

    if (abs(16384-mean_az)<=acel_deadzone) ready++;
    else az_offset=az_offset+(16384-mean_az)/acel_deadzone;

    if (abs(mean_gx)<=giro_deadzone) ready++;
    else gx_offset=gx_offset-mean_gx/(giro_deadzone+1);

    if (abs(mean_gy)<=giro_deadzone) ready++;
    else gy_offset=gy_offset-mean_gy/(giro_deadzone+1);

    if (abs(mean_gz)<=giro_deadzone) ready++;
    else gz_offset=gz_offset-mean_gz/(giro_deadzone+1);

    if (ready==6) break;
  }
}

also how do i measure the mah it is consuming so i can order the right battery?

2 
  while(digitalRead(14)!=LOW){
    digitalWrite(3,LOW);
    digitalWrite(7,LOW);
    delay(1000);
    digitalWrite(3,HIGH);
    digitalWrite(7,HIGH);
    delay(1000)
  }

I think your code will never get past this loop, and the rest of the code will never run. Because you set pin 14 to INPUT_PULLUP, the pin will never read LOW, so it is an infinite loop. Why did you put this code in?

There is no reason that this infinite loop would cause your circuit to overheat. Same for the rest of the code, if that was running. The overheating must be caused by a problem in your circuit, but you have given no information about that.

3

That's a button, one end is LOW and the other is INPUT_PULLUP. It works fine so there's no inf loop there, do you think its cause I'm sending a UDP packet every 25 milli? Or it could be the led, I have a RGB led and some random resistor i found in-between the positive charge like this.

O

‎ / || \
| | | |
| | " | " = resistor
R G + B

4

There is no button:

5

oh mb lemme edit that

6

There is no resistor:

I suggest you post a schematic. Hand drawn is fine. Don't miss anything out, and double-check it for accuracy before you post it. From your description, I can see no reason why your circuit would get more than slightly warm, so there must be some problem in the circuit we might see in the schematic.

Some bright, clear photos of the circuit may also help. They should be taken from above and make it possible for the forum to check every connection.

There is no point measuring the power consumption of your circuit until the overheating problem is fixed.

There is no way your code can cause the overheating. Optimising your code is a low priority right now.

7

I forgot to add the resistor, its in-between the positive cable of the common anode and pin 5

image
image819×715 120 KB

8

Can you draw a schematic please?

If you don't know what that is, you could Google to see examples. What you posted is a writing diagram. I the forum can't check that because the pin designations are not shown. It looks, for instance, like the buttons are connected between two I/o pins? Why are they not connected between an I/o pin and ground?

9

I don't want to make the button have to cross across the entire board so im jus' using another io pin. Also, i ain't drawin' all that (no offence but its late and i have to wake up early) the pins are pretty readable where the wires stop is the destination. You can find the details of each pin by the rows of pin numbers and other stuff like that.

10

Ok, well in that case my best guess is that you have a short circuit somewhere, and that is causing the overheating. It will also be causing damage, so finding and fixing it should be your top priority.

Use a multimeter on 200mA setting. Remove each wire, one at a time, and replace the wire with the multimeter probes. Measure the current. Then replace the wire and move on to the next wire.

None of the measurements should be more than a couple of mA, except those to the RGB led which can be up to 20mA. Anything higher than that could be your short circuit.

If you don't find anything, try disconnecting everything from the Wemos except the USB power and see how warm it gets. ESP8266 and ESP32 chips do naturally get slightly warm because of the power drawn by the WiFi circuit. But it should never be too hot to hold a finger on. If that's happening, I guess the Wemos must be damaged or faulty.

11

This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.