Fix manual/autonomous modes

README.md

@@ -4,21 +4,28 @@ Where all code should live. There may be some changes for CI to work properly bu
 I'll get those sorted out later. Docs can live in each individual folder as a 
 README.
 
-## To-do
+## Flashing the ESP32
 
-- [ ] Motors must spin.
-- [ ] Total latency between the control and the motor must not exceed 100ms.
-- [ ] Stabilised bot must be controlled naturally.
+Follow instructions on [the PlatformIO website](https://platformio.org/install/cli)
+to download PlatformIO CLI.
 
-- [ ] A control algorithm for a MatLab simulation.
-- [ ] Translating the controller into a digital implementation.
-- [ ] Accelerometer data <-> Control Algorithm (ESP32).
-- [ ] Tuning the controller for unaccounted values.
+Then run these following commands:
 
-- [ ] Power consumption <-> User.
-- [ ] Remaining battery energy <-> User.
+```bash
+cd esp32
+./run.sh
+```
 
-- [ ] Camera <-> User
+## Starting the website
 
-Specifications need to be defined for the application. Automonous behaviour
-must be shown.
+To run the website on `localhost:3000`, run the following commands:
+
+```bash
+cd server/website
+npm i
+npm start
+```
+
+## Raspberry Pi setup
+
+See the docs in the [`pi`](pi) folder.

esp32/include/SerialLoop.h

@@ -4,14 +4,12 @@
 #include "KeyDirection.h"
 
 /**
- * Takes an 8-bit message and decodes to direction and speed
+ * Takes an 3-bit message and decodes to direction and speed
  * The first 3 bits are the direction
- * The last 5 bits are the speed
  */
-void decode(uint8_t message, KeyDirection &direction, uint8_t &speed)
+void decode(uint8_t message, KeyDirection &direction)
 {
-    direction = static_cast<KeyDirection>(message >> 5);
-    speed = message & 0b00011111;
+    direction = static_cast<KeyDirection>(message & 0b00000111);
 }
 
 #ifndef NATIVE
@@ -27,18 +25,21 @@ namespace SerialLoop
      */
     void loop()
     {
-        Serial.println("SERIAL");
         // Discard all buffer data
         while (Serial.available() > 1)
         {
             Serial.read();
         }
 
+        if (Serial.available() < 1)
+        {
+            return;
+        }
+
         char message = Serial.read();
 
         KeyDirection direction;
-        uint8_t speed;
-        decode(message, direction, speed);
+        decode(message, direction);
 
         switch (direction)
         {
@@ -61,9 +62,14 @@ namespace SerialLoop
 
         if (xSemaphoreTake(PidController::controlMutex, (TickType_t)0) == pdTRUE)
         {
-            PidController::setDirection(PidDirection(speed, direction));
+            PidController::setDirection(PidDirection(100, direction));
+            Serial.println("Speed: 100, key_dir: " + String(direction));
             xSemaphoreGive(PidController::controlMutex);
         }
+        else
+        {
+            Serial.println("Enter /a to give control back to the Raspberry Pi");
+        }
 
         // Yield to other tasks
         delay(10);

esp32/lib/PidController/PidController.cpp

@@ -154,8 +154,8 @@ void PidController::stabilizedLoop()
         // Serial.println("RIGHT");
         break;
     case KeyDirection::FORWARD:
-        step1.setTargetSpeedRad(-SPEED);
-        step2.setTargetSpeedRad(SPEED);
+        step1.setTargetSpeedRad(SPEED);
+        step2.setTargetSpeedRad(-SPEED);
         // Serial.println("FORWARD");
         break;
     case KeyDirection::LEFT:
@@ -164,8 +164,8 @@ void PidController::stabilizedLoop()
         // Serial.println("LEFT");
         break;
     case KeyDirection::BACKWARD:
-        step1.setTargetSpeedRad(SPEED);
-        step2.setTargetSpeedRad(-SPEED);
+        step1.setTargetSpeedRad(-SPEED);
+        step2.setTargetSpeedRad(SPEED);
         // Serial.println("BACKWARDS");
         break;
     case KeyDirection::STOP:

esp32/lib/WifiSetup/WifiSetup.cpp

@@ -214,9 +214,16 @@ void WifiSetup::callback(char *topic, byte *payload, unsigned int length)
                 key_dir = KeyDirection::STOP;
             }
 
-            Serial.println("Speed: " + String(speed) + ", key_dir: " + String(key_dir));
-
-            PidController::setDirection(PidDirection(speed, key_dir));
+            if (xSemaphoreTake(PidController::controlMutex, (TickType_t)0) == pdFALSE)
+            {
+                Serial.println("Speed: " + String(speed) + ", key_dir: " + String(key_dir));
+                PidController::setDirection(PidDirection(speed, key_dir));
+            }
+            else
+            {
+                Serial.println("Enter /m to take control back from the Raspberry Pi");
+                xSemaphoreGive(PidController::controlMutex);
+            }
         }
         else if (strcmp(topic, "user/pid") == 0)
         {
@@ -255,58 +262,43 @@ void WifiSetup::callback(char *topic, byte *payload, unsigned int length)
             String message = doc["message"];
             if (message == "/auto" || message == "/a")
             {
-                // Release the mutex so the Raspberry Pi can take control
+                // Attempt to take the mutex so the Raspberry Pi can take control
                 if (xSemaphoreTake(
                         PidController::controlMutex,
-                        (TickType_t)0) == pdTRUE)
+                        (TickType_t)0) == pdFALSE)
+                {
+                    // Release the mutex so the Raspberry Pi can take control
+                    PidController::setDirection(PidDirection(0, KeyDirection::STOP));
+                    Serial.println("Automatic control activated and mutex released");
+                }
+                else
                 {
-                    xSemaphoreGive(PidController::controlMutex);
+                    Serial.println("Failed to activate automatic control, mutex not taken");
                 }
+                xSemaphoreGive(PidController::controlMutex);
             }
             else if (message == "/manual" || message == "/m")
             {
                 // Take the mutex so the Wifi can take control
-                xSemaphoreTake(PidController::controlMutex, (TickType_t)0);
+                if (xSemaphoreTake(PidController::controlMutex, (TickType_t)0) == pdTRUE)
+                {
+                    PidController::setDirection(PidDirection(0, KeyDirection::STOP));
+                    Serial.println("Manual control activated");
+                }
+                else
+                {
+                    Serial.println("Failed to activate manual control, mutex not taken");
+                }
             }
             else if (message == "/reset" || message == "/r")
             {
                 // Raise an exception, so the ESP32 resets
                 throw std::runtime_error("Resetting ESP32");
             }
-
-            Serial.println("Received command from CLI: " + message);
-        }
-        else if (strcmp(topic, "esp32/cli") == 0)
-        {
-            String message = doc["message"];
-            if (message == "/auto")
-            {
-                // Release the mutex so the Raspberry Pi can take control
-                xSemaphoreGive(PidController::controlMutex);
-            }
-            else if (message == "/manual")
+            else
             {
-                // Take the mutex so the Wifi can take control
-                xSemaphoreTake(PidController::controlMutex, (TickType_t)0);
+                Serial.println("Unsupported command: " + message);
             }
-
-            Serial.println("Received command from CLI: " + message);
-        }
-        else if (strcmp(topic, "esp32/cli") == 0)
-        {
-            String message = doc["message"];
-            if (message == "/auto")
-            {
-                // Release the mutex so the Raspberry Pi can take control
-                xSemaphoreGive(PidController::controlMutex);
-            }
-            else if (message == "/manual")
-            {
-                // Take the mutex so the Wifi can take control
-                xSemaphoreTake(PidController::controlMutex, (TickType_t)0);
-            }
-
-            Serial.println("Received command from CLI: " + message);
         }
         else
         {

esp32/test/test_serial/test_serial.cpp

@@ -9,40 +9,34 @@ void tearDown(void)
 {
 }
 
-uint8_t encode(KeyDirection direction, uint8_t speed)
+uint8_t encode(KeyDirection direction)
 {
-    TEST_ASSERT_TRUE(speed < 32);
-    return (static_cast<uint8_t>(direction) << 5) | speed;
+    return static_cast<uint8_t>(direction);
 }
 
 void test_decode(void)
 {
     KeyDirection direction;
-    uint8_t speed;
 
-    char message = encode(KeyDirection::RIGHT, 0);
-    TEST_ASSERT_EQUAL_INT(0b01100000, message);
-
-    decode(encode(KeyDirection::RIGHT, 0), direction, speed);
+    char message = encode(KeyDirection::RIGHT);
+    decode(message, direction);
     TEST_ASSERT_EQUAL_INT(KeyDirection::RIGHT, direction);
-    TEST_ASSERT_EQUAL_INT(0, speed);
-
-    decode(encode(KeyDirection::FORWARD, 31), direction, speed);
-    TEST_ASSERT_EQUAL_INT(KeyDirection::FORWARD, direction);
-    TEST_ASSERT_EQUAL_INT(31, speed);
 
-    decode(encode(KeyDirection::LEFT, 18), direction, speed);
+    message = encode(KeyDirection::LEFT);
+    decode(message, direction);
     TEST_ASSERT_EQUAL_INT(KeyDirection::LEFT, direction);
-    TEST_ASSERT_EQUAL_INT(18, speed);
 
-    decode(encode(KeyDirection::BACKWARD, 1), direction, speed);
+    message = encode(KeyDirection::FORWARD);
+    decode(message, direction);
+    TEST_ASSERT_EQUAL_INT(KeyDirection::FORWARD, direction);
+
+    message = encode(KeyDirection::BACKWARD);
+    decode(message, direction);
     TEST_ASSERT_EQUAL_INT(KeyDirection::BACKWARD, direction);
-    TEST_ASSERT_EQUAL_INT(1, speed);
 
-    // Stop
-    decode(encode(KeyDirection::STOP, 1), direction, speed);
+    message = encode(KeyDirection::STOP);
+    decode(message, direction);
     TEST_ASSERT_EQUAL_INT(KeyDirection::STOP, direction);
-    TEST_ASSERT_EQUAL_INT(1, speed);
 }
 
 int main()

pi/uart.py

@@ -1,19 +1,74 @@
 #!/usr/bin/python3
 
+"""
+Turns left for one second
+Turns right for one second
+
+enum KeyDirection
+{
+    FORWARD = 0,
+    BACKWARD = 1,
+    LEFT = 2,
+    RIGHT = 3,
+    STOP = 4
+};
+
+
+Alternates
+"""
+
 import serial
 import time
+import threading
 
-ser = serial.Serial("/dev/ttyUSB0", 115200)
+ser = serial.Serial("/dev/ttyUSB1", 115200)
 # ser = serial.Serial("COM4", 115200)
 
 time.sleep(1)
 
+def turnRight():
+    ser.write((0b011).to_bytes(1, 'big'))
+
+def turnLeft():
+    ser.write((0b010).to_bytes(1, 'big'))
+
+def stop():
+    ser.write((0b100).to_bytes(1, 'big'))
+
+def forward():
+    ser.write((0b000).to_bytes(1, 'big'))
+
+def backward():
+    ser.write((0b001).to_bytes(1, 'big'))
+
+def readSerial():
+    while True:
+        print(ser.readline().decode('utf-8').strip())
+
+def controlMovement():
+    while True:
+        forward()
+        time.sleep(1)
+        stop()
+        time.sleep(1)
+        backward()
+        time.sleep(1)
+        stop()
+        time.sleep(1)
+        turnRight()
+        time.sleep(1)
+        stop()
+        time.sleep(1)
+        turnLeft()
+        time.sleep(1)
+        stop()
+        time.sleep(1)
 try:
+    t1 = threading.Thread(target=readSerial, daemon=True).start()
+    t2 = threading.Thread(target=controlMovement, daemon=True).start()
+
     while True:
-        print(ser.readline().decode('utf-8').strip()) 
-        # 0b011 - Turn Right
-        # 0b10000 - Speed = 16
-        ser.write((0b01110000).to_bytes(1, 'big'))
+        time.sleep(1)
 except KeyboardInterrupt:
     if ser != None:
-        ser.close()
+        ser.close()