Help simplifying nested code

Hi guys, this is my first post to a forum and I am super excited. My project is recreating a seatbelt safety system using an Arduino, buzzer, membrane switch, and gy-521.
The code works great but its far too complicated. I know nesting a bunch of If statements is bad practice. Do you have any suggestions of what I can do make it more simple or just improvements in general?

#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"

const uint8_t buzzer = 3; 
const uint8_t switchPin = 4;

unsigned long currentMillis;
unsigned long previousMillis = 0;
unsigned long previousMillis2 = 0;
unsigned int loopCount = 0;
unsigned int activated = 0;
uint8_t restartCount = 4;
uint8_t xlast = 0;
uint8_t ylast = 0;
uint8_t zlast = 0;
uint8_t previousxlast = 0;
uint8_t previousylast = 0;
uint8_t previouszlast = 0;

MPU6050 mpu;
int16_t ax, ay, az;
int16_t gx, gy, gz;

struct MyData {
  byte X;
  byte Y;
  byte Z;
};
MyData data;


void setup() {
  Serial.begin(9600);
  Wire.begin();
  mpu.initialize();
  DDRD &= ~(1 << switchPin);  //set to 0 to make it an input
  PORTD |= (1 << switchPin);  //set to 1 to turn on the internal pull up resistor
  DDRD |= (1 << buzzer);      //set to output

}

void loop() {

  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  data.X = map(ax, -17000, 17000, 0, 255 );
  data.Y = map(ay, -17000, 17000, 0, 255);
  data.Z = map(az, -17000, 17000, 0, 255);

  currentMillis = millis();

  if ((currentMillis - previousMillis2) >= 2000) {
    previousxlast = xlast;
    previousylast = ylast;
    previouszlast = zlast;
    xlast = data.X;
    ylast = data.Y;
    zlast = data.Z;
    previousMillis2 = currentMillis;
  }

  if ((abs(data.X - xlast) > 4 || abs(data.Y - ylast) > 4) && abs(data.Z - zlast) > 3) {   
    activated = activated + 1;
  }


  if (activated >= 3) {
    if (loopCount < 4) {
      if (((PIND &= (1 << switchPin) >> switchPin) == 0) && currentMillis - previousMillis >= 350) {
        previousMillis = currentMillis;
        noTone(buzzer);

      }
      if (((PIND &= (1 << switchPin) >> switchPin) == 1) && currentMillis - previousMillis >= 5000) {
        previousMillis = currentMillis;
        tone(buzzer, 880, 350);
        loopCount = loopCount + 1;

      }
    }
    if (loopCount >= 4 && loopCount < 35) {
      if (((PIND &= (1 << switchPin) >> switchPin) == 0) && currentMillis - previousMillis >= 350) {
        previousMillis = currentMillis;
        noTone(buzzer);

      }
      if (((PIND &= (1 << switchPin) >> switchPin) == 1) && currentMillis - previousMillis >= 500) {
        previousMillis = currentMillis;
        tone(buzzer, 880, 350);
        loopCount = loopCount + 1;

      }
    }
    if (loopCount >= 40) {
      noTone(buzzer);
    }

    if (abs(data.X - previousxlast) < 2 && abs(data.Y - previousylast) < 2 && abs(data.Z - previouszlast) < 2) {
      restartCount = restartCount - 1;
    }
    if (restartCount == 0) {
      loopCount = 0;
      activated = 0;
      restartCount = 4;
    }
  }

}

Hi, @jimmyjohnsandwitch
Welcome to the forum.

Can you tell us how your code works, that is what is needed from the switch and GY-521 to operate the buzzer and other outputs.
A list of I/O would help, then we can advise.

Thanks.. Tom.. :smiley: :+1: :coffee: :australia:

Please enlighten us why this is bad and how this affects your implementation

Yeah. That's a minor ... essentially insignificant ... problem compared to this...

It's my bedtime. If you haven't spotted the mistake by the time I get up I'll post more.

1 Like

Hi,
If you are nesting, it might be worth looking at switch... case function.
You assign each of your condition combinations a value.
Then use the switch function to select the output response you want.

Tom... :smiley: :+1: :coffee: :australia:

1.

The above codes could be replaced by:

pinMode(switchPin, INPUT_PULLUP);
pinMode(buzzer, OUTPUT);

2.

The above line could be written as:

if (!digitalRead(switchPin) && currentMillis - previousMillis >= 350)`

The only thing I can think of is that

(PIND &= (1 << switchPin) >> switchPin) == 0 

isn't equivalent to:

  PIND = (PIND & (1 << switchPin)) >> switchPin 

Is that it?

Hi Tom :smiley: ,
I added comments throughout the code to explain it and labeled the pins/components
Its meant mimic the chime sequence of a typical modern car except using gerometer (Accelerometer+gyroscope) in place of CANBUS system

#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"

const uint8_t buzzer = 3; //this is to create the beeping when driving and not buckled
const uint8_t switchPin = 4; // switchpin is seatbelt "buckled in" switch

unsigned long currentMillis;
unsigned long previousMillis = 0;
unsigned long previousMillis2 = 0;
unsigned int loopCount = 0;
unsigned int activated = 0;
uint8_t restartCount = 4;
uint8_t xlast = 0;
uint8_t ylast = 0;
uint8_t zlast = 0;
uint8_t previousxlast = 0;
uint8_t previousylast = 0;
uint8_t previouszlast = 0;

MPU6050 mpu;
int16_t ax, ay, az;
int16_t gx, gy, gz;

struct MyData {
  byte X;
  byte Y;
  byte Z;
};
MyData data;


void setup() {
  Serial.begin(9600);
  Wire.begin();
  mpu.initialize();
  DDRD &= ~(1 << switchPin);  //pinMode(switchPin, Input_Pullup) 
  PORTD |= (1 << switchPin);  //sets the internal pullup resistor
  DDRD |= (1 << buzzer);      //pinMode(buzzer, output)

}

void loop() {

  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  data.X = map(ax, -17000, 17000, 0, 255 );
  data.Y = map(ay, -17000, 17000, 0, 255);
  data.Z = map(az, -17000, 17000, 0, 255); //simplifying ranges

  currentMillis = millis();

  if ((currentMillis - previousMillis2) >= 2000) { //every 2 seconds cycle gerometer values
    xlast = data.X;// creating a log of gerometer values, to measure change between cycles
    ylast = data.Y;
    zlast = data.Z;
    previousxlast = xlast; // a second log of gerometer values. Reset count starts counting everytime there is no change above noise between cycles. This creates an additonal buffer to prevent the reset loop.
    previousylast = ylast;
    previouszlast = zlast;
    previousMillis2 = currentMillis; 
  }

  if ((abs(data.X - xlast) > 4 || abs(data.Y - ylast) > 4) && abs(data.Z - zlast) > 3) {   //If significant change in (X or Y axes) and also Y axis is observed, begin activated loop to determine if its just noise or the car is in motion.
    activated = activated + 1;
  }


  if (activated >= 3) { //after 3 loops, the car is determined to be in motion
    if (loopCount < 4) { // the loop count is counting the seatbelt alarm chimes
      if (((PIND &= (1 << switchPin) >> switchPin) == 0) && currentMillis - previousMillis >= 350) { // if(digitalRead(switchPin) == 0  && elapsed time >= noteDuration... 0 = means button is pressed due to internal pull up resistor
        previousMillis = currentMillis;                                                              
        noTone(buzzer); //if seatbelt is connected during warning phase, nothing else happens

      }
      if (((PIND &= (1 << switchPin) >> switchPin) == 1) && currentMillis - previousMillis >= 5000) { //if the seatbelt is not connected, slow reminder chimes every 5 seconds
        previousMillis = currentMillis;
        tone(buzzer, 880, 350);
        loopCount = loopCount + 1;

      }
    }
    if (loopCount >= 4 && loopCount < 40 ) { /after 4 patient chimes, the angry chiming begins until 35 chimes have completed
      if (((PIND &= (1 << switchPin) >> switchPin) == 0) && currentMillis - previousMillis >= 350) { //if seatbelt is attached during the angry chime sequence, nothing else happens
        previousMillis = currentMillis;
        noTone(buzzer);

      }
      if (((PIND &= (1 << switchPin) >> switchPin) == 1) && currentMillis - previousMillis >= 500) { //no seatbelt means angry chiming 
        previousMillis = currentMillis;
        tone(buzzer, 880, 350);
        loopCount = loopCount + 1;

      }
    }
    if (loopCount >= 40) { // after 40 chimes, give up
      noTone(buzzer);
    }

    if (abs(data.X - previousxlast) < 2 && abs(data.Y - previousylast) < 2 && abs(data.Z - previouszlast) < 2) {
      restartCount = restartCount - 1; //if gerometer does not notice any movement, begin a restart cycle
    }
    if (restartCount == 0) { //once restart cycle ==0, reset everything
      loopCount = 0;
      activated = 0;
      restartCount = 4;
    }
  }

}

They are equivalent.

Why are you writing to PIND?

WOW. What a brain fart. I'm embarrassed now :rofl:
Thanks, Jimmy

1 Like

The &= is an assignment operator. Let's look at how the compiler will interpret that.

First, it will evaluate (1<<switchPin)

Then it will take the results of that, and shift it the other way by switchPin bits. The result of this will be a one unless switchpin is greater than the number of bits in an integer. In that case, the result will be zero.

The program will then compare the result of this to zero.

The result of this will be ANDed with the contents of PIND, and the results written back to PIND

Finally, the result of all of that will evaluated as a logical result, and ANDed with the timeout test, and the IF will branch or not, depending on the results.

I'd probably do it as a finite state machine with maybe the following states

Waiting for Car in motion
Car confirmed in motion
In grace period
In warning 1 (slow chime)
In warning 2 (angry chime)
Seat belt OK
Seat belt not OK accepted

The main structure would be that of a switch statement and significantly flatter than a series of nested if statements.

So, just as an example, if the current state is "Seat belt OK" and the person removes the seat belt the state returns to "Car confirmed in motion ". If the car stops, the state returns to "Waiting for car in motion "

1 Like

Thank you for the idea. I love it. I am very limited on memory... how do you think converting into a FSM would affect memory usage? Would you recommend creating the FSM manually with breaks or using the YA_FSM library?

I used your example (loosely) to test (and improve) my own FSM model and put it in a simulation. About half of it is debug/comment so there is scope to slim it down. You can play with it here if you are interested: sketch.ino - Wokwi Arduino and ESP32 Simulator . It is a bit slower than real time. To get it to detect motion, you have to hammer away a bit on the green button. The slide switch is for the seat belt.

1 Like

I am in awe of that. I am in awe of you. That is incredible.

I first started learning how to code 2 months ago. Everyday I have been learning theory, practicing example projects, and reading posts from these forums. I was proud of the code I posted. Its the accumulation of weeks of studying all day and took several days to make and debug. My first project.

It took you a couple of minutes to clean it up and halve the memory used.

I feel amazed by you, overjoyed to see the project be improved, and dismayed that the code I spend days working on and thought was "not perfect but pretty damn good" was actually hot garbage.

Thank you a million for your help and also for humbling me and reminding me that I am a complete novice.

Cheers, Jimmy

That sort of process which you were modelling, in this case the specific example of a car seat belt alarm, is a classic candidate for a finite state machine solution. You identify the states then the conditions for transition from one state to another and any actions to take on those transitions. Usually a good start is to develop a state diagram such as this simple example and annotate it appropriately:

image

The code is then, if you use a model similar to the one I posted, more or less a 1:1 representation of the state diagram.

Incidentally, it did take more than a few minutes to code but I was keen to test/improve my own variant a general solution and put it into a simulator. However, the identification of the states from the original description was indeed quick.

In general, for the best learning experience, it is better to code things like that yourself instead of using a standard ready made library. Then you learn how to solve the problem, not how to use a library.

1 Like

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