From 101f78dcd95920810d903369c0ea64c6c21517e1 Mon Sep 17 00:00:00 2001
From: Paul Guenette <pfguenette@gmail.com>
Date: Tue, 13 Oct 2020 22:41:37 -0400
Subject: [PATCH] Small updates to endstops & homing

---
 Firmware/MotorControl/axis.cpp    |  5 +++--
 Firmware/MotorControl/endstop.cpp |  5 +----
 Firmware/MotorControl/endstop.hpp | 13 ++++++++++++-
 3 files changed, 16 insertions(+), 7 deletions(-)

diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp
index 52150800..53d53e73 100644
--- a/Firmware/MotorControl/axis.cpp
+++ b/Firmware/MotorControl/axis.cpp
@@ -189,9 +189,9 @@ bool Axis::do_checks() {
     // controller_.do_checks();
 
     // Check for endstop presses
-    if (min_endstop_.config_.enabled && min_endstop_.get_state() && !(current_state_ == AXIS_STATE_HOMING)) {
+    if (min_endstop_.config_.enabled && min_endstop_.rose() && !(current_state_ == AXIS_STATE_HOMING)) {
         error_ |= ERROR_MIN_ENDSTOP_PRESSED;
-    } else if (max_endstop_.config_.enabled && max_endstop_.get_state() && !(current_state_ == AXIS_STATE_HOMING)) {
+    } else if (max_endstop_.config_.enabled && max_endstop_.rose() && !(current_state_ == AXIS_STATE_HOMING)) {
         error_ |= ERROR_MAX_ENDSTOP_PRESSED;
     }
 
@@ -410,6 +410,7 @@ bool Axis::run_homing() {
     // Avoid integrator windup issues
     controller_.vel_integrator_torque_ = 0.0f;
 
+    // Driving toward the endstop
     run_control_loop([this](){
         // Note that all estimators are updated in the loop prefix in run_control_loop
         float torque_setpoint;
diff --git a/Firmware/MotorControl/endstop.cpp b/Firmware/MotorControl/endstop.cpp
index e6878edc..2e5b9332 100644
--- a/Firmware/MotorControl/endstop.cpp
+++ b/Firmware/MotorControl/endstop.cpp
@@ -3,6 +3,7 @@
 
 void Endstop::update() {
     debounceTimer_.update();
+    last_state_ = endstop_state_;
     if (config_.enabled) {
         bool last_pin_state = pin_state_;
 
@@ -19,10 +20,6 @@ void Endstop::update() {
     }
 }
 
-bool Endstop::get_state() {
-    return endstop_state_;
-}
-
 bool Endstop::apply_config() {
     debounceTimer_.reset();
     if (config_.enabled) {
diff --git a/Firmware/MotorControl/endstop.hpp b/Firmware/MotorControl/endstop.hpp
index cbba2982..5126f55c 100644
--- a/Firmware/MotorControl/endstop.hpp
+++ b/Firmware/MotorControl/endstop.hpp
@@ -26,11 +26,22 @@ class Endstop {
     bool apply_config();
 
     void update();
-    bool get_state();
+    constexpr bool get_state(){
+        return endstop_state_;
+    }
+
+    constexpr bool rose(){
+        return (endstop_state_ != last_state_) && endstop_state_;
+    }
+
+    constexpr bool fell(){
+        return (endstop_state_ != last_state_) && !endstop_state_;
+    }
 
     bool endstop_state_ = false;
 
    private:
+    bool last_state_ = false;
     bool pin_state_ = false;
     float pos_when_pressed_ = 0.0f;
     Timer<float> debounceTimer_;