fixed current calibration, before merging to slave

This commit is contained in:
Nicolas Trimborn 2021-08-30 11:30:27 +02:00
parent 02ade8660d
commit 02162af995
9 changed files with 111 additions and 189 deletions

View File

@ -10,6 +10,7 @@
#define ETHERCAT_SLAVEDEF_H_
#include "Ethercat_QSPI.h"
#include "arm_math.h"
extern volatile int32_t _channel_data[8];

View File

@ -10,6 +10,7 @@
#define ETHERCAT_SLAVEDEF_H_
#include "Ethercat_QSPI.h"
#include "arm_math.h"
extern volatile int32_t _channel_data[8];

View File

@ -7,7 +7,6 @@
#include "bldc.h"
#include "statemachine.h"
#include "utilities.h"
#include "Ethercat_SlaveDef.h"
@ -217,13 +216,12 @@ void exec_commutation(BLDCMotor_t* const motor)
volatile uint8_t currentHall = motor->readHall();
motor->motor_status.currentHallPattern = currentHall;
//if ((currentHall == INVALID_HALL_0)||(currentHall == INVALID_HALL_7))
//{
//hri_tcc_write_PATTBUF_reg(motor->motor_param->pwm_desc->device.hw, DISABLE_PATTERN);
//motor->motor_state.currentstate == MOTOR_FAULT;
//motor->motor_state.fault == MOTOR_HALLSENSORINVALID;
//return;
//}
if (currentHall == INVALID_HALL_7)
{
motor->motor_state.currentstate == MOTOR_FAULT;
motor->motor_state.fault == MOTOR_HALLSENSORINVALID;
return;
}
// ----------------------------------------------------------------------
// Set Pattern Buffers
@ -232,7 +230,7 @@ void exec_commutation(BLDCMotor_t* const motor)
motor->motor_setpoints.directionOffset];
//TCC0->PATTBUF.reg = temp_M1;
Tcc * tmp = (Tcc *)motor->motor_param->pwm_desc->device.hw;
Tcc * tmp = (Tcc *)motor->pwm_desc->device.hw;
tmp->PATTBUF.reg = (uint16_t)temp_M1;
//motor->motor_param->pwm_desc->device.hw->PATTBUF.reg = temp_M1;
//hri_tcc_write_PATTBUF_reg(motor->motor_param->pwm_desc->device.hw, temp_M1);
@ -303,8 +301,8 @@ void calculate_motor_speed(BLDCMotor_t* const motor)
{
//tic_port(DEBUG_2_PORT);
volatile uint32_t temp_rpm = 0;
hri_tccount32_read_CC_reg(motor->motor_param->speedtimer_hw, 0); /* Read CC0 but throw away)*/
volatile uint32_t period_after_capture = hri_tccount32_read_CC_reg(motor->motor_param->speedtimer_hw, 1);
hri_tccount32_read_CC_reg(motor->speedtimer_hw, 0); /* Read CC0 but throw away)*/
volatile uint32_t period_after_capture = hri_tccount32_read_CC_reg(motor->speedtimer_hw, 1);
if((period_after_capture >= UINT32_MAX)||(period_after_capture == 0)||(period_after_capture > 600000)) {
motor->motor_status.calc_rpm = 0;
} else {
@ -362,7 +360,7 @@ void calculate_motor_speed(BLDCMotor_t* const motor)
void disable_phases(BLDCMotor_t* const motor)
{
Tcc * tmp = (Tcc *)motor->motor_param->pwm_desc->device.hw;
Tcc * tmp = (Tcc *)motor->pwm_desc->device.hw;
tmp->PATTBUF.reg = DISABLE_PATTERN;
}
@ -512,124 +510,40 @@ volatile uint8_t readHallSensorM2(void)
// ----------------------------------------------------------------------
void read_zero_current_offset_value(BLDCMotor_t *motor1, BLDCMotor_t *motor2)
{
uint32_t phase_A_zero_current_offset_temp = 0;
uint32_t phase_B_zero_current_offset_temp = 0;
volatile uint16_t zero_current_offset_temp[2] = {0,0};
uint8_t samples = 32;
uint8_t i;
// ------------------------------------------------------------------
// Motor 1
// -------------------------------------------------------------------
volatile int32_t phase_zero_current_offset [NUM_CUR_SENSORS] = {0};
volatile int16_t zero_current_offset_temp = 0;
const uint8_t samples = 32;
adc_sync_enable_channel(&ADC_1, 9);
//adc_sync_enable_channel(&ADC_1, 0);
/* Single ended */
//ADC1->INPUTCTRL.reg = 0x1809;
/* Differential */
ADC1->INPUTCTRL.reg = 0x0089;
while (ADC1->STATUS.bit.ADCBUSY) {}; /* Wait for bus synchronization. */
for (i=0; i<samples; i++)
for (int n=0; n<NUM_CUR_SENSORS; n++)
{
volatile uint16_t zero_current_offset_temp[2] = {0,0};
while (ADC1->STATUS.bit.ADCBUSY) {}; /* Wait for bus synchronization. */
ADC1->SWTRIG.bit.START = true; /* Start the ADC using a software trigger. */
while (ADC1->INTFLAG.bit.RESRDY == 0); /* Wait for the result ready flag to be set. */
ADC1->INTFLAG.reg = ADC_INTFLAG_RESRDY; /* Clear the flag. */
zero_current_offset_temp[0] = ADC1->RESULT.reg; /* Read the value. */
phase_A_zero_current_offset_temp += zero_current_offset_temp[0];
ADC1->INPUTCTRL.reg = adc1_seq_regs[n];
for (int i=0; i<samples; i++)
{
while (ADC1->STATUS.bit.ADCBUSY) {}; /* Wait for bus synchronization. */
ADC1->SWTRIG.bit.START = true; /* Start the ADC using a software trigger. */
while (ADC1->INTFLAG.bit.RESRDY == 0); /* Wait for the result ready flag to be set. */
ADC1->INTFLAG.reg = ADC_INTFLAG_RESRDY; /* Clear the flag. */
zero_current_offset_temp = (int16_t)ADC1->RESULT.reg; /* Read the value. */
phase_zero_current_offset[n] += zero_current_offset_temp;
}
phase_zero_current_offset[n] = phase_zero_current_offset[n]/samples;
}
adc_sync_disable_channel(&ADC_1, 9);
/* Set Motor Variables */
motor1->Voffset_lsb.A = phase_A_zero_current_offset_temp/samples;
motor1->Voffset_lsb.A = phase_zero_current_offset[0];
motor1->Voffset_lsb.B = phase_zero_current_offset[1];
motor2->Voffset_lsb.A = phase_zero_current_offset[2];
motor2->Voffset_lsb.B = phase_zero_current_offset[3];
/* Single ended */
//ADC1->INPUTCTRL.reg = 0x1808;
/* Differential */
ADC1->INPUTCTRL.reg = 0x0088;
while (ADC1->STATUS.bit.ADCBUSY) {}; /* Wait for bus synchronization. */
for (i=0; i<samples; i++)
{
while (ADC1->STATUS.bit.ADCBUSY) {}; /* Wait for bus synchronization. */
ADC1->SWTRIG.bit.START = true; /* Start the ADC using a software trigger. */
while (ADC1->INTFLAG.bit.RESRDY == 0); /* Wait for the result ready flag to be set. */
ADC1->INTFLAG.reg = ADC_INTFLAG_RESRDY; /* Clear the flag. */
zero_current_offset_temp[1] = ADC1->RESULT.reg; /* Read the value. */
phase_B_zero_current_offset_temp += zero_current_offset_temp[1];
}
/* Set Motor Variables */
motor1->Voffset_lsb.B = phase_B_zero_current_offset_temp/samples;
adc_sync_disable_channel(&ADC_1, 8);
adc_sync_enable_channel(&ADC_1, 7);
//adc_sync_enable_channel(&ADC_1, 0);
/* Check for Defective current sensor based on offset from nominal 1.5V (O current voltage) */
if ((abs(motor1->Voffset_lsb.A) > MAX_CUR_SENSE_OFFSET) || (abs(motor1->Voffset_lsb.B) > MAX_CUR_SENSE_OFFSET))
{
motor1->motor_state.currentstate = MOTOR_FAULT;
motor1->motor_state.fault = MOTOR_CURRENTS_SENSOR;
}
// ------------------------------------------------------------------
// Motor 2
// -------------------------------------------------------------------
phase_A_zero_current_offset_temp = 0;
phase_B_zero_current_offset_temp = 0;
/* Single ended */
//ADC1->INPUTCTRL.reg = 0x1807;
/* Differential */
ADC1->INPUTCTRL.reg = 0x0087;
while (ADC1->STATUS.bit.ADCBUSY) {}; /* Wait for bus synchronization. */
for (i=0; i<samples; i++)
{
volatile uint16_t zero_current_offset_temp[2] = {0,0};
while (ADC1->STATUS.bit.ADCBUSY) {}; /* Wait for bus synchronization. */
ADC1->SWTRIG.bit.START = true; /* Start the ADC using a software trigger. */
while (ADC1->INTFLAG.bit.RESRDY == 0); /* Wait for the result ready flag to be set. */
ADC1->INTFLAG.reg = ADC_INTFLAG_RESRDY; /* Clear the flag. */
zero_current_offset_temp[0] = ADC1->RESULT.reg; /* Read the value. */
phase_A_zero_current_offset_temp += zero_current_offset_temp[0];
}
/* Set Motor Variables */
motor2->Voffset_lsb.A = phase_A_zero_current_offset_temp/samples;
/* Single ended */
//ADC1->INPUTCTRL.reg = 0x1806;
/* Differential */
ADC1->INPUTCTRL.reg = 0x0086;
while (ADC1->STATUS.bit.ADCBUSY) {}; /* Wait for bus synchronization. */
for (i=0; i<samples; i++)
{
while (ADC1->STATUS.bit.ADCBUSY) {}; /* Wait for bus synchronization. */
ADC1->SWTRIG.bit.START = true; /* Start the ADC using a software trigger. */
while (ADC1->INTFLAG.bit.RESRDY == 0); /* Wait for the result ready flag to be set. */
ADC1->INTFLAG.reg = ADC_INTFLAG_RESRDY; /* Clear the flag. */
zero_current_offset_temp[1] = ADC1->RESULT.reg; /* Read the value. */
phase_B_zero_current_offset_temp += zero_current_offset_temp[1];
}
/* Set Motor Variables */
motor2->Voffset_lsb.B = phase_B_zero_current_offset_temp/samples;
adc_sync_disable_channel(&ADC_1, 6);
//adc_sync_disable_channel(&ADC_1, 0);
if ((abs(motor2->Voffset_lsb.A) > MAX_CUR_SENSE_OFFSET) || (abs(motor2->Voffset_lsb.B) > MAX_CUR_SENSE_OFFSET))
{
motor2->motor_state.currentstate = MOTOR_FAULT;

View File

@ -41,10 +41,16 @@
#define ADC_LSB_SIZE (ADC_VOLTAGE_REFERENCE/ADC_MAX_COUNTS)
#define LSB_TO_PU (ADC_LSB_SIZE * ONEON_CURRENT_SENSOR_SENSITIVITY)
//static const uint32_t adc1_seq_regs[4] = {0x0089, 0x0088, 0x0087, 0x0086};
//static volatile int16_t adc0_res[4] = {0};
//static volatile int16_t adc1_res[4] = {0};
extern const uint32_t adc1_seq_regs[4];
extern volatile int16_t adc1_res[4];
// ----------------------------------------------------------------------
// Define the control and PWM frequencies:
// ----------------------------------------------------------------------
// 16kHz is the maximum frequency according to the calculation duration in the mode run and spin.
#define DEVICE_MCU_FREQUENCY_Hz (120000000U)
#define DEVICE_SPEEDTC_DIV (4U)
#define DEVICE_SPEEDTC_FREQUENCY_Hz (DEVICE_MCU_FREQUENCY_Hz/DEVICE_SPEEDTC_DIV)
@ -62,7 +68,7 @@
#define CURRENT_SENSOR_SENSITIVITY 0.4f //V/A
#define ONEON_CURRENT_SENSOR_SENSITIVITY 2.5f //V/A
#define MAX_CUR_SENSE_OFFSET 100
#define NUM_CUR_SENSORS 4
// ----------------------------------------------------------------------
// global variables
// ----------------------------------------------------------------------
@ -70,12 +76,17 @@
static const uint8_t HALL_PATTERN_ARRAY[16] = {0, 5, 3, 1, 6, 4, 2, 0, 0, 3, 6, 2, 5, 1, 4, 0 };
static const uint8_t MOTOR_COMMUTATION_STEPS[8] = {9, 1, 3, 2, 5, 6, 4, 9};
volatile BLDCMotor_t Motor1;
volatile BLDCMotor_t Motor2;
volatile MOTOR_STATE_t Motor1_Status;
volatile MOTOR_STATE_t Motor2_Status;
static volatile BLDCMotor_t Motor1 = {
.pwm_desc = &PWM_0,
.speedtimer_hw = TC2,
};
static volatile BLDCMotor_t Motor2 = {
.pwm_desc = &PWM_1,
.speedtimer_hw = TC4,
};
static volatile MOTOR_STATE_t Motor1_Status;
static volatile MOTOR_STATE_t Motor2_Status;
// ----------------------------------------------------------------------
// functions

View File

@ -82,8 +82,10 @@ volatile typedef struct BLDCmotor
{
/* Hardware */
volatile BLDCMotor_param_t *motor_param;
volatile MOTOR_STATE_t motor_state;
struct pwm_descriptor const *pwm_desc;
void *const speedtimer_hw;
/* Status */
volatile MOTOR_STATE_t motor_state;
volatile MOTOR_Status motor_status;
/* Measured Values */
volatile MOTOR_PHASES_t Iphase_pu;

View File

@ -27,19 +27,19 @@
#define CONF_ADC_0_SEQUENCER_CHANNEL 6U
#define CONF_ADC_1_SEQUENCER_CHANNEL 7U
/* Single Ended */
//const uint32_t adc_seq_regs[4] = {0x1807, 0x1806, 0x1809, 0x1808};
/* Differential */
const uint32_t adc_seq_regs[4] = {0x0089, 0x0088, 0x0087, 0x0086};
const uint32_t adc0_seq_regs[4] = {0};
const uint32_t adc1_seq_regs[4] = {0x0089, 0x0088, 0x0087, 0x0086}; /* Differential */
//const uint32_t adc1_seq_regs[4] = {0x1807, 0x1806, 0x1809, 0x1808}; /* Single Ended */
volatile int16_t adc0_res[4] = {0};
volatile int16_t adc1_res[4] = {0};
volatile int16_t adc_res[4] = {0};
struct _dma_resource *adc_sram_dma_resource;
struct _dma_resource *adc_dmac_sequence_resource;
// ----------------------------------------------------------------------
// PWM Timer Initialization
// ----------------------------------------------------------------------
inline static void configure_tcc_pwm(void)
static void configure_tcc_pwm(void)
{
/* TCC0 */
hri_tcc_set_WEXCTRL_OTMX_bf(TCC0, 0x02);
@ -90,46 +90,14 @@ inline static void configure_tcc_pwm(void)
}
/* Peripherals should be configured before interacting with dma
* CH0 - QSPI_RX - For ECAT DMA Mode - Currently Disabled in ASTART
* CH1 - SERCOM1_RX(SPI1) - Master-Slave IF - Beat Transfer Event Drives CS Pin
* CH2 - SERCOM2_RX(SPI2) - Expansion IF (EMG) - Beat Transfer Event Drives CS Pin
* CH3 - SERCOM5_RX(SPI3) - Angle Sensor
* CH4 - ADC0 - Result Ready (Unused on master) - Currently Disabled in ASTART
* CH5 - ADC1 - Result Ready
* CH6 - ADC0 - Sequencer (Unused on master) - Currently Disabled in ASTART
* CH7 - ADC1 - Sequencer - Triggered by TCC0 overflow event
* CH8 - SERCOM2_TX(SPI2)
* CH9 - SERCOM5_TX(SPI3)
* CH10 - SERCOM1_TX(SPI1)
* CH11 - QSPI_TX - For ECAT DMA Mode - Currently Disabled in ASTART
*/
inline static void init_dma(void)
{
spi_master_init_dma_descriptors();
adc_init_dma_descriptors();
}
// ----------------------------------------------------------------------
// ADC Initialization
// ----------------------------------------------------------------------
inline void adc_init_dma_descriptors(void)
{
adc_sram_dmac_init();
adc_dmac_sequence_init();
hri_adc_set_DSEQCTRL_INPUTCTRL_bit(ADC1);
hri_adc_set_DSEQCTRL_AUTOSTART_bit(ADC1);
}
inline void adc_dmac_sequence_init()
static void adc_dmac_sequence_init()
{
/* Configure the DMAC source address, destination address,
* next descriptor address, data count and Enable the DMAC Channel
*/
_dma_set_source_address(CONF_ADC_1_SEQUENCER_CHANNEL, (const void *)adc_seq_regs);
_dma_set_source_address(CONF_ADC_1_SEQUENCER_CHANNEL, (const void *)adc1_seq_regs);
_dma_set_destination_address(CONF_ADC_1_SEQUENCER_CHANNEL, (const void *)&ADC1->DSEQDATA.reg);
_dma_set_data_amount(CONF_ADC_1_SEQUENCER_CHANNEL, 4);
_dma_set_next_descriptor(CONF_ADC_1_SEQUENCER_CHANNEL, CONF_ADC_1_SEQUENCER_CHANNEL);
@ -137,10 +105,10 @@ inline void adc_dmac_sequence_init()
hri_dmacchannel_set_CHCTRLB_CMD_bf(&DMAC->Channel[CONF_ADC_1_SEQUENCER_CHANNEL], 0x01); //Suspend
}
inline void adc_sram_dmac_init()
static void adc_sram_dmac_init()
{
_dma_set_source_address(CONF_ADC_1_ADC_RES_READY_CHANNEL, (const void *)&ADC1->RESULT.reg);
_dma_set_destination_address(CONF_ADC_1_ADC_RES_READY_CHANNEL, (const void *)adc_res);
_dma_set_destination_address(CONF_ADC_1_ADC_RES_READY_CHANNEL, (const void *)adc1_res);
_dma_set_data_amount(CONF_ADC_1_ADC_RES_READY_CHANNEL, 4);
_dma_set_irq_state(CONF_ADC_1_ADC_RES_READY_CHANNEL, DMA_TRANSFER_COMPLETE_CB, true);
_dma_get_channel_resource(&adc_sram_dma_resource, CONF_ADC_1_ADC_RES_READY_CHANNEL);
@ -149,6 +117,19 @@ inline void adc_sram_dmac_init()
_dma_enable_transaction(CONF_ADC_1_ADC_RES_READY_CHANNEL, false);
}
// ----------------------------------------------------------------------
// ADC Initialization
// ----------------------------------------------------------------------
static void adc_init_dma_descriptors(void)
{
adc_sram_dmac_init();
adc_dmac_sequence_init();
hri_adc_set_DSEQCTRL_INPUTCTRL_bit(ADC1);
hri_adc_set_DSEQCTRL_AUTOSTART_bit(ADC1);
}
// ----------------------------------------------------------------------
// SPI DMA communication between Master & Slave Board
// ----------------------------------------------------------------------
@ -160,7 +141,7 @@ inline void adc_sram_dmac_init()
extern DmacDescriptor _descriptor_section[DMAC_CH_NUM];
extern DmacDescriptor _write_back_section[DMAC_CH_NUM];
void boardToBoardTransferInit(void)
static void boardToBoardTransferInit(void)
{
struct io_descriptor *io;
spi_m_dma_get_io_descriptor(&SPI_1_MSIF, &io);
@ -175,7 +156,7 @@ void boardToBoardTransferInit(void)
spi_m_dma_enable(&SPI_1_MSIF);
}
void spi_master_init_dma_descriptors()
static void spi_master_init_dma_descriptors()
{
_dma_set_source_address(CONF_SERCOM_1_SPI_M_DMA_RX_CHANNEL,
(uint32_t *)&(((SercomSpi *)(SPI_1_MSIF.dev.prvt))->DATA.reg));
@ -203,7 +184,25 @@ void spi_master_init_dma_descriptors()
}
/* Peripherals should be configured before interacting with dma
* CH0 - QSPI_RX - For ECAT DMA Mode - Currently Disabled in ASTART
* CH1 - SERCOM1_RX(SPI1) - Master-Slave IF - Beat Transfer Event Drives CS Pin
* CH2 - SERCOM2_RX(SPI2) - Expansion IF (EMG) - Beat Transfer Event Drives CS Pin
* CH3 - SERCOM5_RX(SPI3) - Angle Sensor
* CH4 - ADC0 - Result Ready (Unused on master) - Currently Disabled in ASTART
* CH5 - ADC1 - Result Ready
* CH6 - ADC0 - Sequencer (Unused on master) - Currently Disabled in ASTART
* CH7 - ADC1 - Sequencer - Triggered by TCC0 overflow event
* CH8 - SERCOM2_TX(SPI2)
* CH9 - SERCOM5_TX(SPI3)
* CH10 - SERCOM1_TX(SPI1)
* CH11 - QSPI_TX - For ECAT DMA Mode - Currently Disabled in ASTART
*/
static void init_dma(void)
{
spi_master_init_dma_descriptors();
adc_init_dma_descriptors();
}
#endif /* CONFIGURATION_H_ */

View File

@ -10,7 +10,6 @@
#define INTERRUPTS_H_
/* TC0 - Interrupt Handler
* Configured to trigger @ 1ms
*/
@ -79,7 +78,7 @@ static void pwm_cb(const struct pwm_descriptor *const descr)
volatile int x = 0;
}
void adc_sram_dma_callback(struct _dma_resource *adc_dma_res)
static void adc_sram_dma_callback(struct _dma_resource *adc_dma_res)
{
Motor1.timerflags.adc_readings_ready_tic = true;
Motor2.timerflags.adc_readings_ready_tic = true;
@ -125,7 +124,7 @@ static void M2_RESET_BAR(void)
// ----------------------------------------------------------------------
void ADS1299_dataReadyISR(void)
{
ADS1299.data_ReadyFlag = true;
//ADS1299.data_ReadyFlag = true;
//int32_t* temp = ADS1299_UPDATECHANNELDATA();
volatile int x = 1;
}

View File

@ -3,17 +3,20 @@
// ----------------------------------------------------------------------
// Header Files
// ----------------------------------------------------------------------
#include "bldc.h"
#include "bldc_types.h"
#include "EtherCAT_QSPI.h"
#include "ADS1299.h"
#include "EtherCAT_SlaveDef.h"
//#include "MSIF_master.h"
#include "configuration.h"
#include "bldc.h"
#include "bldc_types.h"
#include "EtherCAT_SlaveDef.h"
#include "interrupts.h"
#include "statemachine.h"
#include "angle_sensors.h"
#include "ADS1299.h"
@ -26,8 +29,8 @@ void process_currents()
volatile int16_t phase_A_current_raw, phase_B_current_raw;
/* Motor 1 */
phase_A_current_raw = (adc_res[0] - Motor1.Voffset_lsb.A);
phase_B_current_raw = (adc_res[1] - Motor1.Voffset_lsb.B)*-1;
phase_A_current_raw = (adc1_res[0] + Motor1.Voffset_lsb.A);
phase_B_current_raw = (adc1_res[1] + Motor1.Voffset_lsb.B)*-1;
// Covert from LSB to PU (A) and filter out small readings
Motor1.Iphase_pu.A = phase_A_current_raw * LSB_TO_PU;
Motor1.Iphase_pu.B = phase_B_current_raw * LSB_TO_PU;
@ -35,8 +38,8 @@ void process_currents()
Motor1.Iphase_pu.C = -Motor1.Iphase_pu.A - Motor1.Iphase_pu.B;
/* Motor 2 negative is A instead of B*/
phase_A_current_raw = (adc_res[2] - Motor2.Voffset_lsb.A);
phase_B_current_raw = (adc_res[3] - Motor2.Voffset_lsb.B)*-1;
phase_A_current_raw = (adc1_res[2] + Motor2.Voffset_lsb.A);
phase_B_current_raw = (adc1_res[3] + Motor2.Voffset_lsb.B)*-1;
// Covert from LSB to PU (A) and filter out small readings
Motor2.Iphase_pu.A = phase_A_current_raw * LSB_TO_PU;
Motor2.Iphase_pu.B = phase_B_current_raw * LSB_TO_PU;

View File

@ -119,8 +119,6 @@ static const uint16_t COMMUTATION_PATTERN[16] = {
// ----------------------------------------------------------------------
typedef struct
{
struct pwm_descriptor const *pwm_desc;
void *const speedtimer_hw;
const uint16_t motor_Poles;
const uint16_t motor_polePairs;
const uint16_t motor_commutationStates;
@ -141,8 +139,6 @@ typedef struct
/* Small Motor - 2214S024BXTR*/
const static BLDCMotor_param_t FH_22mm24BXTR = {
.pwm_desc = &PWM_0,
.speedtimer_hw = TC2,
.motor_Poles = 14,
.motor_polePairs = 7,
.motor_commutationStates = 42, //polePairs * 6
@ -166,8 +162,6 @@ const static BLDCMotor_param_t FH_22mm24BXTR = {
/* Big Motor - 3216W012BXTR */
const static BLDCMotor_param_t FH_32mm12BXTR = {
.pwm_desc = &PWM_1,
.speedtimer_hw = TC4,
.motor_Poles = 14,
.motor_polePairs = 7,
.motor_commutationStates = 42, //polePairs * 6
@ -192,8 +186,6 @@ const static BLDCMotor_param_t FH_32mm12BXTR = {
/* Big Motor - 3216W024BXTR */
const static BLDCMotor_param_t FH_32mm24BXTR = {
.pwm_desc = &PWM_1,
.speedtimer_hw = TC4,
.motor_Poles = 14,
.motor_polePairs = 7,
.motor_commutationStates = 42, //polePairs * 6