finalll

Control/Control.ino

@@ -0,0 +1,128 @@
+#include <Arduino.h>
+#include <Wire.h>
+#include <SoftwareSerial.h>
+#include "MeMCore.h"
+
+double _wheelDiameter = 6.0;
+double _distanceBetweenWheels = 11.5;
+int _basicMotorSpeed = 100;
+
+// Target position and angle
+double _targetX = 20.0;
+double _targetY = 15.0;
+double _targetAngle = 0.0;
+
+// Current position and angle
+double _currentX = 0.0;
+double _currentY = 0.0;
+double _thetaCurrent = 0.0;
+double _thetaBaseline = 0.0;
+double _errorAngle = 0.0;
+
+MeDCMotor _leftMotor(9);
+MeDCMotor _rightMotor(10);
+
+MeGyro _gyro;
+
+void setup() {
+  Serial.begin();
+
+  // Set no speed for motors initially
+  _leftMotor.run((9) == M1 ? -(0) : (0));
+  _rightMotor.run((10) == M1 ? -(0) : (0));
+
+  // Initialize gyro
+  _gyro.begin();
+
+  // Wait for 2 seconds
+  delay(2000);
+
+  // Retrieve initial baseline for theta angle
+  _thetaBaseline = _gyro.getAngleZ();
+}
+
+void loop() {
+  // Calculate target angle based on target position
+  _targetAngle = calculateTargetAngle(_targetX, _targetY);
+
+  // Rotate to target angle
+  rotateToTargetAngle();
+
+  // Move forward for corresponding distance
+  moveForwardToTarget();
+
+  // Stop the robot after reaching the target
+  stopMotors();
+}
+
+void serialEvent() {
+  // No serial input used in this version
+}
+
+// Function to calculate target angle based on target position
+double calculateTargetAngle(double targetX, double targetY) {
+
+  double deltaX = targetX - _currentX;
+  double deltaY = targetY - _currentY;
+
+  return atan2(deltaY, deltaX) * 180 / M_PI;
+}
+
+void move(int leftMotorSpeed, int rightMotorSpeed)
+{
+  _leftMotor.run((9)==M1?-(leftMotorSpeed):(leftMotorSpeed));
+  _rightMotor.run((10)==M1?-(rightMotorSpeed):(rightMotorSpeed));
+}
+
+// Function to rotate the robot to the target angle
+void rotateToTargetAngle() {
+  _errorAngle = _targetAngle - _thetaCurrent;
+
+  while (abs(_errorAngle) > 5) {
+    double w = _Kp_angle * _errorAngle;
+    double leftWheelSpeed  = -_distanceBetweenWheels * w / _wheelDiameter;
+    double rightWheelSpeed  = _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;
+  }
+}
+  }
+}
+
+// Function to move forward for the corresponding distance
+void moveForwardToTarget() {
+  // Calculate distance to target using Pythagorean theorem
+  double distanceToTarget = sqrt(pow(_targetX - _currentX, 2) + pow(_targetY - _currentY, 2));
+
+  // Calculate time to reach target based on speed = distance / time
+  double timeToTarget = distanceToTarget / _basicMotorSpeed; // time in seconds
+
+  // Move the robot forward
+  move(rightWheelSpeed, leftWheelSpeed);
+
+  // Wait for the calculated time
+  delay(timeToTarget * 1000); // delay function takes time in milliseconds
+
+  // Stop the motors
+  stopMotors();
+}
+
+// Function to stop the motors
+void stopMotors() {
+  _leftMotor.stop();
+  _rightMotor.stop();
+}

servoing/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
+    // do something about it:
+    if (inChar == '\n') 
+    {
+      _stringComplete = true;
+    }
+  }
+}