diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index 76dc7032..bcc486e5 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -244,13 +244,15 @@ int BLDCMotor::absoluteZeroSearch() { // Iterative function looping FOC algorithm, setting Uq on the Motor // The faster it can be run the better void BLDCMotor::loopFOC() { + + // 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; - // shaft angle - shaft_angle = shaftAngle(); // electrical angle - need shaftAngle to be called first electrical_angle = electricalAngle(); @@ -295,6 +297,10 @@ void BLDCMotor::loopFOC() { // - needs to be called iteratively it is asynchronous function // - if target is not set it uses motor.target value void BLDCMotor::move(float new_target) { + + // get angular velocity + shaft_velocity = shaftVelocity(); // read value even if motor is disabled to keep the monitoring updated + // if disabled do nothing if(!enabled) return; // downsampling (optional) @@ -302,8 +308,6 @@ void BLDCMotor::move(float new_target) { motion_cnt = 0; // set internal target variable if(_isset(new_target)) target = new_target; - // get angular velocity - shaft_velocity = shaftVelocity(); switch (controller) { case MotionControlType::torque: diff --git a/src/drivers/BLDCDriver6PWM.cpp b/src/drivers/BLDCDriver6PWM.cpp index 013e6415..5dd99eb2 100644 --- a/src/drivers/BLDCDriver6PWM.cpp +++ b/src/drivers/BLDCDriver6PWM.cpp @@ -24,7 +24,7 @@ BLDCDriver6PWM::BLDCDriver6PWM(int phA_h,int phA_l,int phB_h,int phB_l,int phC_h // enable motor driver void BLDCDriver6PWM::enable(){ // enable_pin the driver - if enable_pin pin available - if ( _isset(enable_pin) ) digitalWrite(enable_pin, HIGH); + if ( _isset(enable_pin) ) digitalWrite(enable_pin, enable_active_high); // set zero to PWM setPwm(0, 0, 0); } @@ -35,7 +35,7 @@ void BLDCDriver6PWM::disable() // set zero to PWM setPwm(0, 0, 0); // disable the driver - if enable_pin pin available - if ( _isset(enable_pin) ) digitalWrite(enable_pin, LOW); + if ( _isset(enable_pin) ) digitalWrite(enable_pin, !enable_active_high); } @@ -82,4 +82,4 @@ void BLDCDriver6PWM::setPwm(float Ua, float Ub, float Uc) { // Set voltage to the pwm pin void BLDCDriver6PWM::setPhaseState(int sa, int sb, int sc) { // TODO implement disabling -} \ No newline at end of file +} diff --git a/src/drivers/BLDCDriver6PWM.h b/src/drivers/BLDCDriver6PWM.h index 68487370..cee37d64 100644 --- a/src/drivers/BLDCDriver6PWM.h +++ b/src/drivers/BLDCDriver6PWM.h @@ -37,6 +37,7 @@ class BLDCDriver6PWM: public BLDCDriver int pwmB_h,pwmB_l; //!< phase B pwm pin number int pwmC_h,pwmC_l; //!< phase C pwm pin number int enable_pin; //!< enable pin number + bool enable_active_high = true; float dead_zone; //!< a percentage of dead-time(zone) (both high and low side in low) for each pwm cycle [0,1] diff --git a/src/drivers/hardware_specific/generic_mcu.cpp b/src/drivers/hardware_specific/generic_mcu.cpp index 170b9dae..fd64e6af 100644 --- a/src/drivers/hardware_specific/generic_mcu.cpp +++ b/src/drivers/hardware_specific/generic_mcu.cpp @@ -12,9 +12,11 @@ #elif defined(_STM32_DEF_) // or stm32 -#elif defined(_SAMD21_) // samd21 for the moment, samd51 in progress... +#elif defined(_SAMD21_) // samd21 -#elif defined(_SAMD51_) // samd21 for the moment, samd51 in progress... +#elif defined(_SAMD51_) // samd51 + +#elif defined(__SAME51J19A__) || defined(__ATSAME51J19A__) // samd51 #else diff --git a/src/drivers/hardware_specific/samd21_mcu.cpp b/src/drivers/hardware_specific/samd21_mcu.cpp index 0c56eef4..cf8db836 100644 --- a/src/drivers/hardware_specific/samd21_mcu.cpp +++ b/src/drivers/hardware_specific/samd21_mcu.cpp @@ -310,17 +310,18 @@ void writeSAMDDutyCycle(int chaninfo, float dc) { uint8_t chan = GetTCChannelNumber(chaninfo); if (tccnCC[chan].reg = (uint32_t)((SIMPLEFOC_SAMD_PWM_RESOLUTION-1) * dc); - //uint32_t chanbit = 0x1<<(TCC_SYNCBUSY_CC0_Pos+chan); - //while ( (tcc->SYNCBUSY.reg & chanbit) > 0 ); + // set via CC +// tcc->CC[chan].reg = (uint32_t)((SIMPLEFOC_SAMD_PWM_RESOLUTION-1) * dc); +// uint32_t chanbit = 0x1<<(TCC_SYNCBUSY_CC0_Pos+chan); +// while ( (tcc->SYNCBUSY.reg & chanbit) > 0 ); // set via CCB + while ( (tcc->SYNCBUSY.vec.CC & (0x1< 0 ); tcc->CCB[chan].reg = (uint32_t)((SIMPLEFOC_SAMD_PWM_RESOLUTION-1) * dc); - while ( (tcc->SYNCBUSY.vec.CCB & (0x1< 0 ); - tcc->STATUS.vec.CCBV |= (0x1<SYNCBUSY.bit.STATUS > 0 ); - tcc->CTRLBSET.reg |= TCC_CTRLBSET_CMD(TCC_CTRLBSET_CMD_UPDATE_Val); - while ( tcc->SYNCBUSY.bit.CTRLB > 0 ); +// while ( (tcc->SYNCBUSY.vec.CCB & (0x1< 0 ); +// tcc->STATUS.vec.CCBV |= (0x1<SYNCBUSY.bit.STATUS > 0 ); +// tcc->CTRLBSET.reg |= TCC_CTRLBSET_CMD(TCC_CTRLBSET_CMD_UPDATE_Val); +// while ( tcc->SYNCBUSY.bit.CTRLB > 0 ); } else { Tc* tc = (Tc*)GetTC(chaninfo); diff --git a/src/drivers/hardware_specific/samd51_mcu.cpp b/src/drivers/hardware_specific/samd51_mcu.cpp index df025a13..69c44848 100644 --- a/src/drivers/hardware_specific/samd51_mcu.cpp +++ b/src/drivers/hardware_specific/samd51_mcu.cpp @@ -3,7 +3,7 @@ #include "./samd_mcu.h" -#ifdef _SAMD51_ +#if defined(_SAMD51_)||defined(_SAME51_) @@ -129,12 +129,13 @@ void writeSAMDDutyCycle(int chaninfo, float dc) { uint8_t chan = GetTCChannelNumber(chaninfo); if (tccnSYNCBUSY.vec.CC & (0x1< 0 ); tcc->CCBUF[chan].reg = (uint32_t)((SIMPLEFOC_SAMD_PWM_RESOLUTION-1) * dc); // TODO pwm frequency! - tcc->STATUS.vec.CCBUFV |= (0x1<SYNCBUSY.bit.STATUS > 0 ); - tcc->CTRLBSET.reg |= TCC_CTRLBSET_CMD(TCC_CTRLBSET_CMD_UPDATE_Val); - while ( tcc->SYNCBUSY.bit.CTRLB > 0 ); +// tcc->STATUS.vec.CCBUFV |= (0x1<SYNCBUSY.bit.STATUS > 0 ); +// tcc->CTRLBSET.reg |= TCC_CTRLBSET_CMD(TCC_CTRLBSET_CMD_UPDATE_Val); +// while ( tcc->SYNCBUSY.bit.CTRLB > 0 ); } else { // Tc* tc = (Tc*)GetTC(chaninfo); diff --git a/src/drivers/hardware_specific/samd_mcu.cpp b/src/drivers/hardware_specific/samd_mcu.cpp index c30b4a90..fdbd7a7f 100644 --- a/src/drivers/hardware_specific/samd_mcu.cpp +++ b/src/drivers/hardware_specific/samd_mcu.cpp @@ -3,7 +3,7 @@ #include "./samd_mcu.h" -#if defined(_SAMD21_)||defined(_SAMD51_) +#if defined(_SAMD21_)||defined(_SAMD51_)||defined(_SAME51_) @@ -90,7 +90,7 @@ tccConfiguration getTCCChannelNr(int pin, EPioType peripheral) { result.tcc.chaninfo = association.tccF; result.wo = association.woF; } -#ifdef _SAMD51_ +#if defined(_SAMD51_)||defined(_SAME51_) else if (peripheral==PIO_TCC_PDEC) { result.tcc.chaninfo = association.tccG; result.wo = association.woG; @@ -758,7 +758,7 @@ void printAllPinInfos() { case PORTA: Serial.print(" PA"); break; case PORTB: Serial.print(" PB"); break; case PORTC: Serial.print(" PC"); break; -#ifdef _SAMD51_ +#if defined(_SAMD51_)||defined(_SAME51_) case PORTD: Serial.print(" PD"); break; #endif } @@ -803,7 +803,7 @@ void printAllPinInfos() { else Serial.print(" None "); -#ifdef _SAMD51_ +#if defined(_SAMD51_)||defined(_SAME51_) Serial.print(" G="); if (association.tccG>=0) { int tcn = GetTCNumber(association.tccG); @@ -842,7 +842,7 @@ void printTCCConfiguration(tccConfiguration& info) { Serial.print(" E "); break; case PIO_TIMER_ALT: Serial.print(" F "); break; -#ifdef _SAMD51_ +#if defined(_SAMD51_)||defined(_SAME51_) case PIO_TCC_PDEC: Serial.print(" G "); break; #endif diff --git a/src/drivers/hardware_specific/samd_mcu.h b/src/drivers/hardware_specific/samd_mcu.h index 9a0d3b05..f0fd5460 100644 --- a/src/drivers/hardware_specific/samd_mcu.h +++ b/src/drivers/hardware_specific/samd_mcu.h @@ -9,7 +9,14 @@ #include "../hardware_api.h" -#if defined(_SAMD21_)||defined(_SAMD51_) +#if defined(__SAME51J19A__) || defined(__ATSAME51J19A__) +#ifndef _SAME51_ +#define _SAME51_ +#endif +#endif + + +#if defined(_SAMD21_)||defined(_SAMD51_)||defined(_SAME51_) #include "Arduino.h" @@ -53,7 +60,7 @@ struct wo_association { uint8_t woE; ETCChannel tccF; uint8_t woF; -#if defined(_SAMD51_) +#if defined(_SAMD51_)||defined(_SAME51_) ETCChannel tccG; uint8_t woG; #endif @@ -63,7 +70,7 @@ struct wo_association { #if defined(_SAMD21_) #define NUM_PIO_TIMER_PERIPHERALS 2 -#elif defined(_SAMD51_) +#elif defined(_SAMD51_)||defined(_SAME51_) #define NUM_PIO_TIMER_PERIPHERALS 3 #endif