#include "Arduino.h"
#include "Wire.h"
#include "MeMCore.h"
#include "SoftwareSerial.h"

MeDCMotor _leftMotor(9);
MeDCMotor _rightMotor(10);
MeUltrasonicSensor ultrasonicSensor(PORT_3);
MeGyro gyro;

double dis;
double ang;
double oldAng;

void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  gyro.begin();
  _leftMotor.run(0);
  _rightMotor.run(0);
  

}

void loop() {
  // put your main code here, to run repeatedly:
  
  for (int i=0; i!=4 ;i++){
    dis=ultrasonicSensor.distanceCm();
    while (ultrasonicSensor.distanceCm()>dis-30){
      _leftMotor.run(-100);
      _rightMotor.run(100);
      //Serial.println(ultrasonicSensor.distanceCm());
    }
    _leftMotor.stop();
    _rightMotor.stop();
    delay(1000);
    ang=gyro.getAngleZ();
    oldAng=gyro.getAngleZ();
    while (abs(ang-gyro.getAngleZ())<90){
      _leftMotor.run(-100);
      _rightMotor.run(-100);
      gyro.update();
      oldAng=gyro.getAngleZ();
    }
    _leftMotor.stop();
    _rightMotor.stop();
    delay(1000);
   
  }
  
  delay(5000);
  
  

}