a

ntm darren

carre/carre/carre.ino

@@ -1,92 +0,0 @@
-#include <MeGyro.h>
-#include <MeMCore.h>
-#include <MeUltrasonicSensor.h>
-#include <Wire.h>
-// The two motor objects
-MeDCMotor motor1(9);
-MeDCMotor motor2(10);
-MeUltrasonicSensor sensor1(3);
-MeGyro gyro;
-double Lspeed = 200;
-double Rspeed = 200;
-double tetadot;
-double distance=0;
-double teta;
-int corners[4][2] = {{0, 0}, {0, 1}, {1, 1}, {1, 0}};
-long s1;
-long s2;
-long x;
-int lspeed;
-void setup()
-{
-  Serial.begin(9600);
-  gyro.begin();
-}
-
-void loop()
-{
-  while (x<1){
-    s1=millis();
-    devant();
-    thetadot();
-    lspeed=((Lspeed/60)*(0.06*3.14));
-    s2=millis();
-    x=x+lspeed*((s2-s1)/1000);
-    Serial.println(s2-s1);
-    Serial.println(x);
-
-  }
-  x=0;
-  while (teta<90){
-  turnright();
-  }
-}
-void turnright(){
-  motor1.run(Lspeed);
-  motor2.run(Rspeed);
-}
-void turnleft(){
-  motor1.run(-Lspeed);
-  motor2.run(-Rspeed);
-}
-void devant(){
-  motor1.run(-Lspeed);
-  motor2.run(Rspeed);
-}
-void derriere(){
-  motor1.run(Lspeed);
-  motor2.run(-Rspeed);
-}
-void stop(){
-  motor1.run(0);
-  motor2.run(0);
-}
-void gyroscope(){
-  gyro.update();
-  Serial.read();
-  Serial.print("X:");
-  Serial.print(gyro.getAngleX() );
-  Serial.print(" Y:");
-  Serial.print(gyro.getAngleY() );
-  Serial.print(" Z:");
-  Serial.println(gyro.getAngleZ() );
-
-}
-void thetadot(){
-  gyro.update();
-  double i=gyro.getAngleZ();
-  delay (10);
-  gyro.update();
-  double f=gyro.getAngleZ();
-  tetadot=((f-i)/0.01);
-  Serial.print("thetadot: ");
-  Serial.println(tetadot);
-}
-void theta(){
-  gyro.update();
-  double i=gyro.getAngleZ();
-  delay (10);
-  gyro.update();
-  double f=gyro.getAngleZ();
-  teta=(f-i);
-}

carre/carre/carre/carre.ino

@@ -0,0 +1,144 @@
+/**
+ * @file    control.ino
+ * @brief   Description: this code drives the mbot robot to a target location (x, y, theta) using the gyro data.
+ */
+
+#include <Arduino.h>
+#include <Wire.h>
+#include <SoftwareSerial.h>
+#include "MeMCore.h"
+
+// Vars to change setup
+double _Kp_angle = -5.0;
+double _targetAngle = 0;
+
+// robot characteristics
+double _wheelDiameter = 6.0; //cm
+double _distanceBetweenWheels = 11.5; //cm
+int _basicMotorSpeed = 100;
+
+// 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(115200);
+
+  // 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()
+{
+  // Define the four corners of the square
+  double corners[4][2] = {{0, 0}, {0, 1}, {1, 1}, {1, 0}};
+  int vitesse = 200;
+
+  // Initialize the current position to the first corner
+  float current_x = corners[0][0];
+  float current_y = corners[0][1];
+
+  // Loop over the four corners of the square
+  int i=0;
+  while (i<4) {
+    // target position
+    float target_x = corners[i][0];
+    float target_y = corners[i][1];
+
+    // calculate the target angle and distance
+    float dx = target_x - current_x;
+    float dy = target_y - current_y;
+    float distance = sqrt(dx*dx + dy*dy);
+    _targetAngle = atan2(dy, dx) * 180 / PI;
+    Serial.println("_targetAngle");
+    Serial.println(_targetAngle);
+    Serial.println("distance ");
+    Serial.println(distance);
+    Serial.println("target ");
+    Serial.println(target_x);
+    Serial.println(target_y);
+
+    // Calculate the time required to travel the distance at the given speed
+    int temps = ((distance)/(3.14*2*0.03*(vitesse/60)));
+
+    // Rotate the robot towards the target angle until the error is within 2 degrees
+    while (fabs(_thetaCurrent - _targetAngle) > 2) {
+      _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 to move the robot
+  move(rightWheelSpeed, leftWheelSpeed);
+
+  // retrieve new values from gyro
+  _gyro.update();
+
+  // get the theta angle (Rz)
+  _thetaCurrent = _gyro.getAngleZ() - _thetaBaseline;
+  _errorAngle = _targetAngle - _thetaCurrent;
+}
+
+// Stop the robot
+move(-vitesse, -vitesse);
+delay(temps*1000);
+
+// Update the current position and move to the next corner
+current_x = target_x;
+current_y = target_y;
+i++;
+if (i==4){
+  i=0;
+}
+}
+
+Serial.println("out of the loop");
+}
+
+// Function to move the robot
+void move(int leftMotorSpeed, int rightMotorSpeed)
+{
+_leftMotor.run((9)==M1?-(leftMotorSpeed):(leftMotorSpeed));
+_rightMotor.run((10)==M1?-(rightMotorSpeed):(rightMotorSpeed));
+}
+
+// Function to stop the robot
+void stop()
+{
+_leftMotor.stop();
+_rightMotor.stop();
+}
+

lineFollower/lineFollower.ino/lineFollower/lineFollower.ino

@@ -1,5 +1,5 @@
-
+#include <Wire.h>
-
+#include <
 // Create a MeLineFollower object
 MeLineFollower lineFollower(PORT_1);