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

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

int distancemax;
int pause;
int vitesse;

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 avancer() {
  while (ultrasonic_3.distanceCm() >= distancemax) {
    mBot_setMotorRight(1, vitesse);
    mBot_setMotorLeft(1, vitesse);
  }
  mBot_setMotorRight(0, 0);
  mBot_setMotorLeft(0, 0);
  delay(1000*pause);
}

void droite() {
  mBot_setMotorRight(-1, vitesse);
  mBot_setMotorLeft(1, vitesse);
  delay(1000*pause);
  mBot_setMotorRight(0, 0);
  mBot_setMotorLeft(0, 0);
}

void gauche() {
  mBot_setMotorLeft(-1, vitesse);
  mBot_setMotorRight(1, vitesse);
  delay(1000*pause);
  mBot_setMotorRight(0, 0);
  mBot_setMotorLeft(0, 0);
}


void setup() {
  distancemax = 3;
  pause = 1;
  vitesse = 22;
  avancer();
  droite();
  avancer();
  gauche();
  avancer();
  gauche();
  avancer();
  droite();
  mBot_setMotorRight(1, 50);
  mBot_setMotorLeft(1, 50);
  delay(1000*1);
  mBot_setMotorRight(0, 0);
  mBot_setMotorLeft(0, 0);
}

void loop() {
}