From d698a872bfefdc15273ac5115dc88dd0e5a272d8 Mon Sep 17 00:00:00 2001
From: Alden Hart <alden08@harts.org>
Date: Sun, 18 Sep 2016 11:03:48 -0400
Subject: [PATCH] Runs full regression on motor power. Ready to push.

---
 g2core/stepper.cpp | 25 +++++++++++++------------
 g2core/stepper.h   | 27 +++++++++++++++++----------
 2 files changed, 30 insertions(+), 22 deletions(-)

diff --git a/g2core/stepper.cpp b/g2core/stepper.cpp
index cd882577..d60b5a0d 100644
--- a/g2core/stepper.cpp
+++ b/g2core/stepper.cpp
@@ -210,9 +210,7 @@ void st_energize_motors(float timeout_seconds)
 void st_deenergize_motors()
 {
     for (uint8_t motor = MOTOR_1; motor < MOTORS; motor++) {
-		if (Motors[motor]->getPowerMode() != MOTOR_ALWAYS_POWERED) {
-			Motors[motor]->disable();
-		}
+        Motors[motor]->disable();
     }
 }
 
@@ -252,7 +250,7 @@ stat_t st_motor_power_callback()     // called by controller
  *    - fire on overflow
  *    - clear interrupt condition
  *    - clear all step pins - this clears those the were set during the previous interrupt
- *    - if downcount == 0  and stop the timer and exit
+ *    - if downcount == 0 and stop the timer and exit
  *    - run the DDA for each channel
  *    - decrement the downcount - if it reaches zero load the next segment
  *
@@ -266,11 +264,11 @@ void dda_timer_type::interrupt()
 {
     dda_timer.getInterruptCause();        // clear interrupt condition
 
-//    for (uint8_t motor=0; motor<MOTORS; motor++) {
-//        Motors[motor]->stepEnd();
-//    }
-
     // clear all steps from the previous interrupt
+	// for (uint8_t motor=0; motor<MOTORS; motor++) {
+	//	  Motors[motor]->stepEnd();
+	// }
+	// loop unrolled version (it's actually faster)
     motor_1.stepEnd();
     motor_2.stepEnd();
 #if MOTORS > 2
@@ -446,7 +444,7 @@ namespace Motate {    // Define timer inside Motate namespace
  *  In aline() code:
  *   - All axes must set steps and compensate for out-of-range pulse phasing.
  *   - If axis has 0 steps the direction setting can be omitted
- *   - If axis has 0 steps the motor must not be enabled to support power mode = 1
+ *   - If axis has 0 steps the motor power must be set accord to the power mode
  */
 
 static void _load_move()
@@ -457,9 +455,12 @@ static void _load_move()
         return;                                                    // exit if the runtime is busy
     }
     if (st_pre.buffer_state != PREP_BUFFER_OWNED_BY_LOADER) {    // if there are no moves to load...
-//        for (uint8_t motor = MOTOR_1; motor < MOTORS; motor++) {
-//            Motors[motor]->motionStopped();    // ...start motor power timeouts
-//        }
+		
+	// ...start motor power timeouts
+	//	for (uint8_t motor = MOTOR_1; motor < MOTORS; motor++) {
+	//		Motors[motor]->motionStopped();    
+	//  }
+	// loop unrolled version
         motor_1.motionStopped();    // ...start motor power timeouts
         motor_2.motionStopped();    // ...start motor power timeouts
 #if (MOTORS > 2)
diff --git a/g2core/stepper.h b/g2core/stepper.h
index 4f1df4d2..2bb1d36f 100644
--- a/g2core/stepper.h
+++ b/g2core/stepper.h
@@ -472,32 +472,39 @@ struct Stepper {
         return (st_cfg.mot[motor].power_level);
     };
 
-    bool isDisabled()
-    {
-        return (_power_mode == MOTOR_DISABLED);
-    };
+//    bool isDisabled()
+//    {
+//        return (_power_mode == MOTOR_DISABLED);
+//    };
     
+    // turn on motor in all cases unless it's disabled
     void enable(float timeout = st_cfg.motor_power_timeout)
     {
-        if (_power_mode != MOTOR_DISABLED) {
-            this->_enableImpl();
-            _power_state = MOTOR_RUNNING;
-            _motor_disable_timeout_ms = timeout * 1000.0;
+        if (_power_mode == MOTOR_DISABLED) {
+            return;
         }
+        this->_enableImpl();
+        _power_state = MOTOR_RUNNING;
+        _motor_disable_timeout_ms = timeout * 1000.0;
     };
-    
+
+    // turn off motor in all cases unless it's permanently enabled
     void disable()
     {
+        if (this->getPowerMode() == MOTOR_ALWAYS_POWERED) {
+            return;
+        }
         this->_disableImpl();
         _motor_disable_timeout.clear();
         _power_state = MOTOR_IDLE; // or MOTOR_OFF
     };
     
+    // turn off motor is only powered when moving
     void motionStopped() {
         if (_power_mode == MOTOR_POWERED_IN_CYCLE) {
             this->enable();
+            _power_state = MOTOR_POWER_TIMEOUT_START;
         }
-        _power_state = MOTOR_POWER_TIMEOUT_START;
     };
     
     virtual void periodicCheck(bool have_actually_stopped) // can be overridden