Issue stopping motor using current sense library

I’m using the INA219 to sense current on a motor connected to an opening and closing door.
For some reason, the code below stops the motor when the current spikes, but does not “end” the cycle in the state machine. Can anyone see what I’m doing wrong? I’ve tried changing the format, adding breaks, etc, but I can’t get it to work how I want.

#include <Wire.h>
#include <Adafruit_INA219.h>

Adafruit_INA219 ina219;

#define MOTOR_IN1 13
#define MOTOR_IN2 12
float current_mA = 0;

const int limit1 = 40;
const int sensor = 32;
const int limit2 = 36;
const int button = 41;
const int greenLED = 44;
const int redLED = 48;
const int jumper = 26;

#define GREENPIN 3
#define REDPIN 4
#define BLUEPIN 6

//int phoneTrip;
int buttonPressed;
int eot1;
int eot2;
int power1 = 0;

#define SENSORPIN1 47
#define SENSORPIN2 43
#define SENSORPIN3 37
int sensorState1 = 0;
int sensorState2 = 0;
int sensorState3 = 0;

void setup() {
  Serial.begin(115200);
  
  pinMode(sensor, INPUT_PULLUP);
  pinMode(limit1, INPUT_PULLUP);
  pinMode(limit2, INPUT_PULLUP);
  pinMode(greenLED, OUTPUT);
  pinMode(button, INPUT_PULLUP);
  pinMode(redLED, OUTPUT);
  pinMode(jumper, INPUT_PULLUP);
  pinMode(MOTOR_IN1, OUTPUT);
  pinMode(MOTOR_IN2, OUTPUT);
  
  pinMode(REDPIN, OUTPUT);
  pinMode(GREENPIN, OUTPUT);
  pinMode(BLUEPIN, OUTPUT);

  analogWrite(REDPIN,0);
  analogWrite(GREENPIN,0);
  analogWrite(BLUEPIN,0);

  pinMode(SENSORPIN1, INPUT);     
  digitalWrite(SENSORPIN1, HIGH); // turn on the pullup
  
  pinMode(SENSORPIN2, INPUT);     
  digitalWrite(SENSORPIN2, HIGH);
  
  pinMode(SENSORPIN3, INPUT);     
  digitalWrite(SENSORPIN3, HIGH);
  
 uint32_t currentFrequency; 
 ina219.begin();

 analogWrite(MOTOR_IN1,255);
 analogWrite(MOTOR_IN2,255);
  //motorHome();
}
void loop() {
  // put your main code here, to run repeatedly:
  engageSystem();
}

void engageSystem(){
  //phoneTrip = digitalRead(sensor);
  buttonPressed = digitalRead(button);
  eot1 = digitalRead(limit1);
  eot2 = digitalRead(limit2);
  digitalWrite(greenLED, buttonPressed);

  sensorState1 = digitalRead(SENSORPIN1);
  sensorState2 = digitalRead(SENSORPIN2);
  sensorState3 = digitalRead(SENSORPIN3);

  digitalWrite(redLED,1);
  
  static enum {CLOSED,OPENING_1,OPENING_2,OPEN,CLOSING_1,CLOSING_2} state;
  static unsigned long StartOpenTime;
  static unsigned long StartCloseTime;
  const unsigned long AccelerationDuration = 1000; //milliseconds - the time it takes to power up from zero to max
  const int MaxPower = 255; //maximum motor power in the normal opening state
  switch (state) {
    case CLOSED:
      if (buttonPressed == 1){ //if the sensor is tripped
        analogWrite(REDPIN,255);
        analogWrite(GREENPIN,255);
        analogWrite(BLUEPIN,255);
        state = OPENING_1;
        StartOpenTime = millis();
      }
      break;
    case OPENING_1:
      power1 = map(millis()-StartOpenTime, 0, AccelerationDuration, 1, MaxPower);
      analogWrite(MOTOR_IN1,power1);
      analogWrite(MOTOR_IN2,0);
      if (power1 >= MaxPower){
        state = OPENING_2;
      }
        
        if (eot1 == 1){
          analogWrite(MOTOR_IN1,255);
          analogWrite(MOTOR_IN2,255);
          state = OPEN;
        }
      break;
    case OPENING_2:
        analogWrite(MOTOR_IN1,255);
        analogWrite(MOTOR_IN2,0);
        
        if (eot1 == 1){
          analogWrite(MOTOR_IN1,255);
          analogWrite(MOTOR_IN2,255);
          state = OPEN;
        }
      break;
    case OPEN:
        
      if(sensorState1 == HIGH && sensorState2 == HIGH && sensorState3 == HIGH){
        delay(5000);
        state = CLOSING_1;
        StartCloseTime  = millis();
      }
      break;
    case CLOSING_1:
    
    analogWrite(REDPIN,0);
    analogWrite(GREENPIN,0);
    analogWrite(BLUEPIN,0);
    
     power1 = map(millis()-StartCloseTime, 0, AccelerationDuration, 1, MaxPower);
     analogWrite(MOTOR_IN1,0);
     analogWrite(MOTOR_IN2,power1);
      if (power1 >= MaxPower){
        state = CLOSING_2;
      }       
        if (eot2 == 1){
          analogWrite(MOTOR_IN1,255);
          analogWrite(MOTOR_IN2,255);
          
          state = CLOSED;
        }
      break;
      
    case CLOSING_2:
        current_mA = ina219.getCurrent_mA();
    
        while (abs(current_mA) < 100){
        analogWrite(MOTOR_IN1,0);
        analogWrite(MOTOR_IN2,255);
        current_mA = ina219.getCurrent_mA();
        Serial.println(current_mA);
        
         if (eot2 == 1){
          analogWrite(MOTOR_IN1,255);
          analogWrite(MOTOR_IN2,255);
          
          state = CLOSED;
          }          
        }
        
        analogWrite(MOTOR_IN1,255);
        analogWrite(MOTOR_IN2,255);
      state = OPEN;
     break;
 
  }
  digitalWrite(redLED,0);
}

your state machine is weird

you don’t really need OPENING_2 nor CLOSING_2, as this is just to check if power1 >= MaxPower and then use MaxPower instead of power1 in the analogWrite(MOTOR_IN2, power1);

you could just cap power1 to MaxPower after doing your map function

      power1 = map(millis() - StartOpenTime, 0, AccelerationDuration, 1, MaxPower);
      if (power1 >= MaxPower) power1 = MaxPower;

and get rid of those 2 states.

why do you keep setting analogWrite(MOTOR_IN2, 0); for example at each state OPENING_1?

I’m not sure why you do this:

      if (eot1 == 1) {
        analogWrite(MOTOR_IN1, [color=red]255[/color]);
        analogWrite(MOTOR_IN2, [color=red]255[/color]);
        state = OPEN;
      }

shouldn’t you stop the motors instead when the door is OPEN?

you only check the current in CLOSING_2 state but if (eot2 == 1) while you are in CLOSING_1 then you won’t check for the current, is that a pb? are you 100% sure you’ll get to CLOSING_2 state?

in CLOSING_2 state, instead of doing a if and let the loop and state machine do their magic, you have a while…       while (abs(current_mA) < 100) { that’s weird. why would you do that? and why in that while do you keep telling the motors

        analogWrite(MOTOR_IN1, 0);
        analogWrite(MOTOR_IN2, 255);

doing that once when you set the state to CLOSING_2 would be enough

Still in that state it is weird you test if (eot2 == 1) { then you say state = CLOSED; but if you exit the while loop and you were closing (state was CLOSING_2) then you say state = OPEN;… Shouldn’t that be closed? or maintain state at CLOSING_2 until if (eot2 == 1) { does it job?

I think you only need 4 stages:

CLOSED
OPENING
OPENED
CLOSING

I’m unsure of what all the sensors are, but if they are meaningful then they define transitions to new states or actions while in a state.

I would suggest you take a piece of paper and draw the 4 stages and put arrows on what event in a given state triggers the transitions from one step to the other.

I see what you are saying about having extra states, but it works the way I have it so I don’t want to start messing with it unless I need to.

Sending 255 to both the motor inputs is actually a brake command with the motor controller I’m using.

I’ve gotten the current sense to work in the CLOSING_2 state by doing as you suggested, but for some reason, if I use that if statement in other states, it doesn’t work?

#include <Wire.h>
#include <Adafruit_INA219.h>

Adafruit_INA219 ina219;

#define MOTOR_IN1 13
#define MOTOR_IN2 12
float current_mA = 0;

const int limit1 = 40;
const int sensor = 32;
const int limit2 = 36;
const int button = 41;
const int greenLED = 44;
const int redLED = 48;
const int jumper = 26;

#define GREENPIN 3
#define REDPIN 4
#define BLUEPIN 6

//int phoneTrip;
int buttonPressed;
int eot1;
int eot2;
int power1 = 0;
int check = 0;

#define SENSORPIN1 47
#define SENSORPIN2 43
#define SENSORPIN3 37
int sensorState1 = 0;
int sensorState2 = 0;
int sensorState3 = 0;

void setup() {
  Serial.begin(115200);
  
  pinMode(sensor, INPUT_PULLUP);
  pinMode(limit1, INPUT_PULLUP);
  pinMode(limit2, INPUT_PULLUP);
  pinMode(greenLED, OUTPUT);
  pinMode(button, INPUT_PULLUP);
  pinMode(redLED, OUTPUT);
  pinMode(jumper, INPUT_PULLUP);
  pinMode(MOTOR_IN1, OUTPUT);
  pinMode(MOTOR_IN2, OUTPUT);
  
  pinMode(REDPIN, OUTPUT);
  pinMode(GREENPIN, OUTPUT);
  pinMode(BLUEPIN, OUTPUT);

  analogWrite(REDPIN,0);
  analogWrite(GREENPIN,0);
  analogWrite(BLUEPIN,0);

  pinMode(SENSORPIN1, INPUT);     
  digitalWrite(SENSORPIN1, HIGH); // turn on the pullup
  
  pinMode(SENSORPIN2, INPUT);     
  digitalWrite(SENSORPIN2, HIGH);
  
  pinMode(SENSORPIN3, INPUT);     
  digitalWrite(SENSORPIN3, HIGH);
  
 uint32_t currentFrequency; 
 ina219.begin();

 analogWrite(MOTOR_IN1,255);
 analogWrite(MOTOR_IN2,255);
}
void loop() {
  engageSystem();
}

void engageSystem(){
  //phoneTrip = digitalRead(sensor);
  buttonPressed = digitalRead(button);
  eot1 = digitalRead(limit1);
  eot2 = digitalRead(limit2);
  digitalWrite(greenLED, buttonPressed);

  sensorState1 = digitalRead(SENSORPIN1);
  sensorState2 = digitalRead(SENSORPIN2);
  sensorState3 = digitalRead(SENSORPIN3);

  digitalWrite(redLED,1);
  
  static enum {CLOSED,OPENING_1,OPENING_2,OPEN,CLOSING_1,CLOSING_2} state;
  static unsigned long StartOpenTime;
  static unsigned long StartCloseTime;
  const unsigned long AccelerationDuration = 1000; //milliseconds - the time it takes to power up from zero to max
  const int MaxPower = 255; //maximum motor power in the normal opening state
  
  switch (state) {
    case CLOSED:
      if (buttonPressed == 1){ //if the sensor is tripped
        analogWrite(REDPIN,255);
        analogWrite(GREENPIN,255);
        analogWrite(BLUEPIN,255);
        StartOpenTime = millis();
        state = OPENING_1;
        
      }
      break;
    case OPENING_1:
      power1 = map(millis()-StartOpenTime, 0, AccelerationDuration, 1, MaxPower);
      analogWrite(MOTOR_IN1,power1);
      analogWrite(MOTOR_IN2,0);
      if (power1 >= MaxPower){
        state = OPENING_2;
      }
        
        if (eot1 == 1){
          analogWrite(MOTOR_IN1,255);
          analogWrite(MOTOR_IN2,255);
          state = OPEN;
        }
      break;
    case OPENING_2:
        analogWrite(MOTOR_IN1,255);
        analogWrite(MOTOR_IN2,0);
        
        if (eot1 == 1){
          analogWrite(MOTOR_IN1,255);
          analogWrite(MOTOR_IN2,255);
          state = OPEN;
        }
      break;
    case OPEN:
        
      if(sensorState1 == HIGH && sensorState2 == HIGH && sensorState3 == HIGH){
        delay(5000);
        StartCloseTime  = millis();
        state = CLOSING_1;
        
      }
      break;
      
    case CLOSING_1:
    
    analogWrite(REDPIN,0);
    analogWrite(GREENPIN,0);
    analogWrite(BLUEPIN,0);
    
     power1 = map(millis()-StartCloseTime, 0, AccelerationDuration, 1, MaxPower);
     analogWrite(MOTOR_IN1,0);
     analogWrite(MOTOR_IN2,power1);
     
     current_mA = ina219.getCurrent_mA();
     
      if (power1 >= MaxPower){
        state = CLOSING_2;
      }
             
      if (eot2 == 1){
         analogWrite(MOTOR_IN1,255);
         analogWrite(MOTOR_IN2,255);
         state = CLOSED;
      }
      
      break;
      
    case CLOSING_2:
        current_mA = ina219.getCurrent_mA();
        analogWrite(MOTOR_IN1,0);
        analogWrite(MOTOR_IN2,255);
        
        if (abs(current_mA) > 150){
          analogWrite(MOTOR_IN1,255);
          analogWrite(MOTOR_IN2,255);
          state = OPEN;
          break;
        }
          
        
        if (eot2 == 1){
            analogWrite(MOTOR_IN1,255);
            analogWrite(MOTOR_IN2,255);
            state = CLOSED;
            break;
        }          
   }
digitalWrite(redLED,0);         
}

What is this

 uint32_t currentFrequency;

in setup() for?

void loop() {
  engageSystem();
}

It is pointless to have all the code in one function called by loop().

  eot1 = digitalRead(limit1);

The digitalRead() function returns HIGH or LOW. Use those names everywhere in the code where you compare eot1 or eot2 to something else.

#define SENSORPIN1 47
#define SENSORPIN2 43
#define SENSORPIN3 37
int sensorState1 = 0;
int sensorState2 = 0;
int sensorState3 = 0;

You do NOT have generic sensors. Use names that reflect the kind of sensors you have connected.

I've gotten the current sense to work in the CLOSING_2 state by doing as you suggested, but for some reason, if I use that if statement in other states, it doesn't work?

What if statement? Are you also reading the current sensor in the other states? Is that a question, or a statement? Statements do not end with question marks.

Why are you not measuring the current regardless of state?

"uint32_t currentFrequency;" is in the example sketch for using the current sense. I don't know what it does so I left it in.

I have it written like this because I am going to use this in another sketch with other functions, this way I can just copy and paste it.

I don't understand why you must be so critical of my naming schemes and use of language. I'm looking for help with my code, if theres something you don't understand about my question, you can ask for clarification. I don't need to be talked down to just because i'm not a professional electrical engineer.

I don't understand why you must be so critical of my naming schemes

I am critical of poor names, regardless of who writes the code.

If you need to use the value obtained, for instance, from a temperature sensor, does it make sense to store the value in a variable called sensorValue1? Or, does it make more sense to store the value in a variable called temperature?

What happens when you add a second sensor, to measure current. Using sensorValue1 and sensorValue2, you have to be very careful to use the correct name when comparing the measured current to some threshold. When using temperature and current, there is far less likely a chance of making a typo resulting in using the wrong variable's value.

I don't know what it does so I left it in.

How can you not know that that is declaring a local variable that never gets used?

edit*
The code below works the way I want it to. I changed the location of the “break;” in OPENING_2.
Can someone possibly explain why the break has to be in different locations in the two states? I’m hesitant to try and add the current sense to the other two moving states before I understand whats going on better.

#include <Wire.h>
#include <Adafruit_INA219.h>

Adafruit_INA219 ina219;

#define MOTOR_IN1 13
#define MOTOR_IN2 12
float current_mA = 0;

const int limit1 = 40;
const int sensor = 32;
const int limit2 = 36;
const int button = 41;
const int greenLED = 44;
const int redLED = 48;
const int jumper = 26;

#define GREENPIN 3
#define REDPIN 4
#define BLUEPIN 6

//int phoneTrip;
int buttonPressed;
int eot1;
int eot2;
int power1 = 0;
int check = 0;

#define SENSORPIN1 47
#define SENSORPIN2 43
#define SENSORPIN3 37
int sensorState1 = 0;
int sensorState2 = 0;
int sensorState3 = 0;

void setup() {
  Serial.begin(115200);
  
  pinMode(sensor, INPUT_PULLUP);
  pinMode(limit1, INPUT_PULLUP);
  pinMode(limit2, INPUT_PULLUP);
  pinMode(greenLED, OUTPUT);
  pinMode(button, INPUT_PULLUP);
  pinMode(redLED, OUTPUT);
  pinMode(jumper, INPUT_PULLUP);
  pinMode(MOTOR_IN1, OUTPUT);
  pinMode(MOTOR_IN2, OUTPUT);
  
  pinMode(REDPIN, OUTPUT);
  pinMode(GREENPIN, OUTPUT);
  pinMode(BLUEPIN, OUTPUT);

  analogWrite(REDPIN,0);
  analogWrite(GREENPIN,0);
  analogWrite(BLUEPIN,0);

  pinMode(SENSORPIN1, INPUT);     
  digitalWrite(SENSORPIN1, HIGH); // turn on the pullup
  
  pinMode(SENSORPIN2, INPUT);     
  digitalWrite(SENSORPIN2, HIGH);
  
  pinMode(SENSORPIN3, INPUT);     
  digitalWrite(SENSORPIN3, HIGH);
  
 ina219.begin();

 analogWrite(MOTOR_IN1,255);
 analogWrite(MOTOR_IN2,255);
}

void loop(){
  buttonPressed = digitalRead(button);
  eot1 = digitalRead(limit1);
  eot2 = digitalRead(limit2);
  digitalWrite(greenLED, buttonPressed);

  sensorState1 = digitalRead(SENSORPIN1);
  sensorState2 = digitalRead(SENSORPIN2);
  sensorState3 = digitalRead(SENSORPIN3);

  digitalWrite(redLED,1);
  
  static enum {CLOSED,OPENING_1,OPENING_2,OPEN,CLOSING_1,CLOSING_2} state;
  static unsigned long StartOpenTime;
  static unsigned long StartCloseTime;
  const unsigned long AccelerationDuration = 1000; //milliseconds - the time it takes to power up from zero to max
  const int MaxPower = 255; //maximum motor power in the normal opening state
  
  switch (state) {
    case CLOSED:
      if (buttonPressed == 1){ //if the sensor is tripped
        analogWrite(REDPIN,255);
        analogWrite(GREENPIN,255);
        analogWrite(BLUEPIN,255);
        StartOpenTime = millis();
        state = OPENING_1;
        
      }
      break;
      
    case OPENING_1:
      power1 = map(millis()-StartOpenTime, 0, AccelerationDuration, 1, MaxPower);
      analogWrite(MOTOR_IN1,power1);
      analogWrite(MOTOR_IN2,0);
      if (power1 >= MaxPower){
        state = OPENING_2;
      }
        
      if (eot1 == HIGH){
          analogWrite(MOTOR_IN1,255);
          analogWrite(MOTOR_IN2,255);
          state = OPEN;
      }
      break;
      
    case OPENING_2:
        current_mA = ina219.getCurrent_mA();
        analogWrite(MOTOR_IN1,255);
        analogWrite(MOTOR_IN2,0);
                        
        if (eot1 == HIGH){
          analogWrite(MOTOR_IN1,255);
          analogWrite(MOTOR_IN2,255);
          state = OPEN;
          
        }
        
        if (abs(current_mA) > 150){
          analogWrite(MOTOR_IN1,255);
          analogWrite(MOTOR_IN2,255);
          state = OPEN;
        }
        
      break;
    case OPEN:
        
      if(sensorState1 == HIGH && sensorState2 == HIGH && sensorState3 == HIGH){
        delay(5000);
        StartCloseTime  = millis();
        state = CLOSING_1;
        
      }
      break;
      
    case CLOSING_1:
    
    analogWrite(REDPIN,0);
    analogWrite(GREENPIN,0);
    analogWrite(BLUEPIN,0);
    
     power1 = map(millis()-StartCloseTime, 0, AccelerationDuration, 1, MaxPower);
     analogWrite(MOTOR_IN1,0);
     analogWrite(MOTOR_IN2,power1);
     
     current_mA = ina219.getCurrent_mA();
     
      if (power1 >= MaxPower){
        state = CLOSING_2;
      }
             
      if (eot2 == HIGH){
         analogWrite(MOTOR_IN1,255);
         analogWrite(MOTOR_IN2,255);
         state = CLOSED;
      }
      
      break;
      
    case CLOSING_2:
        current_mA = ina219.getCurrent_mA();
        analogWrite(MOTOR_IN1,0);
        analogWrite(MOTOR_IN2,255);
        
        if (abs(current_mA) > 150){
          analogWrite(MOTOR_IN1,255);
          analogWrite(MOTOR_IN2,255);
          state = OPEN;
          break;
        }
          
        
        if (eot2 == HIGH){
            analogWrite(MOTOR_IN1,255);
            analogWrite(MOTOR_IN2,255);
            state = CLOSED;
            break;
        }          
   }
digitalWrite(redLED,0);         
}

The break statement defines the end of processing for a case. The usual position is right at the end of the case.

If you have other break statements, that are not in while statements, they will seem to prematurely end the case.

When I write CLOSING_2 as such, the system seems to work correctly:

case CLOSING_2:
        current_mA = ina219.getCurrent_mA();
        analogWrite(MOTOR_IN1,0);
        analogWrite(MOTOR_IN2,255);
        
        if (abs(current_mA) > 150){
          analogWrite(MOTOR_IN1,255);
          analogWrite(MOTOR_IN2,255);
          state = OPEN;
          break;
        }
          
        
        if (eot2 == HIGH){
            analogWrite(MOTOR_IN1,255);
            analogWrite(MOTOR_IN2,255);
            state = CLOSED;
            break;
        }

But if i simply move the break to the end of the case, it somehow effects the entire the state machine, and does not work correctly anymore. But the opening state is the opposite. Is there an explaination for this?

case CLOSING_2:
        current_mA = ina219.getCurrent_mA();
        analogWrite(MOTOR_IN1,0);
        analogWrite(MOTOR_IN2,255);
        
        if (abs(current_mA) > 150){
          analogWrite(MOTOR_IN1,255);
          analogWrite(MOTOR_IN2,255);
          state = OPEN;
                  }
          
        
        if (eot2 == HIGH){
            analogWrite(MOTOR_IN1,255);
            analogWrite(MOTOR_IN2,255);
            state = CLOSED;
           
        }
break;

In the first block of code, the break in the if (abs(current_mA) > 150) body prevents the if (eot2 == HIGH) statement from being reached. In the second block of code, that statement is reached, and evaluated, and the body may, or may not, be executed.

The flow of execution is not the same.

Why is it that the current check prevents the second if statement from being reached, but not the other way around?

Also, if the first block I have posted supposedly stops the eot2==HIGH check from being reached, why does that work correctly and the second block doesnt?

Why is it that the current check prevents the second if statement from being reached, but not the other way around?

The current check statement only prevents the second if statement from being evaluated if:

  1. the current value exceeds the threshold
  2. the body contains a break statement

Also, if the first block I have posted supposedly stops the eot2==HIGH check from being reached

The first snippet did not stop the if(eot2 == HIGH) statement from being reached, and evaluated.