/**
 * @file    control.ino
 * @author  Guillaume Gibert
 * @version V0.1
 * @date    2021/04/18
 * @brief   Description: this code drives the mbot robot to a target location (x, y, theta) using the gyro data.
 *
 * Function List:
 * 1. void control::move(int, int)
 * 2. void control::stop(void) 
 *
 */
#include <Arduino.h>
#include <Wire.h>
#include <SoftwareSerial.h>
#include "MeMCore.h"

// Vars to change setup
double _Kp_angle = -5.0;
double _targetAngle = 90;

// robot characteristics
double _wheelDiameter = 6.0; //cm
double _distanceBetweenWheels = 11.5; //cm
int _basicMotorSpeed = 100;

// declare motors
MeDCMotor _leftMotor(9);
MeDCMotor _rightMotor(10);

//declare motor speed
int lSpeed;
int rSpeed;
float duration;

// declare gyro
MeGyro _gyro;

// position, baseline...
double _thetaCurrent = 0.0;
double _thetaBaseline = 0.0;
double _errorAngle = 0.0; // in deg

int positions[][2] = {{0, 0}, {0, 25}, {25, 25}, {25, 0}};



void setup()
{
  // initialize the serial port
  Serial.begin(115200);

  // set no speed for left and right motors 
  _leftMotor.run((9)==M1?-(0):(0));
  _rightMotor.run((10)==M1?-(0):(0));

  //set speed for active motors
  lSpeed = -100;
  rSpeed = 100;
  duration = 1000.0;

  // initialize the gyro
  _gyro.begin();

  // wait for 2 seconds
  delay(2000);

  // retrieve new values from gyro
  _gyro.update();

  // record the baseline for the theta angle (Rz)
  _thetaBaseline = _gyro.getAngleZ();

}

void loop()
{
  for (int i = 0; i<4; i ++){
    if (i == 0){
      _targetAngle = 0;
    }
    if (i == 1){
      _targetAngle = 90;
    }
    if (i == 2){
      _targetAngle = 180;
    }
    if (i == 3){
      _targetAngle = -90;
    }
    _targetAngle = 90;
    // adjust the angle
    _errorAngle = _targetAngle - _thetaCurrent;
    
    while (abs(_errorAngle) > 5.0){
      // determine the command (proportionnal controller) i.e. angular velocity
      double v = 0.0;
      double w = _Kp_angle * _errorAngle;
      
      // compute the command
      double leftWheelSpeed  =  v / (_wheelDiameter/2) - _distanceBetweenWheels * w / (_wheelDiameter) ;
      double rightWheelSpeed  = v / (_wheelDiameter/2) + _distanceBetweenWheels * w / (_wheelDiameter) ;
      
      // crop wheel speed
      if (leftWheelSpeed > 250)
        leftWheelSpeed = 250;
      if (rightWheelSpeed > 250)
        rightWheelSpeed = 250;
      
      // send the commands
      move(rightWheelSpeed, leftWheelSpeed);
      
      // retrieve new values from gyro
      _gyro.update();
      
      // get the theta angle (Rz)
      _thetaCurrent = _gyro.getAngleZ() - _thetaBaseline;
      _errorAngle = _targetAngle - _thetaCurrent;
      //Serial.println(_errorAngle);
    }
    _leftMotor.run(lSpeed);
    _rightMotor.run(rSpeed);
    delay(duration);
    stop();
  }
  
}

void move(int leftMotorSpeed, int rightMotorSpeed)
{
  _leftMotor.run((9)==M1?-(leftMotorSpeed):(leftMotorSpeed));
  _rightMotor.run((10)==M1?-(rightMotorSpeed):(rightMotorSpeed));
}

void stop()
{
  _leftMotor.stop();
  _rightMotor.stop();
}