Merge branch 'develop'

autonom/autonom.ino

@@ -0,0 +1,99 @@
+#include "Arduino.h"
+#include "Wire.h"
+#include "MeMCore.h"
+
+MeGyro gyro(PORT_1); // Initialize the gyro sensor on port 1
+MeDCMotor _leftMotor(9);
+MeDCMotor _rightMotor(10);
+// Define constants
+const int targetSpeed = 50; // Speed for translation movements
+const int wheelDistanceBetweenWheels = 11; // Distance between the wheels in centimeters
+const int wheelRadius = 3; // Radius of the wheels in centimeters
+
+// Function to calculate wheel speeds based on translational and rotational speeds
+void calculateWheelSpeeds(float v, float theta_dot, float &vL, float &vR) {
+  vL = v/wheelRadius - (wheelDistanceBetweenWheels*theta_dot)/wheelRadius*2;
+  vR = v/wheelRadius + (wheelDistanceBetweenWheels*theta_dot)/wheelRadius*2;
+}
+
+// Function to perform rotation movement
+void rotate(float targetAngle, float v) {
+  const float KP = 0.5; // Proportional control gain
+  const float MIN_SPEED = 10; // Minimum rotation speed
+
+  float currentAngle;
+  float error;
+  float rotationSpeed;
+
+  do {
+    // Get current angle
+    gyro.update();
+    currentAngle = gyro.getAngleZ();
+    Serial.print("Current angle :");
+    Serial.print(currentAngle);
+    Serial.print(" Objective :");
+    Serial.println(targetAngle);
+
+    // Calculate error
+    error = targetAngle - currentAngle;
+    while (error > 180) error -= 360;
+    while (error < -180) error += 360;
+
+    // Calculate rotation speed using proportional control
+    rotationSpeed = KP * error;
+
+    // Constrain rotation speed to ensure it doesn't exceed the minimum speed
+    rotationSpeed = constrain(rotationSpeed, -MIN_SPEED, MIN_SPEED);
+
+    // Calculate wheel speeds based on rotation speed
+    float vL, vR;
+    calculateWheelSpeeds(v, rotationSpeed, vL, vR);
+
+    // Apply speeds to motors
+    _leftMotor.run(-vL);
+    _rightMotor.run(vR);
+  } while (abs(error) > 5); // Continue rotation until error is within acceptable threshold
+}
+
+
+// Function to perform translation movement
+void translate(float distance) {
+  // Calculate time required to travel given distance at target speed
+  float time = distance / targetSpeed;
+  _leftMotor.run(-targetSpeed);
+  _rightMotor.run(targetSpeed);
+  Serial.print("Time calculated for next segment : ");
+  Serial.println(time*1000);
+  delay(time * 1000); // Convert time to milliseconds
+}
+
+void setup() {
+  Serial.begin(9600); // Initialize serial communication
+  gyro.begin(); // Initialize the gyro sensor
+  _leftMotor.run(0);
+  _rightMotor.run(0);
+}
+
+void loop() {
+  // Array containing the 2D positions of the 4 corners of the square
+  float targetPoints[4][2] = {{0, 0}, {10, 0}, {10, 10}, {0, 10}};
+
+  for (int i = 0; i < 4; i++) {
+    float targetX = targetPoints[i][0];
+    float targetY = targetPoints[i][1];
+
+    // Calculate rotation angle towards target point
+    float currentX = 0; // Assume starting from origin
+    float currentY = 0; // Assume starting from origin
+    float targetAngle = atan2(targetY - currentY, targetX - currentX) * 180 / PI;
+
+    // Perform rotation towards target point
+    rotate(targetAngle, targetSpeed);
+
+    // Calculate distance to target point
+    float distance = sqrt(pow(targetX - currentX, 2) + pow(targetY - currentY, 2));
+
+    // Perform translation towards target point
+    translate(distance);
+  }
+}

gyro/gyro.ino

@@ -0,0 +1,28 @@
+#include "Arduino.h"
+#include "Wire.h"
+#include "MeMCore.h"
+
+MeGyro gyro(PORT_1);
+
+void setup() {  
+  Serial.begin(9600); // Initialize serial communication
+  gyro.begin(); // Initialize the gyro sensor
+}
+
+void loop() {
+  gyro.update();
+  // Read gyro data
+  double anglex = gyro.getAngleX();
+  double angley = gyro.getAngleY();
+  double anglez = gyro.getAngleZ();
+  
+  // Print gyro data
+  Serial.print("anglex:");
+  Serial.print(anglex);
+  Serial.print(",angley:");
+  Serial.print(angley);
+  Serial.print(",anglez:");
+  Serial.println(anglez);
+  
+  delay(500); // Delay for stability, adjust as needed
+}