Merge pull request #72 from runger1101001/dev

runger1101001 · web-flow · commit c9ff03e9f557 · 2021-05-05T01:09:31.000+02:00
SAME51 support (tested on Feather M4 CAN) and minor tweaks
diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp
@@ -247,13 +247,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();
 
@@ -298,15 +300,17 @@ 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)
   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();
 
   switch (controller) {
     case MotionControlType::torque:
diff --git 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
-}
+}
diff --git 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
@@ -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
@@ -310,17 +310,18 @@ void writeSAMDDutyCycle(int chaninfo, float dc) {
 	uint8_t chan = GetTCChannelNumber(chaninfo);
 	if (tccn<TCC_INST_NUM) {
 		Tcc* tcc = (Tcc*)GetTC(chaninfo);
-			// 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 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<<chan)) > 0 );
 		tcc->CCB[chan].reg = (uint32_t)((SIMPLEFOC_SAMD_PWM_RESOLUTION-1) * dc);
-		while ( (tcc->SYNCBUSY.vec.CCB & (0x1<<chan)) > 0 );
-		tcc->STATUS.vec.CCBV |= (0x1<<chan);
-		while ( tcc->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<<chan)) > 0 );
+//		tcc->STATUS.vec.CCBV |= (0x1<<chan);
+//		while ( tcc->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
@@ -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 (tccn<TCC_INST_NUM) {
 		Tcc* tcc = (Tcc*)GetTC(chaninfo);
-		// set via CCBUF]
+		// set via CCBUF
+		while ( (tcc->SYNCBUSY.vec.CC & (0x1<<chan)) > 0 );
 		tcc->CCBUF[chan].reg = (uint32_t)((SIMPLEFOC_SAMD_PWM_RESOLUTION-1) * dc); // TODO pwm frequency!
-		tcc->STATUS.vec.CCBUFV |= (0x1<<chan);
-		while ( tcc->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<<chan);
+//		while ( tcc->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
@@ -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
@@ -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
 
