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

…ng-system

Feature/amrap 241 software braking system

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)
 

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;