Friday, November 16, 2012

Simple motor stall detector

Even with four corner IR obstacle detectors and the forward facing ultrasonic sensor there are still circumstances where the motors will be stalled and the 'bot should stop.  I did not want, or need, the complexity of an interrupt driven encoder routine so wrote the following simple code to detect a stalled situation:

First I needed to know if we are moving.  Obviously, as for other code posts, I am using my interface library to talk to the Arduino.   Note also that I am using a dictionary structure to pass context (like pin assignments) around the modules that comprise the 'bot.   Not great practice but easy for what I wanted to accomplish.

    def moving(self):
        """
        Returns the true if we are moving and false if not.   Checks changes

        in the encoders if no change is detected over 'n' iterations then
        no movement is indicated
        """
        try:
            self.movingIteration += 1
        except:
            self.movingIteration = 1          
            self.lastMovingIteration = 1
            self.notMoving = 0
            self.lastEnc1 = 0
            self.lastEnc2 = 0

        enc1 = int(self.utility.arduinoReturn(
            self.arduino.analogRead(self.intercom["pins"]["propulsionLeftEnc1"])))
        enc2 = int(self.utility.arduinoReturn(
            self.arduino.analogRead(self.intercom["pins"]["propulsionLeftEnc2"])))

        # Count the number of times the encoders have not shown a change
        if self.lastEnc1 == enc1 and self.lastEnc2 == enc2:
            if self.lastMovingIteration == self.movingIteration - 1:
                self.notMoving += 1
            else:
                self.notMoving = 0
  
        # Probability is pretty low that we would not encounter the same combination
        # of readings five times!
        if self.notMoving == 6:

             self.notMoving = 0
             return False
      
        self.lastEnc1 = enc1
        self.lastEnc2 = enc2
        self.lastMovingIteration = self.movingIteration

        return True


Obviously there is at least one issue with the approach that I am using...namely that I am only looking at one tread.   We could get ourselves into a predicament where the left tread is free wheeling but the right is stalled...!  Oh well.

Anyway, if we know that we are not moving we also need to know if we are supposed to be!  My test for this is to look for current flowing to the two motors:

    def currentLeft(self):
        """
        Returns the current for the left motor
        """
        self.leftCurrent =  int(self.utility.arduinoReturn
            (self.arduino.analogRead(self.intercom["pins"]

                ["propulsionLeftCur"])))
        return self.leftCurrent


Obviously there are two of these but I am only showing the one.

The test for a stalled condition is then a marriage of the above methods:

    def stalled(self):
        # Is current going to the motors?
        if self.currentLeft() > 25 or self.currentRight() > 25:
            # Are we moving?  Is this the second time in a row?   

            # If so we are stalled
            if not self.moving():
                return True
               
        return False


I use 25 rather than 0 as there appears to be some residual current flow for some amount of time after the motor is shut down.

No comments:

Post a Comment