#include "MeMCore.h"
#include <Wire.h>

MeGyro gyro;
MeDCMotor leftMotor(9);
MeDCMotor rightMotor(10);

uint8_t motorSpeed = 100;

void setup() {

  Serial.begin(115200);
  gyro.begin();

  leftMotor.run(0);
  rightMotor.run(0);

}

void loop() {
  //(0,0)
  gyro.update();
  goForward();
  delay(6000);
  stop();

  //(10,0)
  
  float error;
  float d_target = 90;
  float p;
  float v_e;
  float v_r;
  
  while (error > 10){
    gyro.update();
    error = d_target - gyro.getAngleZ();
    v_e = p*error;
    v_r = -v_e;
    goLeft(v_r);
  }
  stop();
  
  //(10,10)
  //(0,10)

}

void goForward(){

  leftMotor.run(-motorSpeed);
  rightMotor.run(motorSpeed);
  delay(1000);  

}

void goBackward(){

  leftMotor.run(motorSpeed);
  rightMotor.run(-motorSpeed);
  delay(100);  

}

void goLeft(float rotateSpeed){

  leftMotor.run(rotateSpeed);
  rightMotor.run(rotateSpeed);
  delay(100);

}

void stop(){

  leftMotor.stop();
  rightMotor.stop();
  delay(100);

}