book collections email follower instructable user

Step 6: Motor Control

Picture of Motor Control

Implementing the code for motor control will involve a bit more logic than the IMU code. This is because it will be receiving its values from the PID algorithm we will be writing later on in the tutorial.

So let's start by updating the header file:

#ifndef Monstro_h
#define Monstro_h

#include "Arduino.h"
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>

class Monstro {

    Monstro(int leftForward, int leftBackward, int leftSpeedPin,
        int rightForward, int rightBackward, int rightSpeedPin,
        int trigA, int echoA, int trigB, int echoB);

    // Behavior
    bool Update();
    void Initialize();

    // IMU
    volatile double xTilt;
    volatile double yTilt;
    volatile double zTilt;
    // IMU
    Adafruit_BNO055 _bno;
    void initializeIMU();
    void readIMU();

    // Motors
    int _leftForward;
    int _leftBackward;
    int _leftSpeedPin;
    int _rightForward;
    int _rightBackward;
    int _rightSpeedPin ;
    void initializeMotors();
    void setMotors(int leftMotorSpeed, int rightMotorSpeed);


The new lines are at the bottom of the header files, under the comment "Motors". We are defining private variables which will hold a reference to the pins which each motor is controlled by (the direction pins, and the speed pins). We have also included two more functions, one to initialize the motors, and one to actually change the speed and direction of the motors.

Now lets update our source file to reflect these changes:

#include "Arduino.h"<br>#include "monstro.h"

Monstro::Monstro(int leftForward, int leftBackward, int leftSpeedPin,
         int rightForward, int rightBackward, int rightSpeedPin,
         int trigA, int echoA, int trigB, int echoB)
    _leftForward = leftForward;
    _leftBackward = leftBackward;
    _leftSpeedPin = leftSpeedPin;
    _rightForward = rightForward;
    _rightBackward = rightBackward;
    _rightSpeedPin = rightSpeedPin;

// Behavior
void Monstro::Initialize() {
bool Monstro::Update() {

// IMU
void Monstro::initializeIMU() {
  _bno = Adafruit_BNO055(55);
  if (!_bno.begin())
    Serial.print("No BNO055 detected");
    while (1);
void Monstro::readIMU() {
  sensors_event_t event;

  xTilt = event.orientation.x;
  yTilt = event.orientation.y;
  zTilt = event.orientation.z;

// Motors
void Monstro::initializeMotors() {
  pinMode(_leftForward, OUTPUT);
  pinMode(_leftBackward, OUTPUT);
  pinMode(_leftSpeedPin, OUTPUT);  
  pinMode(_rightForward, OUTPUT);
  pinMode(_rightBackward, OUTPUT);
  pinMode(_rightSpeedPin, OUTPUT);
void Monstro::setMotors(int leftMotorSpeed, int rightMotorSpeed) {
  if (rightMotorSpeed <= 0) {
    digitalWrite(_rightBackward, LOW);
    digitalWrite(_rightForward, HIGH);
    analogWrite(_rightSpeedPin, abs(rightMotorSpeed));
  else {
    digitalWrite(_rightBackward, HIGH);
    digitalWrite(_rightForward, LOW);
    analogWrite(_rightSpeedPin, rightMotorSpeed);
  if (leftMotorSpeed <= 0) {
    digitalWrite(_leftBackward, LOW);
    digitalWrite(_leftForward, HIGH);
    analogWrite(_leftSpeedPin, abs(leftMotorSpeed));
  else {
    digitalWrite(_leftBackward, HIGH);
    digitalWrite(_leftForward, LOW);
    analogWrite(_leftSpeedPin, leftMotorSpeed);

The updates are the following:

  • We have now assigned our private pin variables values inside of the constructor, so that these can be changed by the user depending on their particular setup.
  • We have added our motor initialization into our general Initialize() function.
  • We have implemented the motor initialization routine, which involves setting the pins to Output.
  • And we have defined our function which will actually actuate the motors setMotors(). Depending on the values passed into this function (-255 to 255) the motors will start spinning at different speeds and in different directions. These values will be generated by the PID algorithm in the next section.