merge from dev

Osoyoo_car_enacter/Osoyoo_car_enacter.ino

@@ -1,28 +1,38 @@
-/*  ___   ___  ___  _   _  ___   ___   ____ ___  ____  
- * / _ \ /___)/ _ \| | | |/ _ \ / _ \ / ___) _ \|    \ 
- *| |_| |___ | |_| | |_| | |_| | |_| ( (__| |_| | | | |
- * \___/(___/ \___/ \__  |\___/ \___(_)____)___/|_|_|_|
- *                  (____/ 
- * Arduino Mecanum Omni Direction Wheel Robot Car Lesson5 Wifi Control
- * Tutorial URL http://osoyoo.com/?p=30022
- * CopyRight www.osoyoo.com
- * 
+/*
+ * .______        _______..__   __.
+ * |   _  \      /       ||  \ |  |
+ * |  |_)  |    |   (----`|   \|  |
+ * |   _  <      \   \    |  . `  |
+ * |  |_)  | .----)   |   |  |\   |
+ * |______/  |_______/    |__| \__|
+ *
+ *  BSN2 2021-2022
+ *   Aleksei Apostolou, Daniel Duval, Célien Fiorelli, Geordi Gampio, Julina Matouassiloua
+ *
+ *  Teachers
+ *   Raphaël Cazorla, Florian Tholin, Olivier Georgeon
+ *
+ *  Bachelor Sciences du Numerique. ESQESE. UCLy. France
+ *
+ * Warning: If libraries MPU6050_Light, MPU6050, and HMC5883L are not installed
+ * and if the robot does not have an IMU board then please comment the corresponding code below
  */
+
 #include "src/wheel/omny_wheel_motion.h"
 #include "src/lightSensor/LightSensor.h"
 LightSensor ls;
 int previous_floor = 0; 
 
 
-#define WifiMode "R"        //Define the wifi mode of the robot, 'R' for router and 'W' for robot connection
+#define WifiMode "R"        // Define the wifi mode of the robot, 'R' for router and 'W' for robot connection
 
-#include "src/imu/gyro.h"
-#include "src/imu/compass.h"
+#include "src/imu/gyro.h"    // Comment if the robot has no IMU
+#include "src/imu/compass.h" // Comment if the robot has no compass
 
 #include "src/wifi/JsonOutcome.h"
 JsonOutcome outcome;
 
-#include "src/head/Head.h";
+#include "src/head/Head.h"
 Head head;
 
 #include "src/utils/DelayAction.h"
@@ -47,8 +57,6 @@ int floorOutcome = 0;
 
 void setup()
 {
-// init_GPIO();
-
   Serial.begin(9600);   // initialize serial for debugging
 
   head.servo_port();
@@ -62,16 +70,16 @@ void setup()
     wifiBot.wifiInitRouter();
   }
 
-  // mpu_setup();
-  // compass_setup();
+  compass_setup(); // Comment if the robot has no compass
+  mpu_setup();     // Must be after compass_setup() otherwise yaw is distorted
 }
 
 void loop()
 {
   da.checkDelayAction(millis());
 
   int packetSize = wifiBot.Udp.parsePacket();
-  // gyro_update();
+  gyro_update();
 
   if (packetSize) { // if you get a client
     Serial.print("Received packet of size ");
@@ -82,9 +90,13 @@ void loop()
       packetBuffer[len] = 0;
     }
 
-    JSONVar jsonReceive = JSON.parse(packetBuffer);
-    if (jsonReceive.hasOwnProperty("action")) {
-      action = ((const char*) jsonReceive["action"])[0];
+    if (len == 1) { // If one character, then it is the action
+        action = packetBuffer[0];
+    } else {        // If more than one characters, then it is a JSon string
+      JSONVar jsonReceive = JSON.parse(packetBuffer);
+      if (jsonReceive.hasOwnProperty("action")) {
+        action = ((const char*) jsonReceive["action"])[0];
+      }
     }
 
     endTime = millis() + 2000;
@@ -104,7 +116,7 @@ void loop()
   }
 
   int current_floor = ls.tracking();
-  if (current_floor != previous_floor) // la fonction renvoi true si elle capte une ligne noir
+  if (current_floor != previous_floor) // la fonction renvoie true si elle capte une ligne noir
   {
     stop_Stop();
     if (current_floor > 0)
@@ -122,24 +134,19 @@ void loop()
   {
     stop_Stop();
 
-    outcome.addValue("echo_distance", (String) head.distUS.dist());
-    outcome.addValue("head_angle", (String) (head.current_angle -  90));
-    outcome.addValue( "floor", (String) floorOutcome);
-    outcome.addValue( "status", (String) floorOutcome);
-
-    //renvoi JSON du azimuth
-    // outcome.addValue( "yaw", (String) (gyroZ()));
-    // outcome.addValue( "compass", (String) (degreesNorth()));
+    outcome.addInt("echo_distance", (int) head.distUS.dist());
+    outcome.addInt("head_angle", (int) (head.current_angle -  90));
+    outcome.addInt("floor", (int) floorOutcome);
+    outcome.addInt("status", (int) floorOutcome);
+    outcome.addInt("yaw", (int) (gyroZ()));
+    outcome.addInt("azimuth", (int) (degreesNorth()));  // Comment if the robot as no compass
 
     //Send outcome to PC
     wifiBot.sendOutcome(outcome.get());
     outcome.clear();
 
     actionStep = 0;
     floorOutcome = 0;
-  }
-  if(actionStep == 0)
-  {
-      // reset_gyroZ(); //calibrer l'angle Z à 0 tant qu'il n'a pas fait d'action
+    reset_gyroZ();
   }
 }

Osoyoo_car_enacter/src/head/Head.cpp

@@ -51,12 +51,13 @@ void Head::scan(int angleMin, int angleMax, int Nbre_mesure, int index_0) {
   int angle;
   for (int pos = 0; pos <= Nbre_mesure; pos++) {
     head_servo.write(angleMin+(pas*pos));
-    delay(300);
+    delay(150);
     distances[pos] = distUS.dist();
   }
   indexMin = getIndexMin(Nbre_mesure, distances);
   angle = (indexMin + index_0)*pas;
   head_servo.write(angle);
+  delay(150); // Wait because it will read the distance again
   current_angle = angle;
 }
 

Osoyoo_car_enacter/src/imu/compass.cpp

@@ -11,9 +11,15 @@
 #include <MPU6050.h>
 
 HMC5883L compass;
+MPU6050 _mpu;
 
 void compass_setup()
 {
+  // The compass works, but it distorts the measurement of yaw
+  _mpu.begin(MPU6050_SCALE_2000DPS, MPU6050_RANGE_2G);
+  _mpu.setI2CMasterModeEnabled(false);
+  _mpu.setI2CBypassEnabled(true) ;
+  _mpu.setSleepEnabled(false);
 
   // Initialize Initialize HMC5883L
   Serial.println("Initialize HMC5883L");

Osoyoo_car_enacter/src/imu/gyro.cpp

@@ -12,13 +12,16 @@
      Serial.print(F("MPU6050 status: "));
      Serial.println(status);
      while (status != 0) { } // stop everything if could not connect to MPU6050
-       Serial.println(F("Calculating offsets, do not move MPU6050"));
-       delay(1000);
-       mpu.calcOffsets(); // gyro and accelero
-       Serial.println("Done!\n");
+     Serial.println(F("Calculating offsets, do not move MPU6050"));
+     delay(1000);
+     mpu.calcOffsets(); // gyro and accelero
+     mpu.setAngleZ(0);
+     Serial.println("Done!\n");
  }
  void reset_gyroZ(){
     mpu.setAngleZ(0); // créer une fonction pour réinitialiser l'angle à 0 quand on ne fait plus d'action.
+    // Il faut créer cette fonction dans la librairie MPU6050_light.h ligne 91, inserer :
+    // 	void setAngleZ(float value) {angleZ = value;};
  }
  void gyro_update(){
     mpu.update(); //Créer une fonction pour update le mpu dans le loop.

Osoyoo_car_enacter/src/wheel/omny_wheel_motion.h

@@ -12,7 +12,7 @@
 #define omny_wheel_motion_h
 
 //Definition de la vitesse des roues
-#define SPEED 85
+#define SPEED 100
 
 //Selection des ports sur la cartes Arduino et la carte WIFI
 #define speedPinR 9   //  RIGHT WHEEL PWM pin D45 connect front MODEL-X ENA

Osoyoo_car_enacter/src/wifi/JsonOutcome.cpp

@@ -11,6 +11,11 @@ void JsonOutcome::addValue(char key[], String value)
     data[(String) key] = value;
 }
 
+void JsonOutcome::addInt(String key, int value)
+{
+    data[key] = value;
+}
+
 String JsonOutcome::get()
 {
     return JSON.stringify(data);

Osoyoo_car_enacter/src/wifi/JsonOutcome.h

@@ -25,6 +25,8 @@ class JsonOutcome
      */
     void addValue(char key[], String value);
 
+    void addInt(String, int value);
+
     /*
      * Method for deleting all key/value of "data"
      * Execute after sending json data by wifi for clear data

Python/OsoyooControllerBSN/Display/Controller.py

@@ -1,3 +1,4 @@
+import sys
 import json
 from ..RobotDefine import *
 import threading
@@ -6,17 +7,18 @@
 import math
 from ..Display.OsoyooCar import OsoyooCar
 from ..Display.EgoMemoryWindow import EgoMemoryWindow
+from ..Display.ModalWindow import ModalWindow
 import pyglet
 from pyrr import matrix44
 
 
 class Controller:
-    def __init__(self, view: EgoMemoryWindow):
+    def __init__(self, view: EgoMemoryWindow, robot_ip):
         # View
         self.view = view
 
         # Model
-        self.wifiInterface = WifiInterface(ip=self.view.ip)
+        self.wifiInterface = WifiInterface(robot_ip)
         self.phenomena = []
         self.robot = OsoyooCar(self.view.batch)
 
@@ -49,7 +51,7 @@ def watch_outcome(self, dt):
     def update_model(self):
         """ Updating the model from the latest received outcome """
         outcome = json.loads(self.outcome_bytes)
-        print(outcome)
+        # print(outcome)
         floor = 0
         if 'floor' in outcome:
             floor = int(outcome['floor'])
@@ -80,10 +82,9 @@ def update_model(self):
                 translation[1] = -SHIFT_DISTANCE
             if self.action == "8":
                 if not blocked:
-                    #translation[0] = STEP_FORWARD_DISTANCE * outcome['duration'] / 1000
+                    # translation[0] = STEP_FORWARD_DISTANCE * outcome['duration'] / 1000
                     translation[0] = 100
 
-
             # Actual measured displacement if any
             if 'yaw' in outcome:
                 rotation = outcome['yaw']
@@ -147,21 +148,26 @@ def update_model(self):
             if 'azimuth' in outcome:
                 self.view.azimuth = outcome['azimuth']
 
-            # Update the origin
-            # self.view.update_environment_matrix(displacement_matrix)
-
 
 # Testing the controller by remote controlling the robot from the egocentric memory window
+# Set the IP address. Run:
+# python -m Python.OsoyooControllerBSN.Display.Controller <Robot's IP>
 if __name__ == "__main__":
-    ip_ = "10.40.22.252"
-    emw = EgoMemoryWindow(ip=ip_)
-    controller = Controller(emw)
-
-
+    ip = "192.168.4.1"
+    if len(sys.argv) > 1:
+        ip = sys.argv[1]
+    else:
+        print("Please provide your robot's IP address")
+    print("Sending to: " + ip)
+    emw = EgoMemoryWindow()
+    controller = Controller(emw, ip)
 
     @emw.event
     def on_text(text):
         """ Receiving the action from the window and calling the controller to send the action to the robot """
+        if text == "C":
+            window = ModalWindow(controller.phenomena)
+            return
         if controller.enact_step == 0:
             if text == "/":  # Send the angle marked by the mouse click
                 # emw.on_text(text)