#include <MeMCore.h>
#include <Arduino.h>
#include <Wire.h>
#include <SoftwareSerial.h>

// Ultrasonic on PORT_3
MeUltrasonicSensor ultrasonic_3(PORT_3);
MeDCMotor motor_L(9);
MeDCMotor motor_R(10);

void mBot_setMotorLeft(int8_t dir, int16_t speed) {
  speed = speed/100.0*255;
  motor_L.run((9) == M1 ? -(dir*speed) : (dir*speed));
}

void mBot_setMotorRight(int8_t dir, int16_t speed) {
  speed = speed/100.0*255;
  motor_R.run((10) == M1 ? -(dir*speed) : (dir*speed));
}


void setup() {
}

void loop() {
  if (ultrasonic_3.distanceCm() > 5) {
    mBot_setMotorRight(1, 25);
    mBot_setMotorLeft(1, 25);
  } else {
    mBot_setMotorRight(0, 0);
    mBot_setMotorLeft(0, 0);
  }
}