diff --git a/control/control.ino b/control/control.ino
new file mode 100644
index 0000000..a35b324
--- /dev/null
+++ b/control/control.ino
@@ -0,0 +1,139 @@
+/**
+ * @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();
+}