diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index d47ca51f..6b46572d 100644 --- a/src/BLDCMotor.cpp +++ b/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; diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp index 2acd6f11..99667030 100644 --- a/src/StepperMotor.cpp +++ b/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(); } diff --git a/src/common/base_classes/BLDCDriver.h b/src/common/base_classes/BLDCDriver.h index a657d17c..3e220d2e 100644 --- a/src/common/base_classes/BLDCDriver.h +++ b/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 * diff --git a/src/common/base_classes/FOCMotor.h b/src/common/base_classes/FOCMotor.h index 8184ad1c..272b4074 100644 --- a/src/common/base_classes/FOCMotor.h +++ b/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 diff --git a/src/common/base_classes/StepperDriver.h b/src/common/base_classes/StepperDriver.h index 33f08843..aa9f9e4d 100644 --- a/src/common/base_classes/StepperDriver.h +++ b/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 diff --git a/src/drivers/BLDCDriver3PWM.cpp b/src/drivers/BLDCDriver3PWM.cpp index bc239875..13cba6e8 100644 --- a/src/drivers/BLDCDriver3PWM.cpp +++ b/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; } diff --git a/src/drivers/BLDCDriver6PWM.cpp b/src/drivers/BLDCDriver6PWM.cpp index eaeb2cfa..5b2ba9d9 100644 --- a/src/drivers/BLDCDriver6PWM.cpp +++ b/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 diff --git a/src/drivers/StepperDriver2PWM.cpp b/src/drivers/StepperDriver2PWM.cpp index a0e61ca0..469ae694 100644 --- a/src/drivers/StepperDriver2PWM.cpp +++ b/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; } diff --git a/src/drivers/StepperDriver4PWM.cpp b/src/drivers/StepperDriver4PWM.cpp index 6c905348..e4305e76 100644 --- a/src/drivers/StepperDriver4PWM.cpp +++ b/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; }