Add servoing

motion.ino

@@ -23,20 +23,23 @@ void setup()
 void loop()
 {
   // Move forward
-  Serial.println("Move forward");
+  if (distance_cm > 20) {
-  move(100, 100, 2000);
+      Serial.println("Move forward");
- 
+      move(1500, 1500, 100);
+
   // Move backwards
-  Serial.println("Move backward");
+      Serial.println("Move backward");
-  move(-100, -100, 2000);
+      move(-100, -100, 500); 
   
-  // Turn right
+  //Turn right
   Serial.println("Turn right");
   move(100, -100, 2000);
+  }
 
   // Turn left
   Serial.println("Turn left");
   move(-100, 100, 2000);
+  delay(100);
  
 }
 

roomba.ino

@@ -0,0 +1,54 @@
+#include <Arduino.h>
+#include <Wire.h>
+#include <SoftwareSerial.h>
+#include <MeMCore.h>
+
+
+// declare motors
+MeDCMotor _leftMotor(9);
+MeDCMotor _rightMotor(10);
+
+// declare motor speeds
+int _leftMotorSpeed = 0;
+int _rightMotorSpeed = 0;
+
+void setup()
+{
+  Serial.begin(9600);
+  // set no speed for left and right motors 
+  _leftMotor.run((9)==M1?-(0):(0));
+  _rightMotor.run((10)==M1?-(0):(0));
+}
+
+void loop()
+{
+  // Move forward
+  Serial.println("Move forward");
+  move(100, 100, 2000);
+ 
+  // Move backwards
+  Serial.println("Move backward");
+  move(-100, -100, 2000);
+  
+  // Turn right
+  Serial.println("Turn right");
+  move(100, -100, 2000);
+
+  // Turn left
+  Serial.println("Turn left");
+  move(-100, 100, 2000);
+ 
+}
+
+void move(int leftMotorSpeed, int rightMotorSpeed, int duration)
+{
+  _leftMotorSpeed = leftMotorSpeed;
+  _rightMotorSpeed = rightMotorSpeed;
+  
+  _leftMotor.run((9)==M1?-(_leftMotorSpeed):(_leftMotorSpeed));
+  _rightMotor.run((10)==M1?-(_rightMotorSpeed):(_rightMotorSpeed));
+  delay(duration);
+  _leftMotor.stop();
+  _rightMotor.stop();
+  
+}

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;
+    }
+  }
+}

ultrasonic_sensor.ino

@@ -0,0 +1,22 @@
+#include <Arduino.h>
+#include "MeMCore.h"
+
+// Define the Ultrasonic Sensor using Makeblock library
+MeUltrasonicSensor ultrasonic(PORT_3);  // Change PORT_3 to the actual port used
+
+void setup() {
+    Serial.begin(9600);  // Start serial communication
+}
+
+void loop() {
+    double distance_cm = ultrasonic.distanceCm();   // Get distance in cm
+    double distance_in = ultrasonic.distanceInch(); // Get distance in inches
+
+    Serial.print("Distance: ");
+    Serial.print(distance_cm);
+    Serial.print(" cm | ");
+    Serial.print(distance_in);
+    Serial.println(" inches");
+
+    delay(500);  // Wait for 500ms before the next measurement
+}