FIX read sensor after the openloop check

Author: askuric

src/BLDCMotor.cpp

@@ -247,14 +247,13 @@ int BLDCMotor::absoluteZeroSearch() {
 // Iterative function looping FOC algorithm, setting Uq on the Motor
 // The faster it can be run the better
 void BLDCMotor::loopFOC() {
-  
+  // if open-loop do nothing
+  if( controller==MotionControlType::angle_openloop || controller==MotionControlType::velocity_openloop ) return;
   // shaft angle
   shaft_angle = shaftAngle(); // read value even if motor is disabled to keep the monitoring updated
 
   // if disabled do nothing
   if(!enabled) return;
-  // if open-loop do nothing
-  if( controller==MotionControlType::angle_openloop || controller==MotionControlType::velocity_openloop ) return;
 
   // electrical angle - need shaftAngle to be called first
   electrical_angle = electricalAngle();

src/StepperMotor.cpp

@@ -216,13 +216,14 @@ int StepperMotor::absoluteZeroSearch() {
 // Iterative function looping FOC algorithm, setting Uq on the Motor
 // The faster it can be run the better
 void StepperMotor::loopFOC() {
-  // if disabled do nothing
-  if(!enabled) return; 
   // if open-loop do nothing
   if( controller==MotionControlType::angle_openloop || controller==MotionControlType::velocity_openloop ) return;
-
   // shaft angle 
   shaft_angle = shaftAngle();
+  
+  // if disabled do nothing
+  if(!enabled) return; 
+
   electrical_angle = electricalAngle();
 
   // set the phase voltage - FOC heart function :) 
@@ -235,15 +236,15 @@ void StepperMotor::loopFOC() {
 // - needs to be called iteratively it is asynchronous function
 // - if target is not set it uses motor.target value
 void StepperMotor::move(float new_target) {
+  // get angular velocity
+  shaft_velocity = shaftVelocity();
   // if disabled do nothing
   if(!enabled) return; 
   // downsampling (optional)
   if(motion_cnt++ < motion_downsample) return;
   motion_cnt = 0;
   // set internal target variable
   if(_isset(new_target) ) target = new_target;
-  // get angular velocity
-  shaft_velocity = shaftVelocity();
   // choose control loop
   switch (controller) {
     case MotionControlType::torque: