Merge pull request #7 from LexxPluss/feature/amrap-241-software-braki…

…ng-system Feature/amrap 241 software braking system
LexxPluss · Mar 25, 2024 · 5266819 · 5266819
2 parents 8f3efd4 + 9789874
commit 5266819
Show file tree

Hide file tree

Showing 2 changed files with 59 additions and 12 deletions.
diff --git a/README.md b/README.md
@@ -31,7 +31,7 @@ When writing on battery power, the power switch must be held down to prevent pow
 ### STLINK Tools (Open souce version)
 
 ```bash
-$ st-flash --connect-under-reset cmake_build/LEXXPLUSS_PB01/develop/GCC_ARM/LexxHard-PowerBoard-Firmware.bin 0x8000000
+$ st-flash --connect-under-reset write cmake_build/LEXXPLUSS_PB01/develop/GCC_ARM/LexxHard-PowerBoard-Firmware.bin 0x8000000
 ```
 
 ### STM32CubeProgrammer
@@ -68,6 +68,9 @@ Main Board and ROS Status (Published from Main Board)
 | 0 | Emergency stop | 1:asserted |
 | 1 | Power off | 1:asserted |
 | 2 | ROS Heartbeat timeout | 1:asserted |
+| 3 | mainboard overheat | |
+| 4 | actuatorboard overheat | |
+| 5 | wheel poweroff | |
 
 ### 514 (0x202)
 

diff --git a/main.cpp b/main.cpp
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2022, LexxPluss Inc.
+ * Copyright (c) 2022-2024, LexxPluss Inc.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -265,7 +265,15 @@ class emergency_switch { // Variables Implemented
         }
         if (left_count > COUNT) {
             left_count = COUNT;
+            bool left_asserted_prev{left_asserted};
             left_asserted = now == 1;
+            if (!left_asserted_prev && left_asserted) {
+                left_timer.reset();
+                left_timer.start();
+            } else if (!left_asserted) {
+                left_timer.reset();
+                left_timer.stop();
+            }
         }
         now = right.read();
         if (right_prev != now) {
@@ -276,10 +284,29 @@ class emergency_switch { // Variables Implemented
         }
         if (right_count > COUNT) {
             right_count = COUNT;
+            bool right_asserted_prev{right_asserted};
             right_asserted = now == 1;
+            if (!right_asserted_prev && right_asserted) {
+                right_timer.reset();
+                right_timer.start();
+            } else if (!right_asserted) {
+                right_timer.reset();
+                right_timer.stop();
+            }
+        }
+    }
+    bool asserted() {
+        auto left_elapsed{left_timer.elapsed_time().count()};
+        auto right_elapsed{right_timer.elapsed_time().count()};
+        if (left_elapsed  >= (DELAY_TIME_MS*1000) ||
+            right_elapsed >= (DELAY_TIME_MS*1000)) {
+            left_timer.stop();
+            right_timer.stop();
+            return true;
+        } else {
+            return false;
         }
     }
-    bool asserted() const {return left_asserted || right_asserted;}
     void get_raw_state(bool &left, bool &right) const {
         left = left_asserted;
         right = right_asserted;
@@ -289,7 +316,9 @@ class emergency_switch { // Variables Implemented
     uint32_t left_count{0}, right_count{0};
     int left_prev{-1}, right_prev{-1};
     bool left_asserted{false}, right_asserted{false};
+    Timer left_timer, right_timer;
     static constexpr uint32_t COUNT{5};
+    static constexpr uint16_t DELAY_TIME_MS{500};
 };
 
 class wheel_switch { // Variables Implemented
@@ -642,14 +671,20 @@ class mainboard_controller {  // No pins declared
         can.register_callback(0x201, callback(this, &mainboard_controller::handle_can));
     }
     void poll() {
-        if (timer.elapsed_time() > 3s) {
+        if (heartbeat_timer.elapsed_time() > 3s) {
             heartbeat_timeout = true;
-            timer.stop();
-            timer.reset();
+            heartbeat_timer.stop();
+            heartbeat_timer.reset();
         }
     }
-    bool emergency_stop_from_ros() const {
-        return emergency_stop;
+    bool emergency_stop_from_ros() {
+        auto elapsed{delay_timer.elapsed_time().count()};
+        if (elapsed > DELAY_TIME_MS*1000) {
+            delay_timer.stop();
+            return true;
+        } else {
+            return false;
+        }
     }
     bool power_off_from_ros() const {
         return power_off;
@@ -671,22 +706,31 @@ class mainboard_controller {  // No pins declared
     }
 private:
     void handle_can(const CANMessage &msg) {
+        bool emergency_stop_prev{emergency_stop};
         heartbeat_timeout = false;
-        timer.reset();
-        timer.start();
+        heartbeat_timer.reset();
+        heartbeat_timer.start();
         emergency_stop = msg.data[0] != 0;
         power_off = msg.data[1] != 0;
         ros_heartbeat_timeout = msg.data[2] != 0;
         mainboard_overheat = msg.data[3] != 0;
         actuatorboard_overheat = msg.data[4] != 0;
         wheel_poweroff = msg.data[5] != 0;
+        if (!emergency_stop_prev && emergency_stop) {
+            delay_timer.reset();
+            delay_timer.start();
+        } else if (!emergency_stop) {
+            delay_timer.reset();
+            delay_timer.stop();
+        }
         if (!ros_heartbeat_timeout)
             heartbeat_detect = true;
     }
     can_driver &can;
-    Timer timer;
+    Timer heartbeat_timer, delay_timer;
     bool heartbeat_timeout{true}, heartbeat_detect{false}, ros_heartbeat_timeout{false}, emergency_stop{true}, power_off{false},
          mainboard_overheat{false}, actuatorboard_overheat{false}, wheel_poweroff{false};
+    static constexpr uint16_t DELAY_TIME_MS{500};
 };
 
 class state_controller { // Variables Implemented
@@ -1061,7 +1105,7 @@ class state_controller { // Variables Implemented
         watchdog.kick();
     }
     void poll_10s() {
-        uint8_t buf[8]{'2', '0', '3'}; // version
+        uint8_t buf[8]{'2', '1', '0'}; // version
         can.send(CANMessage{0x203, buf});
     }
     can_driver can;