From 5125602875154e4923b21b4d4d17e65981eeb633 Mon Sep 17 00:00:00 2001 From: copper280z Date: Fri, 10 Nov 2023 21:49:39 -0500 Subject: [PATCH] Change references to BLDCDriver to FOCDriver in current sense base class change setPwm function in FOCDriver to accept 3 args, with a default value for the 3rd Change stepper driver classes to use 3 arg setPwm, ignoring 3rd arg. created InlineCurrentSenseSync class in lib/currentsense. fixed timer bug in calibrate encoder, in main defined a label for serialusb, to make it easier to swap out. --- .../src/common/base_classes/CurrentSense.cpp | 10 +- .../src/common/base_classes/CurrentSense.h | 6 +- .../src/common/base_classes/FOCDriver.h | 10 +- .../src/common/base_classes/StepperDriver.h | 4 +- .../src/drivers/StepperDriver2PWM.cpp | 4 +- .../src/drivers/StepperDriver2PWM.h | 2 +- .../src/drivers/StepperDriver4PWM.cpp | 5 +- .../src/drivers/StepperDriver4PWM.h | 3 +- .../currentsense/InlineCurrentSenseSync.cpp | 254 ++++++++++++++++++ .../lib/currentsense/InlineCurrentSenseSync.h | 75 ++++++ firmware/src/main.cpp | 55 ++-- 11 files changed, 390 insertions(+), 38 deletions(-) create mode 100644 firmware/lib/currentsense/InlineCurrentSenseSync.cpp create mode 100644 firmware/lib/currentsense/InlineCurrentSenseSync.h diff --git a/firmware/lib/Arduino-FOC/src/common/base_classes/CurrentSense.cpp b/firmware/lib/Arduino-FOC/src/common/base_classes/CurrentSense.cpp index 7a74447..e548c2d 100644 --- a/firmware/lib/Arduino-FOC/src/common/base_classes/CurrentSense.cpp +++ b/firmware/lib/Arduino-FOC/src/common/base_classes/CurrentSense.cpp @@ -110,12 +110,12 @@ DQCurrent_s CurrentSense::getFOCCurrents(float angle_el) /** Driver linking to the current sense */ -void CurrentSense::linkDriver(BLDCDriver *_driver) +void CurrentSense::linkDriver(FOCDriver *_driver) { driver = _driver; } -void CurrentSense::linkDriver(StepperDriver *_driver) -{ - driver = _driver; -} \ No newline at end of file +// void CurrentSense::linkDriver(StepperDriver *_driver) +// { +// driver = _driver; +// } diff --git a/firmware/lib/Arduino-FOC/src/common/base_classes/CurrentSense.h b/firmware/lib/Arduino-FOC/src/common/base_classes/CurrentSense.h index 00fa0cf..2672dff 100644 --- a/firmware/lib/Arduino-FOC/src/common/base_classes/CurrentSense.h +++ b/firmware/lib/Arduino-FOC/src/common/base_classes/CurrentSense.h @@ -24,8 +24,8 @@ class CurrentSense{ * Linking the current sense with the motor driver * Only necessary if synchronisation in between the two is required */ - void linkDriver(BLDCDriver *driver); - void linkDriver(StepperDriver *driver); + void linkDriver(FOCDriver *driver); + // void linkDriver(StepperDriver *driver); // variables bool skip_align = false; //!< variable signaling that the phase current direction should be verified during initFOC() @@ -72,4 +72,4 @@ class CurrentSense{ DQCurrent_s getFOCCurrents(float angle_el); }; -#endif \ No newline at end of file +#endif diff --git a/firmware/lib/Arduino-FOC/src/common/base_classes/FOCDriver.h b/firmware/lib/Arduino-FOC/src/common/base_classes/FOCDriver.h index 67343f7..7994134 100644 --- a/firmware/lib/Arduino-FOC/src/common/base_classes/FOCDriver.h +++ b/firmware/lib/Arduino-FOC/src/common/base_classes/FOCDriver.h @@ -1,7 +1,8 @@ -#ifndef FOCMOTOR_H -#define FOTMOTOR_H +#ifndef FOCDRIVER_H +#define FOCDRIVER_H #include "Arduino.h" +#include "../foc_utils.h" class FOCDriver{ public: @@ -11,6 +12,9 @@ class FOCDriver{ virtual void disable() = 0; + virtual void setPwm(float a, float b, float c); + // virtual void setPwm(uint8_t a, uint8_t b); + long pwm_frequency; float voltage_power_supply; float voltage_limit; @@ -18,4 +22,4 @@ class FOCDriver{ void* params = 0; }; -#endif \ No newline at end of file +#endif diff --git a/firmware/lib/Arduino-FOC/src/common/base_classes/StepperDriver.h b/firmware/lib/Arduino-FOC/src/common/base_classes/StepperDriver.h index f0369ec..1793164 100644 --- a/firmware/lib/Arduino-FOC/src/common/base_classes/StepperDriver.h +++ b/firmware/lib/Arduino-FOC/src/common/base_classes/StepperDriver.h @@ -12,7 +12,7 @@ class StepperDriver: public FOCDriver{ * @param Ua phase A voltage * @param Ub phase B voltage */ - virtual void setPwm(float Ua, float Ub) = 0; + virtual void setPwm(float Ua, float Ub, float Uc=NOT_SET) = 0; }; -#endif \ No newline at end of file +#endif diff --git a/firmware/lib/Arduino-FOC/src/drivers/StepperDriver2PWM.cpp b/firmware/lib/Arduino-FOC/src/drivers/StepperDriver2PWM.cpp index dbbf5b8..f690d42 100644 --- a/firmware/lib/Arduino-FOC/src/drivers/StepperDriver2PWM.cpp +++ b/firmware/lib/Arduino-FOC/src/drivers/StepperDriver2PWM.cpp @@ -86,7 +86,7 @@ int StepperDriver2PWM::init() { // Set voltage to the pwm pin -void StepperDriver2PWM::setPwm(float Ua, float Ub) { +void StepperDriver2PWM::setPwm(float Ua, float Ub, float Uc) { float duty_cycle1(0.0f),duty_cycle2(0.0f); // limit the voltage in driver Ua = _constrain(Ua, -voltage_limit, voltage_limit); @@ -104,4 +104,4 @@ void StepperDriver2PWM::setPwm(float Ua, float Ub) { // write to hardware _writeDutyCycle2PWM(duty_cycle1, duty_cycle2, params); -} \ No newline at end of file +} diff --git a/firmware/lib/Arduino-FOC/src/drivers/StepperDriver2PWM.h b/firmware/lib/Arduino-FOC/src/drivers/StepperDriver2PWM.h index b349af0..6b6f978 100644 --- a/firmware/lib/Arduino-FOC/src/drivers/StepperDriver2PWM.h +++ b/firmware/lib/Arduino-FOC/src/drivers/StepperDriver2PWM.h @@ -58,7 +58,7 @@ class StepperDriver2PWM: public StepperDriver * @param Ua phase A voltage * @param Ub phase B voltage */ - void setPwm(float Ua, float Ub) override; + void setPwm(float Ua, float Ub, float NOT_USED=NOT_SET) override; private: diff --git a/firmware/lib/Arduino-FOC/src/drivers/StepperDriver4PWM.cpp b/firmware/lib/Arduino-FOC/src/drivers/StepperDriver4PWM.cpp index 836f547..9777855 100644 --- a/firmware/lib/Arduino-FOC/src/drivers/StepperDriver4PWM.cpp +++ b/firmware/lib/Arduino-FOC/src/drivers/StepperDriver4PWM.cpp @@ -60,8 +60,9 @@ int StepperDriver4PWM::init() { } + // Set voltage to the pwm pin -void StepperDriver4PWM::setPwm(float Ualpha, float Ubeta) { +void StepperDriver4PWM::setPwm(float Ualpha, float Ubeta, float Uc) { float duty_cycle1A(0.0f),duty_cycle1B(0.0f),duty_cycle2A(0.0f),duty_cycle2B(0.0f); // limit the voltage in driver Ualpha = _constrain(Ualpha, -voltage_limit, voltage_limit); @@ -78,4 +79,4 @@ void StepperDriver4PWM::setPwm(float Ualpha, float Ubeta) { duty_cycle2A = _constrain(abs(Ubeta)/voltage_power_supply,0.0f,1.0f); // write to hardware _writeDutyCycle4PWM(duty_cycle1A, duty_cycle1B, duty_cycle2A, duty_cycle2B, params); -} \ No newline at end of file +} diff --git a/firmware/lib/Arduino-FOC/src/drivers/StepperDriver4PWM.h b/firmware/lib/Arduino-FOC/src/drivers/StepperDriver4PWM.h index e4b2ee4..e10b824 100644 --- a/firmware/lib/Arduino-FOC/src/drivers/StepperDriver4PWM.h +++ b/firmware/lib/Arduino-FOC/src/drivers/StepperDriver4PWM.h @@ -45,7 +45,8 @@ class StepperDriver4PWM: public StepperDriver * @param Ua phase A voltage * @param Ub phase B voltage */ - void setPwm(float Ua, float Ub) override; + void setPwm(float Ua, float Ub, float NOT_USED = NOT_SET) override; + // void setPwm(float Ua, float Ub) override; private: diff --git a/firmware/lib/currentsense/InlineCurrentSenseSync.cpp b/firmware/lib/currentsense/InlineCurrentSenseSync.cpp new file mode 100644 index 0000000..bffa346 --- /dev/null +++ b/firmware/lib/currentsense/InlineCurrentSenseSync.cpp @@ -0,0 +1,254 @@ +#include "InlineCurrentSenseSync.h" +#include "communication/SimpleFOCDebug.h" +// InlineCurrentSensor constructor +// - shunt_resistor - shunt resistor value +// - gain - current-sense op-amp gain +// - phA - A phase adc pin +// - phB - B phase adc pin +// - phC - C phase adc pin (optional) +InlineCurrentSenseSync::InlineCurrentSenseSync(float _shunt_resistor, float _gain, int _pinA, int _pinB, int _pinC){ + pinA = _pinA; + pinB = _pinB; + pinC = _pinC; + + shunt_resistor = _shunt_resistor; + amp_gain = _gain; + volts_to_amps_ratio = 1.0f /_shunt_resistor / _gain; // volts to amps + // gains for each phase + gain_a = volts_to_amps_ratio; + gain_b = volts_to_amps_ratio; + gain_c = volts_to_amps_ratio; +}; + + +InlineCurrentSenseSync::InlineCurrentSenseSync(float _mVpA, int _pinA, int _pinB, int _pinC){ + pinA = _pinA; + pinB = _pinB; + pinC = _pinC; + + volts_to_amps_ratio = 1000.0f / _mVpA; // mV to amps + // gains for each phase + gain_a = volts_to_amps_ratio; + gain_b = volts_to_amps_ratio; + gain_c = volts_to_amps_ratio; +}; + + + +// Inline sensor init function +int InlineCurrentSenseSync::init(){ + // if no linked driver its fine in this case + // at least for init() + void* drv_params = driver ? driver->params : nullptr; + // configure ADC variables + params = _configureADCInline(drv_params,pinA,pinB,pinC); + // if init failed return fail + if (params == SIMPLEFOC_CURRENT_SENSE_INIT_FAILED) return 0; + _driverSyncLowSide(driver->params, params); + // calibrate zero offsets + calibrateOffsets(); + // set the initialized flag + initialized = (params!=SIMPLEFOC_CURRENT_SENSE_INIT_FAILED); + // return success + return 1; +} +// Function finding zero offsets of the ADC +void InlineCurrentSenseSync::calibrateOffsets(){ + const int calibration_rounds = 1000; + + // find adc offset = zero current voltage + offset_ia = 0; + offset_ib = 0; + offset_ic = 0; + // read the adc voltage 1000 times ( arbitrary number ) + for (int i = 0; i < calibration_rounds; i++) { + if(_isset(pinA)) offset_ia += _readADCVoltageInline(pinA, params); + if(_isset(pinB)) offset_ib += _readADCVoltageInline(pinB, params); + if(_isset(pinC)) offset_ic += _readADCVoltageInline(pinC, params); + _delay(1); + } + // calculate the mean offsets + if(_isset(pinA)) offset_ia = offset_ia / calibration_rounds; + if(_isset(pinB)) offset_ib = offset_ib / calibration_rounds; + if(_isset(pinC)) offset_ic = offset_ic / calibration_rounds; +} + +// read all three phase currents (if possible 2 or 3) +PhaseCurrent_s InlineCurrentSenseSync::getPhaseCurrents(){ + PhaseCurrent_s current; + current.a = (!_isset(pinA)) ? 0 : (_readADCVoltageInline(pinA, params) - offset_ia)*gain_a;// amps + current.b = (!_isset(pinB)) ? 0 : (_readADCVoltageInline(pinB, params) - offset_ib)*gain_b;// amps + current.c = (!_isset(pinC)) ? 0 : (_readADCVoltageInline(pinC, params) - offset_ic)*gain_c; // amps + return current; +} + +// Function aligning the current sense with motor driver +// if all pins are connected well none of this is really necessary! - can be avoided +// returns flag +// 0 - fail +// 1 - success and nothing changed +// 2 - success but pins reconfigured +// 3 - success but gains inverted +// 4 - success but pins reconfigured and gains inverted +int InlineCurrentSenseSync::driverAlign(float voltage){ + + int exit_flag = 1; + if(skip_align) return exit_flag; + + if (driver==nullptr) { + SIMPLEFOC_DEBUG("CUR: No driver linked!"); + return 0; + } + + if (!initialized) return 0; + + if(_isset(pinA)){ + // set phase A active and phases B and C down + driver->setPwm(voltage, 0, 0); + _delay(2000); + PhaseCurrent_s c = getPhaseCurrents(); + // read the current 100 times ( arbitrary number ) + for (int i = 0; i < 100; i++) { + PhaseCurrent_s c1 = getPhaseCurrents(); + c.a = c.a*0.6f + 0.4f*c1.a; + c.b = c.b*0.6f + 0.4f*c1.b; + c.c = c.c*0.6f + 0.4f*c1.c; + _delay(3); + } + driver->setPwm(0, 0, 0); + // align phase A + float ab_ratio = c.b ? fabs(c.a / c.b) : 0; + float ac_ratio = c.c ? fabs(c.a / c.c) : 0; + if(_isset(pinB) && ab_ratio > 1.5f ){ // should be ~2 + gain_a *= _sign(c.a); + }else if(_isset(pinC) && ac_ratio > 1.5f ){ // should be ~2 + gain_a *= _sign(c.a); + }else if(_isset(pinB) && ab_ratio < 0.7f ){ // should be ~0.5 + // switch phase A and B + int tmp_pinA = pinA; + pinA = pinB; + pinB = tmp_pinA; + float tmp_offsetA = offset_ia; + offset_ia = offset_ib; + offset_ib = tmp_offsetA; + gain_a *= _sign(c.b); + exit_flag = 2; // signal that pins have been switched + }else if(_isset(pinC) && ac_ratio < 0.7f ){ // should be ~0.5 + // switch phase A and C + int tmp_pinA = pinA; + pinA = pinC; + pinC= tmp_pinA; + float tmp_offsetA = offset_ia; + offset_ia = offset_ic; + offset_ic = tmp_offsetA; + gain_a *= _sign(c.c); + exit_flag = 2;// signal that pins have been switched + }else{ + // error in current sense - phase either not measured or bad connection + return 0; + } + } + + if(_isset(pinB)){ + // set phase B active and phases A and C down + driver->setPwm(0, voltage, 0); + _delay(200); + PhaseCurrent_s c = getPhaseCurrents(); + // read the current 50 times + for (int i = 0; i < 100; i++) { + PhaseCurrent_s c1 = getPhaseCurrents(); + c.a = c.a*0.6 + 0.4f*c1.a; + c.b = c.b*0.6 + 0.4f*c1.b; + c.c = c.c*0.6 + 0.4f*c1.c; + _delay(3); + } + driver->setPwm(0, 0, 0); + float ba_ratio = c.a ? fabs(c.b / c.a) : 0; + float bc_ratio = c.c ? fabs(c.b / c.c) : 0; + if(_isset(pinA) && ba_ratio > 1.5f ){ // should be ~2 + gain_b *= _sign(c.b); + }else if(_isset(pinC) && bc_ratio > 1.5f ){ // should be ~2 + gain_b *= _sign(c.b); + }else if(_isset(pinA) && ba_ratio < 0.7f ){ // it should be ~0.5 + // switch phase A and B + int tmp_pinB = pinB; + pinB = pinA; + pinA = tmp_pinB; + float tmp_offsetB = offset_ib; + offset_ib = offset_ia; + offset_ia = tmp_offsetB; + gain_b *= _sign(c.a); + exit_flag = 2; // signal that pins have been switched + }else if(_isset(pinC) && bc_ratio < 0.7f ){ // should be ~0.5 + // switch phase A and C + int tmp_pinB = pinB; + pinB = pinC; + pinC = tmp_pinB; + float tmp_offsetB = offset_ib; + offset_ib = offset_ic; + offset_ic = tmp_offsetB; + gain_b *= _sign(c.c); + exit_flag = 2; // signal that pins have been switched + }else{ + // error in current sense - phase either not measured or bad connection + return 0; + } + } + + // if phase C measured + if(_isset(pinC)){ + // set phase C active and phases A and B down + driver->setPwm(0, 0, voltage); + _delay(200); + PhaseCurrent_s c = getPhaseCurrents(); + // read the adc voltage 500 times ( arbitrary number ) + for (int i = 0; i < 100; i++) { + PhaseCurrent_s c1 = getPhaseCurrents(); + c.a = c.a*0.6 + 0.4f*c1.a; + c.b = c.b*0.6 + 0.4f*c1.b; + c.c = c.c*0.6 + 0.4f*c1.c; + _delay(3); + } + driver->setPwm(0, 0, 0); + float ca_ratio = c.a ? fabs(c.c / c.a) : 0; + float cb_ratio = c.b ? fabs(c.c / c.b) : 0; + if(_isset(pinA) && ca_ratio > 1.5f ){ // should be ~2 + gain_c *= _sign(c.c); + }else if(_isset(pinB) && cb_ratio > 1.5f ){ // should be ~2 + gain_c *= _sign(c.c); + }else if(_isset(pinA) && ca_ratio < 0.7f ){ // it should be ~0.5 + // switch phase A and C + int tmp_pinC = pinC; + pinC = pinA; + pinA = tmp_pinC; + float tmp_offsetC = offset_ic; + offset_ic = offset_ia; + offset_ia = tmp_offsetC; + gain_c *= _sign(c.a); + exit_flag = 2; // signal that pins have been switched + }else if(_isset(pinB) && cb_ratio < 0.7f ){ // should be ~0.5 + // switch phase B and C + int tmp_pinC = pinC; + pinC = pinB; + pinB = tmp_pinC; + float tmp_offsetC = offset_ic; + offset_ic = offset_ib; + offset_ib = tmp_offsetC; + gain_c *= _sign(c.b); + exit_flag = 2; // signal that pins have been switched + }else{ + // error in current sense - phase either not measured or bad connection + return 0; + } + } + + if(gain_a < 0 || gain_b < 0 || gain_c < 0) exit_flag +=2; + // exit flag is either + // 0 - fail + // 1 - success and nothing changed + // 2 - success but pins reconfigured + // 3 - success but gains inverted + // 4 - success but pins reconfigured and gains inverted + + return exit_flag; +} diff --git a/firmware/lib/currentsense/InlineCurrentSenseSync.h b/firmware/lib/currentsense/InlineCurrentSenseSync.h new file mode 100644 index 0000000..43798e2 --- /dev/null +++ b/firmware/lib/currentsense/InlineCurrentSenseSync.h @@ -0,0 +1,75 @@ +#ifndef INLINE_CS_SYNC_LIB_H +#define INLINE_CS_SYNC_LIB_H + +#include "Arduino.h" +#include "SimpleFOC.h" +#include "common/foc_utils.h" +#include "common/time_utils.h" +#include "common/defaults.h" +#include "common/base_classes/CurrentSense.h" +#include "common/lowpass_filter.h" +// #include "current_sense/hardware_api.h" +#include "utils.h" + + +class InlineCurrentSenseSync: public CurrentSense{ + public: + /** + InlineCurrentSense class constructor + @param shunt_resistor shunt resistor value + @param gain current-sense op-amp gain + @param phA A phase adc pin + @param phB B phase adc pin + @param phC C phase adc pin (optional) + */ + InlineCurrentSenseSync(float shunt_resistor, float gain, int pinA, int pinB, int pinC = NOT_SET); + /** + InlineCurrentSense class constructor + @param mVpA mV per Amp ratio + @param phA A phase adc pin + @param phB B phase adc pin + @param phC C phase adc pin (optional) + */ + InlineCurrentSenseSync(float mVpA, int pinA, int pinB, int pinC = NOT_SET); + + // CurrentSense interface implementing functions + int init() override; + PhaseCurrent_s getPhaseCurrents() override; + int driverAlign(float align_voltage) override; + + // ADC measuremnet gain for each phase + // support for different gains for different phases of more commonly - inverted phase currents + // this should be automated later + float gain_a; //!< phase A gain + float gain_b; //!< phase B gain + float gain_c; //!< phase C gain + + // // per phase low pass fileters + // LowPassFilter lpf_a{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current A low pass filter + // LowPassFilter lpf_b{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current B low pass filter + // LowPassFilter lpf_c{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current C low pass filter + + float offset_ia; //!< zero current A voltage value (center of the adc reading) + float offset_ib; //!< zero current B voltage value (center of the adc reading) + float offset_ic; //!< zero current C voltage value (center of the adc reading) + + private: + + // hardware variables + int pinA; //!< pin A analog pin for current measurement + int pinB; //!< pin B analog pin for current measurement + int pinC; //!< pin C analog pin for current measurement + + // gain variables + float shunt_resistor; //!< Shunt resistor value + float amp_gain; //!< amp gain value + float volts_to_amps_ratio; //!< Volts to amps ratio + + /** + * Function finding zero offsets of the ADC + */ + void calibrateOffsets(); + +}; + +#endif diff --git a/firmware/src/main.cpp b/firmware/src/main.cpp index bffb6d2..908e528 100644 --- a/firmware/src/main.cpp +++ b/firmware/src/main.cpp @@ -13,6 +13,7 @@ #include "can.h" #include "dfu.h" #include "utils.h" +#include "InlineCurrentSenseSync.h" #include "lemon-pepper.h" #define USBD_MANUFACTURER_STRING "matei repair lab" @@ -44,6 +45,8 @@ extern uint8_t RxData[8]; #define MOTORKV 40 #define ENC_PPR 16383 // max 16383 (zero index) -> *4 for CPR, -1 is done in init to prevent rollover on 16 bit timer +#define SERIALPORT SerialUSB + /** * SPI clockdiv of 16 gives ~10.5MHz clock. May still be stable with lower divisor. * The HW encoder is configured using PPR, which is then *4 for CPR (full 12384 gives overflow on 16 bit timer.) @@ -56,11 +59,11 @@ STM32HWEncoder enc = STM32HWEncoder(ENC_PPR, ENC_A, ENC_B, ENC_Z); * The current sense amps have a gain of 90mA/V -> over 1.5A this is 135mA so we need gain of 24 to get full-scale. * Actually we are limited to powers of 2 for gain. So it should be 16. This gives sensitivity of 1440mV/A. * */ -InlineCurrentSense currentsense = InlineCurrentSense(1440, ISENSE_U, ISENSE_V, ISENSE_W); +InlineCurrentSenseSync currentsense = InlineCurrentSenseSync(1440, ISENSE_U, ISENSE_V); StepperDriver4PWM driver = StepperDriver4PWM(MOT_A1, MOT_A2, MOT_B1, MOT_B2); StepperMotor motor = StepperMotor(POLEPAIRS); -Commander commander = Commander(SerialUSB); +Commander commander = Commander(SERIALPORT); uint16_t counter = 0; @@ -76,17 +79,23 @@ void setup() pinMode(CAL_EN, OUTPUT); pinMode(MOT_EN, OUTPUT); - SerialUSB.begin(115200); + SERIALPORT.begin(115200); EEPROM.get(0, boardData); digitalWrite(MOT_EN, HIGH); digitalWrite(CAL_EN, LOW); - if (!configureCAN()) + uint8_t ret = configureCAN(); + if (!ret){ SIMPLEFOC_DEBUG("CAN init failed."); - if (!configureFOC()) + digitalWrite(LED_FAULT, HIGH); + } + ret = configureFOC(); + if (!ret){ SIMPLEFOC_DEBUG("FOC init failed."); + digitalWrite(LED_FAULT, HIGH); + } if (sensor.getABZResolution() != ENC_PPR) // Check that PPR of the encoder matches our expectation. { @@ -137,16 +146,24 @@ void loop() motor.move(); commander.run(); - if (counter == 0xFFF) - { - digitalToggle(LED_GOOD); - SerialUSB.print(sensor.getAngle()); - SerialUSB.print("\t"); - SerialUSB.print(enc.getAngle()); - SerialUSB.print("\t"); - SerialUSB.println(sensor.getABZResolution()); - // SerialUSB.printf("%d\t%d\t%d\n", sensor.getAngle(), sensor.getABZResolution(), enc.getAngle()); + + if (counter == 0xFF) + { + PhaseCurrent_s current = currentsense.getPhaseCurrents(); + digitalToggle(LED_GOOD); + SERIALPORT.print(current.a); + SERIALPORT.print("\t"); + SERIALPORT.print(current.b); + SERIALPORT.print("\t"); + SERIALPORT.println(current.c); + // SERIALPORT.print(sensor.getAngle()); + // SERIALPORT.print("\t"); + // SERIALPORT.print(enc.getAngle()); + // SERIALPORT.print("\t"); + // SERIALPORT.println(sensor.getABZResolution()); + + // SERIALPORT.printf("%d\t%d\t%d\n", sensor.getAngle(), sensor.getABZResolution(), enc.getAngle()); counter = 0; } @@ -168,7 +185,7 @@ uint8_t configureFOC(void) commander.verbose = VerboseMode::machine_readable; #ifdef SIMPLEFOC_STM32_DEBUG - SimpleFOCDebug::enable(&SerialUSB); + SimpleFOCDebug::enable(&SERIALPORT); #endif // Encoder initialization. @@ -227,7 +244,7 @@ uint8_t configureFOC(void) // Monitor initialization #ifdef HAS_MONITOR - motor.useMonitoring(SerialUSB); + motor.useMonitoring(SERIALPORT); motor.monitor_start_char = 'M'; motor.monitor_end_char = 'M'; motor.monitor_downsample = 250; @@ -283,13 +300,13 @@ uint8_t calibrateEncoder(void) currentSettings.autocal_freq = 0x1; sensor.setOptions4(currentSettings); - uint16_t calTime = micros(); - while (calTime - micros() < 2000000) + uint32_t calTime = micros(); + while ((micros() - calTime) < 2000000) { motor.loopFOC(); motor.move(); - if (calTime - micros() > 2000) + if ((micros() -calTime) > 2000) { // after motor is spinning at constant speed, enable calibration. digitalWrite(LED_GOOD, HIGH);