#include "MeMCore.h"
#include <Wire.h>
const int Port_1 =9;
const int Port_2 =10;

MeDCMotor motor1(Port_1);
MeDCMotor motor2(Port_2);
MeUltrasonicSensor ultraSensor(PORT_3); /* Ultrasonic module can ONLY be connected to port 3, 4, 6, 7, 8 of base shield. */
uint8_t motorSpeed =100;
double distance;
double threshold= 10;
int Squarex[]={0,25,25,0};
int Squarey[]={0,0,25,25};
bool tmp=0;
double angle =0.0;
double angleDifference=0.0;
double targetAngle = 90.0;

MeGyro gyro;

void setup()
{
Serial.begin(9600);
gyro.begin();
}

void loop()
{
     if(tmp == 0){          /* if-loop with flag to make the robot go 25 cm forward  */
      motorSpeed = 100;
      motor1.run(-motorSpeed); /* value: between -255 and 255. */
      motor2.run(motorSpeed); /* value: between -255 and 255. */
      delay(2000);
      motor1.stop();
      motor2.stop();
      gyro.update();
      angle = gyro.getAngleZ();  /* Sense the initial position of the robot before turning */
      tmp = 1;                  /* update flag */
     }

     if (tmp == 1) {              /* if loop for rotation after translation */

      gyro.update();      
      angleDifference = gyro.getAngleZ() - angle;  /* Calculate how much the robot turned */   
       
       if(angleDifference < targetAngle){ 
        if (angleDifference < 0){
          motorSpeed = 2.83*(targetAngle);
         }
        else{
          motorSpeed = 2.83*(targetAngle-angleDifference); 
         }               
         motor1.run(motorSpeed);
         motor2.run(motorSpeed);        
        }
        
       if(angleDifference >= targetAngle){ /* if robot reaches 90°, stop rotation and update flag*/
         motor1.stop();
         motor2.stop();
         tmp = 0;          
        }       
     }
}