#include <I2CMotor.h>

// User can change these pins as needed:
#define SDA_PIN 8
#define SCL_PIN 3
#define I2C_FREQ 100000

I2CMotor motor(SDA_PIN, SCL_PIN, I2C_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);
}