Wemos Motor Shield blocking other I2C calls?

Hello, I realise that this firmware may well be unmaintained now, however I think I've hit a bug and I'd really appreciate some help if possible!

I'm working on a project that takes the output of an INA219 Breakout Board and uses it to drive devices connected to the original Wemos D1 Motor Shield (although I've updated the firmware to this version to avoid I2C crashes).

I've got both devices connected to an Arduino Nano, because for this project I need more I/O than the D1 mini can provide.

The devices are connected to the SCL/SDA ports A4 and A5, and a scan of the I2C bus reveals two devices - the INA219 on 0x40, and the motor shield on 0x30 as expected from the docs. I also have a servo connected to D9.

The code looks as follows:

#include "WEMOS_Motor.h"
#include <Wire.h>
#include <Adafruit_INA219.h> // You will need to download this library
#include <Servo.h>

int pwm = 0;

//Motor shield I2C Address: 0x30
//PWM frequency: 1000Hz(1kHz)
Motor M1(0x30,_MOTOR_A, 1000);//Motor A
Motor M2(0x30,_MOTOR_B, 1000);//Motor B

Servo s1;
Adafruit_INA219 sensor219; // Declare and instance of INA219

void setup(void) 
{
  s1.attach(9);
  s1.write(90);
  delay(100);
  s1.write(0);
  Serial.begin(9600);    
  Serial.println("Setup complete, waiting instructions...");
  delay(1000);
  sensor219.begin();
}

void loop(void) 
{
  float busVoltage = 0;
  float current = 0; // Measure in milli amps
  float power = 0;

  busVoltage = sensor219.getBusVoltage_V();
  current = sensor219.getCurrent_mA();
  power = busVoltage * (current/1000); // Calculate the Power

  int s1cur = s1.read();
  if (busVoltage < 8.00 && s1cur < 128){
    s1.write(s1cur + 1);
    pwm = pwm + 1;
    M1.setmotor( _CW, pwm);
    M2.setmotor(_CCW, pwm);
    Serial.println("Increasing");
  }
  else if (busVoltage > 8.00 && s1cur > 0){
    s1.write(s1cur - 1);
    pwm = pwm - 1;
    M1.setmotor( _CW, pwm);
    M2.setmotor(_CCW, pwm);
    Serial.println("Decreasing");
  }

  Serial.print("Bus Voltage:   "); 
  Serial.print(busVoltage); 
  Serial.println(" V");  

  Serial.print("Servo Position: ");
  Serial.println(s1cur);
/*  
  Serial.print("Current:       "); 
  Serial.print(current); 
  Serial.println(" mA");

  Serial.print("Power:         "); 
  Serial.print(power); 
  Serial.println(" W");  

  Serial.println("");  */
  delay(50);
}

The idea behind it is that as the current drops, the motors activate and slowly get faster, whilst the servo swings to a set position. As the current rises, the motors slow down, and the servo drifts back toward 0°.

The code works fine, as long as I keep the Motor M1(0x30,_MOTOR_A, 1000);//Motor A and equivalent for "Motor B" commented out. As soon as I uncomment those lines, the whole setup hangs, and it appears to do so when trying to initialise the INA219.

Obviously this is sub-optimal, as it means I can only have the Servo working and not the motors as intended.

I'm wondering if there's something in the Wemos Library that is causing the I2C bus to crash, but my knowledge of these things isn't sufficient to know where to start looking for a solution!

All and any help is appreciated.

pbugalski / wemos_motor_shield

Wemos Motor Shield blocking other I2C calls? #14