#include "I2CMotorMaster.h"

I2CMotorMaster motor(8, 3, 100000); // SDA, SCL, Freq

void setup() {
    Serial.begin(115200);
    motor.begin();

    if(!motor.found()) {
      Serial.println("No motor controller found. Halting.");
      while (1);
    }

    motor.identify();

    // Set device name and min/max PWM
    motor.setDeviceName("FM");
    motor.setMinMax(40, 255);

    Serial.println("All tests complete.");
}

void loop() {
    
    // Set direction forward and test speeds
    motor.setMotorDirection(1, 1); // Motor 1, forward
    motor.setMotorSpeed(1, 50);
    delay(2000);
    motor.setMotorSpeed(1, 60);
    delay(2000);
    motor.setMotorSpeed(1, 80);
    delay(2000);
    motor.setMotorSpeed(1, 100);
    delay(2000);

    // Set direction backward and test speeds
    motor.setMotorDirection(1, 2); // Motor 1, backward
    motor.setMotorSpeed(1, 50);
    delay(2000);
    motor.setMotorSpeed(1, 60);
    delay(2000);
    motor.setMotorSpeed(1, 80);
    delay(2000);
    motor.setMotorSpeed(1, 100);
    delay(2000);

    // Stop
    motor.emergencyStop();
    delay(2000);

}