Problem with wrapping Adafruit Motor Controller function calls in a class

Question

I'm working on an Arduino robot and I ran into a problem when reorganizing my code. I have an Adafruit Motor Shield v2.3 connected to an Arduino Uno. My robot can be controlled using a bluetooth connection (BlueSMIRF module) or it can be told to avoid obstacles using an ultra sonic range finder.

Everything worked fine and the robot moved nicely until I decided I wanted to organize the code better, so that my code for moving the robot would be in a separate class, my code for servo movement also in a class as well as my code for the logic for movement (received bluetooth command - do this, or avoid obstacles like so, etc.). Suddenly, none of the motors respond or just one.

I'm not sure if I have a problem with my C++ and passing pointers to my functions or what is going on...

Here is my main .ino file, its relevant parts, I left out stuff that concerns the bluetooth connection etc. And yes, I know the hardcoded include paths look ugly, I'll figure out what to do with includes at some point... :

// bluetoothRobot.ino

#include <Wire.h>
#include <Adafruit_MotorShield.h>
#include <Servo.h>
#include <SonarSRF08.h>
#include <SoftwareSerial.h>  
#include <MemoryFree.h>
#include "utility/Adafruit_PWMServoDriver.h"

#include "\Users\Jarno\Arduino\bluetoothOhjausBot\MotorController.h"
#include "\Users\Jarno\Arduino\bluetoothOhjausBot\AvoidObstacleController.h"
#include "\Users\Jarno\Arduino\bluetoothOhjausBot\MemoryFree-master\MemoryFree.h"

MotorController* motorController;

Adafruit_MotorShield AFMS = Adafruit_MotorShield();

Adafruit_DCMotor *rightRearMotor = AFMS.getMotor(1);
Adafruit_DCMotor *leftRearMotor = AFMS.getMotor(2);
Adafruit_DCMotor *rightFrontMotor = AFMS.getMotor(3);
Adafruit_DCMotor *leftFrontMotor = AFMS.getMotor(4);

void setup() 
{ 
    motorController->start(&AFMS, leftRearMotor, leftFrontMotor, rightFrontMotor, rightRearMotor); 
    AFMS.begin();     
}  

void loop() 
{ 
    if(bluetooth.available())  {
        char command = (char) bluetooth.read();

        if (controlMode == 2) {
            motorController->executeBluetoothCommand(command);
        }
    } 
}

Here is my MotorController.h:

// MotorController.h

#ifndef MotorController_h
#define MotorController_h

#include "Arduino.h"
#include <Wire.h>
#include <Adafruit_MotorShield.h>

class MotorController {
    public:
      int executeBluetoothCommand(char command);
      void start(Adafruit_MotorShield *AFMS, Adafruit_DCMotor *rightRearMotor, Adafruit_DCMotor *leftRearMotor, Adafruit_DCMotor *rightFrontMotor, Adafruit_DCMotor *leftFrontMotor);
      void turnRight();
      void turnLeft();
      void moveForward();
      void moveBackward();
      void stopRobot();
      void setRobotSpeed(int leftWheelsSpeed, int rightWheelsSpeed);   
    private:
      Adafruit_MotorShield* AFMS;
      Adafruit_DCMotor* leftRearMotor;
      Adafruit_DCMotor* rightRearMotor;
      Adafruit_DCMotor* rightFrontMotor;
      Adafruit_DCMotor* leftFrontMotor;
  };

#endif

And here is MotorController.cpp:

// MotorController.cpp

#include "\Users\Jarno\Arduino\bluetoothOhjausBot\MotorController.h"

void MotorController::start(Adafruit_MotorShield *shield, Adafruit_DCMotor *rrm, Adafruit_DCMotor *lrm, Adafruit_DCMotor *rfm, Adafruit_DCMotor *lfm) {
      AFMS = shield;
      rightRearMotor = rrm;
      leftRearMotor = lrm;
      rightFrontMotor = rfm;
      leftFrontMotor = lfm;
      setRobotSpeed(80,80);
  }
int MotorController::executeBluetoothCommand(char command) {
        switch(command) {
            case 'F':
              moveForward(); 
              break;    

            case 'B':
              moveBackward();
              break;       

            case 'L':
              turnLeft();
              break;       

            case 'R':
              turnRight();
              break;     

            case 'S':
              stopRobot();
              break;  

            /*case 'W':
              stopRobot();
              controlMode = 1;
              tiltServoAngle = 120;
              panServoAngle = 90;
              motorController.setPanAndTilt(panServoAngle, tiltServoAngle);
            */
            default:
              return -1;
              }
    return 0;              

}
void MotorController::setRobotSpeed(int leftWheelsSpeed, int rightWheelsSpeed) {
        leftFrontMotor->setSpeed(leftWheelsSpeed);       
        leftRearMotor->setSpeed(leftWheelsSpeed);
        rightRearMotor->setSpeed(rightWheelsSpeed);
        rightFrontMotor->setSpeed(rightWheelsSpeed);
  }
void MotorController::moveForward() {

        setRobotSpeed(80,80);
        leftRearMotor->run(FORWARD);
        rightRearMotor->run(FORWARD);
        leftFrontMotor->run(FORWARD);
        rightFrontMotor->run(FORWARD);


  }

void MotorController::moveBackward() {
        leftRearMotor->run(BACKWARD);
        rightRearMotor->run(BACKWARD);
        rightFrontMotor->run(BACKWARD);
        leftFrontMotor->run(BACKWARD);
        setRobotSpeed(80,80);
  }

void MotorController::turnLeft() {
        leftRearMotor->run(BACKWARD);
        rightRearMotor->run(FORWARD);
        rightFrontMotor->run(FORWARD);
        leftFrontMotor->run(BACKWARD);
        setRobotSpeed(80, 80);
  }

void MotorController::turnRight() {
        leftRearMotor->run(FORWARD);
        rightRearMotor->run(BACKWARD);
        rightFrontMotor->run(BACKWARD);
        leftFrontMotor->run(FORWARD);
        setRobotSpeed(80, 80);

  }

void MotorController::stopRobot() {
        leftRearMotor->run(RELEASE);
        rightRearMotor->run(RELEASE);
        rightFrontMotor->run(RELEASE);
        leftFrontMotor->run(RELEASE);
  }

I used to have a constructor for MotorController with the Motor Shield library AFMS.begin() call in it and all. But this caused the Arduino to constantly reset for some reason. I coudn't understand why but I moved the motor configurations and call to begin() to my .ino file where the problem doesn't occur.

Problem: What happens with the code as it is, is which ever pointer to motor is passed last seems to work and that particular motor turns but the others won't. If I comment out the call to setRobotSpeed in MotorController::start, nothing happens with any motor even though speed is also set in the relevant functions.

As said, the robot worked fine when the code was in my .ino file. Although, for some reason I had to call the Motor Shield library's begin() function always before any other commands to motors. So it was called every time the user told the robot to move forward, move backward etc. Otherwise nothing happened with the motors. Now, if I add a call AFMS->run() to my MotorController.cpp, the Arduino starts to constantly reset...

Also, none of the other stuff (blueSMIRF, the range finder code etc.) should interfere since they were also there when it worked and I even commented out that stuff.

I'm baffled. Maybe its some silly mistake with my C++? Or is there really something weird going on here?

Any help would be much appreciated, I've spent hours trying to figure this thing out! Thanks in advance!

Answer 1

The obvious issue in your current code is that motorController is a pointer to your class but I don't see any place it was instantiated properly.

Hence, it points anywhere the first time you use it, which can explain resets of the MCU.

Now, either you keep it as a pointer:

MotorController* motorController;

And then you instantiate it in place:

MotorController* motorController = new MotorController();

Or you can also instantiate it in setup() as first line (ie before it is used anywhere else):

void setup() {
    motorController = new MotorController();
    ...
}

However, for reasons discussed elsewhere on this site, I would not use dynamic allocation but I'd rather define motorController as an instance of MotorController instead of a pointer:

MotorController motorController;

Now everywhere you were using motorController->xxx you'll have to replace it it with motorController.xxx.

That should remove the most important bug of your current code (I can-'t tell if there are other issues but this one must be fixed in priority).

Answer 2

One thing about instantiating classes before setup() is called is that init() (which you don't see) may undo all of your careful setup.

Your code starts:

Adafruit_DCMotor *rightRearMotor = AFMS.getMotor(1);
Adafruit_DCMotor *leftRearMotor = AFMS.getMotor(2);
Adafruit_DCMotor *rightFrontMotor = AFMS.getMotor(3);
Adafruit_DCMotor *leftFrontMotor = AFMS.getMotor(4);

but these are constructed before setup(). And then you call MotorController::start() with those values. You haven't provided the code for AFMS::getMotor() - so it may be doing things internally that is undone by init().

Instead of creating the whole pointer, why not do this:

const byte rightRearMotor = 1;
const byte leftRearMotor = 2;
const byte rightFrontMotor = 3;
const byte leftFrontMotor = 4;

and then callig AFMS.getMotor() inside MotorController::start():

void MotorController::start(Adafruit_MotorShield *shield,
                            byte rrm, byte lrm, byte rfm, byte lfm) {
    AFMS = shield;
    rightRearMotor = AFMS.getMotor(rrm);
    leftRearMotor = AFMS.getMotor(lrm);
    rightFrontMotor = AFMS.getMotor(rfm);
    leftFrontMotor = AFMS.getMotor(lfm);
    setRobotSpeed(80,80);
}