From 59679eae69693ea1c401ee9f0e0ba5c9bde7905c Mon Sep 17 00:00:00 2001
From: "lymeng.ly" <lymeng.ly@ecam.fr>
Date: Mon, 17 Mar 2025 16:40:09 +0100
Subject: [PATCH] Add servoing

---
 servoing.ino | 146 +++++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 146 insertions(+)
 create mode 100644 servoing.ino

diff --git a/servoing.ino b/servoing.ino
new file mode 100644
index 0000000..f7ca40d
--- /dev/null
+++ b/servoing.ino
@@ -0,0 +1,146 @@
+/**
+ * @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;
+const long int _baudRate = 115200;
+
+// robot characteristics
+double _wheelDiameter = 6.0; //cm
+double _distanceBetweenWheels = 11.5; //cm
+int _basicMotorSpeed = 100;
+
+// internal global variables
+String _inputString = "";         // a String to hold incoming data
+bool _stringComplete = false;  // whether the string is complete
+
+// declare motors
+MeDCMotor _leftMotor(9);
+MeDCMotor _rightMotor(10);
+
+// declare gyro
+MeGyro _gyro;
+
+// position, baseline...
+double _thetaCurrent = 0.0;
+double _thetaBaseline = 0.0;
+double _errorAngle = 0.0; // in deg
+
+
+
+void setup()
+{
+  // initialize the serial port
+  Serial.begin(_baudRate);
+
+  // set no speed for left and right motors 
+  _leftMotor.run((9)==M1?-(0):(0));
+  _rightMotor.run((10)==M1?-(0):(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()
+{
+    // checks if an update of Kp value is available
+    if (_stringComplete) 
+    {
+      // updates the Kp value
+      _Kp_angle = _inputString.toDouble(); 
+
+      // resets the input string to accept new input from the serial port
+      _inputString = "";
+      _stringComplete = false;
+    }
+     
+    // adjust the angle
+    _errorAngle = _targetAngle - _thetaCurrent;
+    
+    // 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;
+
+    // for display
+    Serial.print("Current_angle:"); Serial.print(_thetaCurrent);Serial.print(",");
+    Serial.print("Target_angle:"); Serial.println(_targetAngle);
+}
+
+void move(int leftMotorSpeed, int rightMotorSpeed)
+{
+  _leftMotor.run((9)==M1?-(leftMotorSpeed):(leftMotorSpeed));
+  _rightMotor.run((10)==M1?-(rightMotorSpeed):(rightMotorSpeed));
+}
+
+void stop()
+{
+  _leftMotor.stop();
+  _rightMotor.stop();
+}
+
+/*
+  SerialEvent occurs whenever a new data comes in the hardware serial RX. This
+  routine is run between each time loop() runs, so using delay inside loop can
+  delay response. Multiple bytes of data may be available.
+*/
+void serialEvent() 
+{
+  while (Serial.available()) 
+  {
+    // gets the new byte:
+    char inChar = (char)Serial.read();
+    // adds it to the _inputString:
+    _inputString += inChar;
+    // if the incoming character is a newline, set a flag so the main loop can
+    // does something about it:
+    if (inChar == '\n') 
+    {
+      _stringComplete = true;
+    }
+  }
+}