Merge pull request #143 from runger1101001/motor_status

support motor intialization status tracking

Author: runger1101001

src/BLDCMotor.cpp

@@ -24,6 +24,11 @@ void BLDCMotor::linkDriver(BLDCDriver* _driver) {
 
 // init hardware pins
 void BLDCMotor::init() {
+  if (!driver || !driver->initialized) {
+    motor_status = FOCMotorStatus::motor_init_failed;
+    if(monitor_port) monitor_port->println(F("MOT: Init not possible, driver not initialized"));
+  }
+  motor_status = FOCMotorStatus::motor_initializing;
   if(monitor_port) monitor_port->println(F("MOT: Init"));
 
   // if no current sensing and the user has set the phase resistance of the motor use current limit to calculate the voltage limit
@@ -57,6 +62,7 @@ void BLDCMotor::init() {
   if(monitor_port) monitor_port->println(F("MOT: Enable driver."));
   enable();
   _delay(500);
+  motor_status = FOCMotorStatus::motor_uncalibrated;
 }
 
 
@@ -87,6 +93,9 @@ void BLDCMotor::enable()
 // FOC initialization function
 int  BLDCMotor::initFOC( float zero_electric_offset, Direction _sensor_direction) {
   int exit_flag = 1;
+
+  motor_status = FOCMotorStatus::motor_calibrating;
+
   // align motor if necessary
   // alignment necessary for encoders!
   if(_isset(zero_electric_offset)){
@@ -117,9 +126,11 @@ int  BLDCMotor::initFOC( float zero_electric_offset, Direction _sensor_direction
 
   if(exit_flag){
     if(monitor_port) monitor_port->println(F("MOT: Ready."));
+    motor_status = FOCMotorStatus::motor_ready;
   }else{
     if(monitor_port) monitor_port->println(F("MOT: Init FOC failed."));
     disable();
+    motor_status = FOCMotorStatus::motor_calib_failed;
   }
 
   return exit_flag;

src/StepperMotor.cpp

@@ -25,6 +25,11 @@ void StepperMotor::linkDriver(StepperDriver* _driver) {
 
 // init hardware pins
 void StepperMotor::init() {
+  if (!driver || !driver->initialized) {
+    motor_status = FOCMotorStatus::motor_init_failed;
+    if(monitor_port) monitor_port->println(F("MOT: Init not possible, driver not initialized"));
+  }
+  motor_status = FOCMotorStatus::motor_initializing;
   if(monitor_port) monitor_port->println(F("MOT: Init"));
 
   // if set the phase resistance of the motor use current limit to calculate the voltage limit
@@ -53,6 +58,7 @@ void StepperMotor::init() {
   enable();
   _delay(500);
 
+  motor_status = FOCMotorStatus::motor_uncalibrated;
 }
 
 
@@ -84,6 +90,9 @@ void StepperMotor::enable()
 // FOC initialization function
 int  StepperMotor::initFOC( float zero_electric_offset, Direction _sensor_direction ) {
   int exit_flag = 1;
+  
+  motor_status = FOCMotorStatus::motor_calibrating;
+
   // align motor if necessary
   // alignment necessary for encoders!
   if(_isset(zero_electric_offset)){
@@ -105,8 +114,10 @@ int  StepperMotor::initFOC( float zero_electric_offset, Direction _sensor_direct
 
   if(exit_flag){
     if(monitor_port) monitor_port->println(F("MOT: Ready."));
+    motor_status = FOCMotorStatus::motor_ready;
   }else{
     if(monitor_port) monitor_port->println(F("MOT: Init FOC failed."));
+    motor_status = FOCMotorStatus::motor_calib_failed;
     disable();
   }
 

src/common/base_classes/BLDCDriver.h

@@ -22,6 +22,8 @@ class BLDCDriver{
         float dc_b; //!< currently set duty cycle on phaseB
         float dc_c; //!< currently set duty cycle on phaseC
 
+        bool initialized = false; // true if driver was successfully initialized
+
         /**
          * Set phase voltages to the harware
          *

src/common/base_classes/FOCMotor.h

@@ -51,6 +51,21 @@ enum FOCModulationType : uint8_t {
   Trapezoid_150      = 0x03,     
 };
 
+
+
+enum FOCMotorStatus : uint8_t {
+  motor_uninitialized = 0x00,     //!< Motor is not yet initialized
+  motor_initializing  = 0x01,     //!< Motor intiialization is in progress
+  motor_uncalibrated  = 0x02,     //!< Motor is initialized, but not calibrated (open loop possible)
+  motor_calibrating   = 0x03,     //!< Motor calibration in progress
+  motor_ready         = 0x04,     //!< Motor is initialized and calibrated (closed loop possible)
+  motor_error         = 0x08,     //!< Motor is in error state (recoverable, e.g. overcurrent protection active)
+  motor_calib_failed  = 0x0E,     //!< Motor calibration failed (possibly recoverable)
+  motor_init_failed   = 0x0F,     //!< Motor initialization failed (not recoverable)
+};
+
+
+
 /**
  Generic motor class
 */
@@ -153,6 +168,7 @@ class FOCMotor
 
     // motor status vairables
     int8_t enabled = 0;//!< enabled or disabled motor flag
+    FOCMotorStatus motor_status = FOCMotorStatus::motor_uninitialized; //!< motor status
 
     // pwm modulation related variables
     FOCModulationType foc_modulation;//!<  parameter derterniming modulation algorithm

src/common/base_classes/StepperDriver.h

@@ -14,6 +14,8 @@ class StepperDriver{
         long pwm_frequency; //!< pwm frequency value in hertz
         float voltage_power_supply; //!< power supply voltage 
         float voltage_limit; //!< limiting voltage set to the motor
+        
+        bool initialized = false; // true if driver was successfully initialized
 
         /** 
          * Set phase voltages to the harware 

src/drivers/BLDCDriver3PWM.cpp

@@ -57,6 +57,7 @@ int BLDCDriver3PWM::init() {
   // Set the pwm frequency to the pins
   // hardware specific function - depending on driver and mcu
   _configure3PWM(pwm_frequency, pwmA, pwmB, pwmC);
+  initialized = true; // TODO atm the api gives no feedback if initialization succeeded
   return 0;
 }
 

src/drivers/BLDCDriver6PWM.cpp

@@ -58,7 +58,9 @@ int BLDCDriver6PWM::init() {
 
   // configure 6pwm
   // hardware specific function - depending on driver and mcu
-  return _configure6PWM(pwm_frequency, dead_zone, pwmA_h,pwmA_l, pwmB_h,pwmB_l, pwmC_h,pwmC_l);
+  int result = _configure6PWM(pwm_frequency, dead_zone, pwmA_h,pwmA_l, pwmB_h,pwmB_l, pwmC_h,pwmC_l);
+  initialized = (result==0);
+  return result;
 }
 
 // Set voltage to the pwm pin

src/drivers/StepperDriver2PWM.cpp

@@ -80,6 +80,7 @@ int StepperDriver2PWM::init() {
   // Set the pwm frequency to the pins
   // hardware specific function - depending on driver and mcu
   _configure2PWM(pwm_frequency, pwm1, pwm2);
+  initialized = true; // TODO atm the api gives no feedback if initialization succeeded
   return 0;
 }
 

src/drivers/StepperDriver4PWM.cpp

@@ -55,6 +55,7 @@ int StepperDriver4PWM::init() {
   // Set the pwm frequency to the pins
   // hardware specific function - depending on driver and mcu
   _configure4PWM(pwm_frequency, pwm1A, pwm1B, pwm2A, pwm2B);
+  initialized = true; // TODO atm the api gives no feedback if initialization succeeded
   return 0;
 }