From 1b6dab714361baa9cd172c9efcdd028c6f3e6b20 Mon Sep 17 00:00:00 2001 From: Nicolas Trimborn Date: Thu, 12 Aug 2021 10:18:07 +0200 Subject: [PATCH] fixed speed time issues --- .../Motor_Master/Motor_Master/bldc.c | 44 +++--- .../Motor_Master/Motor_Master/bldc.h | 3 +- .../Motor_Master/Motor_Master/interrupts.h | 23 ++++ .../Motor_Master/Motor_Master/main.c | 9 +- .../Motor_Master/motorparameters.h | 129 +++++------------- .../Motor_Master/Motor_Master/statemachine.h | 6 +- Twincat/MotorData/.vs/MotorData/v15/.suo | Bin 36864 -> 36864 bytes 7 files changed, 91 insertions(+), 123 deletions(-) diff --git a/2_Motor_Master/Motor_Master/Motor_Master/bldc.c b/2_Motor_Master/Motor_Master/Motor_Master/bldc.c index 662768c..e097c42 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/bldc.c +++ b/2_Motor_Master/Motor_Master/Motor_Master/bldc.c @@ -14,11 +14,11 @@ void motor_StateMachine(BLDCMotor_t* const motor) { - if (motor->motor_state.fault) - { - motor->motor_state.previousstate = motor->motor_state.currentstate; - motor->motor_state.currentstate = MOTOR_FAULT; - } + //if (motor->motor_state.fault) + //{ + //motor->motor_state.previousstate = motor->motor_state.currentstate; + //motor->motor_state.currentstate = MOTOR_FAULT; + //} switch (motor->motor_state.currentstate) { @@ -189,9 +189,16 @@ void exec_commutation(BLDCMotor_t* const motor) // ---------------------------------------------------------------------- //tic_port(DEBUG_2_PORT); //motor->motor_status.currentHallPattern = readM1Hall(); - volatile uint8_t currentHall = 1; - currentHall = motor->readHall(); + 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; + } // ---------------------------------------------------------------------- // Set Pattern Buffers @@ -265,9 +272,9 @@ void calculate_motor_speed(BLDCMotor_t* const motor) { //tic_port(DEBUG_2_PORT); volatile uint32_t temp_rpm = 0; - hri_tccount32_read_CC_reg(TC2, 0); /* Read CC0 but throw away)*/ - volatile uint32_t period_after_capture = hri_tccount32_read_CC_reg(TC2, 1); - if((period_after_capture > UINT32_MAX)||(period_after_capture == 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); + if((period_after_capture >= UINT32_MAX)||(period_after_capture == 0)||(period_after_capture > 600000)) { motor->motor_status.calc_rpm = 0; } else { ////uint32_t test = (SPEEDFACTOR, period_after_capture); @@ -290,14 +297,12 @@ void calculate_motor_speed(BLDCMotor_t* const motor) temp_rpm = temp_rpm; } - //motor->motor_status.calc_rpm = (int16_t)temp_rpm; - //toc_port(DEBUG_2_PORT); + motor->motor_status.calc_rpm = (int16_t)temp_rpm; - //#ifdef AVERAGE_SPEED_MEASURE - //if(temp_rpm==0) - //#define n_SAMPLE 8 - //static uint32_t speed_average = 0; - //static uint8_t count = 1; + #ifdef AVERAGE_SPEED_MEASURE + + //volatile uint32_t speed_average = 0; + //volatile uint8_t count = 1; //// To avoid noise an average is realized on 8 samples //speed_average += temp_rpm; //if(count >= n_SAMPLE) @@ -318,7 +323,7 @@ void calculate_motor_speed(BLDCMotor_t* const motor) //return; //} //else count++; - //#endif + #endif motor->motor_status.calc_rpm = (int16_t)temp_rpm; //toc_port(DEBUG_2_PORT); @@ -445,6 +450,7 @@ volatile uint8_t readHallSensorM2(void) motor_read = (motor_read & M2_HALL_B_MASK) | (uint8_t)((PORT->Group[M2_HALL_B_GROUP].IN.reg & M2_HALL_B_PORT)>>(M2_HALL_B_LSR)); motor_read = (motor_read & M2_HALL_C_MASK) | (uint8_t)((PORT->Group[M2_HALL_C_GROUP].IN.reg & M2_HALL_C_PORT)>>(M2_HALL_C_LSR)); + return motor_read; //if(((motor_read == INVALID_HALL_0) || (motor_read == INVALID_HALL_7))) { //Motor2.motor_state.fault = MOTOR_HALLSENSORINVALID; //Motor2.motor_state.currentstate = MOTOR_FAULT; @@ -458,7 +464,7 @@ volatile uint8_t readHallSensorM2(void) //return ((a << 2) | //(b << 1) | //(c << 0)); - return motor_read; + } // ---------------------------------------------------------------------- diff --git a/2_Motor_Master/Motor_Master/Motor_Master/bldc.h b/2_Motor_Master/Motor_Master/Motor_Master/bldc.h index 2254838..c917e17 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/bldc.h +++ b/2_Motor_Master/Motor_Master/Motor_Master/bldc.h @@ -29,7 +29,8 @@ #define INVALID_HALL_0 (0) #define INVALID_HALL_7 (7) -#define AVERAGE_SPEED_MEASURE 1 +//#define AVERAGE_SPEED_MEASURE +#define n_SAMPLE 8 // ---------------------------------------------------------------------- // ADC Parameters diff --git a/2_Motor_Master/Motor_Master/Motor_Master/interrupts.h b/2_Motor_Master/Motor_Master/Motor_Master/interrupts.h index c650168..229a9c2 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/interrupts.h +++ b/2_Motor_Master/Motor_Master/Motor_Master/interrupts.h @@ -24,6 +24,29 @@ void adc_sram_dma_callback(struct _dma_resource *adc_dma_res) Motor2.timerflags.adc_readings_ready_tic = true; } +// ---------------------------------------------------------------------- +// EtherCAT Cycle Timer - 1kHz +// ---------------------------------------------------------------------- +void TC2_Handler(void) +{ + if (TC2->COUNT32.INTFLAG.bit.OVF == 0x01) { + TC2->COUNT32.INTFLAG.bit.OVF = 0x01; + Motor1.motor_status.calc_rpm = 0; + } +} + +// ---------------------------------------------------------------------- +// +// ---------------------------------------------------------------------- +void TC4_Handler(void) +{ + if (TC4->COUNT32.INTFLAG.bit.OVF == 0x01) { + TC4->COUNT32.INTFLAG.bit.OVF = 0x01; + Motor2.motor_status.calc_rpm = 0; + } +} + + #endif /* INTERRUPTS_H_ */ \ No newline at end of file diff --git a/2_Motor_Master/Motor_Master/Motor_Master/main.c b/2_Motor_Master/Motor_Master/Motor_Master/main.c index 3bf90e9..a91aa00 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/main.c +++ b/2_Motor_Master/Motor_Master/Motor_Master/main.c @@ -79,8 +79,10 @@ void One_ms_timer_init(void) void enable_NVIC_IRQ(void) { - //NVIC_EnableIRQ(TC7_IRQn); // TC7: TC_ECAT - //NVIC_EnableIRQ(TC0_IRQn); // TC0: TC_SPEED + //hri_tc_clear_INTEN_OVF_bit(TC2); + //hri_tc_clear_INTEN_OVF_bit(TC4); + NVIC_EnableIRQ(TC2_IRQn); // TC2: M1_Speed_Timer + NVIC_EnableIRQ(TC4_IRQn); // TC4: M2_Speed_Timer NVIC_EnableIRQ(DMAC_0_IRQn); NVIC_EnableIRQ(TCC1_0_IRQn); NVIC_EnableIRQ(TCC1_0_IRQn); @@ -175,9 +177,6 @@ int main(void) //speed_timer_init(); enable_NVIC_IRQ(); - - - /* Replace with your application code */ while (1) { //delay_ms(100); diff --git a/2_Motor_Master/Motor_Master/Motor_Master/motorparameters.h b/2_Motor_Master/Motor_Master/Motor_Master/motorparameters.h index a688f0d..668eaa3 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/motorparameters.h +++ b/2_Motor_Master/Motor_Master/Motor_Master/motorparameters.h @@ -71,109 +71,48 @@ ** W7 | M1_0 | CC0 || M2_0 | CC0 | */ -/* CCW from Back - works best in forward diraction*/ -//static const uint16_t COMMUTATION_PATTERN[] = { - //0x003F, // (0) invalid state - //0x053D, // (1) - //0x1437, // (2) - //0x113D, // (3) - //0x111F, // (4) - //0x141F, // (5) - //0x0537, // (6) - //0x003F, // (7) invalid state - //0x003F, // (8) invalid state - //0x0537, // (9) - //0x141F, // (10) - //0x111F, // (11) - //0x113D, // (12) - //0x1437, // (13) - //0x053D, // (14) - //0x003F // (15) invalid state -//}; - -////* CW From Back */ -//static const uint16_t COMMUTATION_PATTERN[16] = { - //0x003F, // (0) invalid state - //0x0537, // (1) - //0x141F, // (2) - //0x111F, // (3) - //0x113D, // (4) - //0x1437, // (5) - //0x053D, // (6) - //0x003F, // (7) invalid state - //0x003F, // (8) invalid state - //0x053D, // (9) - //0x1437, // (10) - //0x113D, // (11) - //0x111F, // (12) - //0x141F, // (13) - //0x0537, // (14) - //0x003F // (15) invalid state -//}; - -////* CW From Back */ -//static const uint16_t COMMUTATION_PATTERN[16] = { - //0x003F, // (0) invalid state - //0x0537, // (1) - //0x141F, // (2) - //0x111F, // (3) - //0x113D, // (4) - //0x1437, // (5) - //0x053D, // (6) - //0x003F, // (7) invalid state - //0x003F, // (8) invalid state - //0x0537, // (9) - //0x053D, // (10) - //0x1437, // (11) - //0x113D, // (12) - //0x111F, // (13) - //0x141F, // (14) - //0x003F // (15) invalid state -//}; -// - #define DISABLE_PATTERN 0x003F -////* A&C phase swapped */ -static const uint16_t COMMUTATION_PATTERN[16] = { - 0x003F, // (0) invalid state - 0x1437, // (1) - 0x053D, // (2) - 0x113D, // (3) - 0x111F, // (4) - 0x0537, // (5) - 0x141F, // (6) - 0x003F, // (7) invalid state - 0x003F, // (8) invalid state - 0x141F, // (9) - 0x0537, // (10) - 0x111F, // (11) - 0x113D, // (12) - 0x053D, // (13) - 0x1437, // (14) - 0x003F, // (15) invalid state -}; - -//////* A&C phase swapped and modified to match currents */ +//////* A&C phase swapped */ //static const uint16_t COMMUTATION_PATTERN[16] = { //0x003F, // (0) invalid state - //0x141F, // (1) - //0x0537, // (2) - //0x111F, // (3) - //0x113D, // (4) - //0x053D, // (5) - //0x1437, // (6) + //0x1437, // (1) + //0x053D, // (2) + //0x113D, // (3) + //0x111F, // (4) + //0x0537, // (5) + //0x141F, // (6) //0x003F, // (7) invalid state //0x003F, // (8) invalid state - //0x1437, // (9) - //0x053D, // (10) - //0x113D, // (11) - //0x111F, // (12) - //0x0537, // (13) - //0x141F, // (14) - //0x003F // (15) invalid state + //0x141F, // (9) + //0x0537, // (10) + //0x111F, // (11) + //0x113D, // (12) + //0x053D, // (13) + //0x1437, // (14) + //0x003F, // (15) invalid state //}; +////* A&C phase swapped and modified to match currents */ +static const uint16_t COMMUTATION_PATTERN[16] = { + 0x003F, // (0) invalid state + 0x141F, // (1) + 0x0537, // (2) + 0x111F, // (3) + 0x113D, // (4) + 0x053D, // (5) + 0x1437, // (6) + 0x003F, // (7) invalid state + 0x003F, // (8) invalid state + 0x1437, // (9) + 0x053D, // (10) + 0x113D, // (11) + 0x111F, // (12) + 0x0537, // (13) + 0x141F, // (14) + 0x003F // (15) invalid state +}; + // ---------------------------------------------------------------------- // Motor Physical Parameters // ---------------------------------------------------------------------- diff --git a/2_Motor_Master/Motor_Master/Motor_Master/statemachine.h b/2_Motor_Master/Motor_Master/Motor_Master/statemachine.h index 7c24574..3bbb186 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/statemachine.h +++ b/2_Motor_Master/Motor_Master/Motor_Master/statemachine.h @@ -59,9 +59,9 @@ typedef enum typedef enum { - MOTOR_NOFAULT = 0, - MOTOR_HALLSENSORINVALID = 1, - MOTOR_DRIVER_OVER_CURRENT = 2, + MOTOR_NOFAULT = 0xE1, + MOTOR_HALLSENSORINVALID = 0xE2, + MOTOR_DRIVER_OVER_CURRENT = 0xE3, } MOTOR_FAULTS_t; typedef struct MOTOR_STATE diff --git a/Twincat/MotorData/.vs/MotorData/v15/.suo b/Twincat/MotorData/.vs/MotorData/v15/.suo index c74cc05bdc5b1262b244b7054749f3ae297d6677..02fe54571a259ce61074cd7434a987e1cb5a344d 100644 GIT binary patch delta 2356 zcmdT_Z%kWN6u_C zbts}2)`WUK3sd4ee-pOFKIqiA_zX-*evxu0K%_&nM{&|cRUBPF z-Ij;<8D2vDWyCqeD=9lHS9|!h8h@wSth7Y6X)FA#x%hQd!7B zL(PC1zD9+{kxAX?l{giXSd(4}5o184DpIA>Ms9^|<2<*+l<7T)*2o~&e0txi!Pap< zRSG=CT9|G=rRoWr7Vb7*lQ=b)n43OAH;0ptIP6%i>#5mkJ8b3l>#4gLL!_Z!FYhzx z==ISv*eU!zc|<85QPeFG&*>f63p^4~i%-PQ7`IFTkw`}c$`~VNKX^6uux0j3+=?Fi zf4r7uFk2fWPV4Ed+KaPd#-=tUU!{mrf_XQ-d({@z@{94ed{l;&p*p2k2PMbefIs~Xa8_9#O8V)N z^n)GgT!cEE*!RExUd38xKKdRm~Tas|HeZNO*2R0prbuLgQJojK>9gpxi-QoYQ( zV7GAnpP=NMYJiQ!m9i)?{lT}|qYdq2C;c$JZ7vyudz;bptzWUs1Hhk*~9AFFKv>WaW zIPmf=&<&hXx0C07m0*3WHJ%u&thi%$zEk;#dQnk{|;wOYVZ$rc9ecp z>`TNA{l~w9?fhTx^>~*_FAlO17GhHv{5s2q-Ekk!0bi(5V_+f}a3;aC<0rWhehB%v z3CbppaWnWPeB1&HAs;MF$n$JTuVb5xS7&cBp-8i-L2c=fj?A3QIrK2lL(D7?ae?ZT zGA1b%liALgaT)h-XQG|V^suN4a_KEsgFuJchDM;Bb|@m(5rIUFFheOSAz0n_$U)G$gJ_>X96)3vs0#V> z;EM3lVnBdI0={y&`6URO zr}#K*nV%Q9Ixq%dOPffg=;FvyzSxg{8i@`L+2NBIwRArsl>s4#U!*&?tDM{pTaGDS z1mWyC*;ywRV(gGA4k2e7_{2(xE4CIQt_85>ek|mxlj6A@LY~?Y%4?Gt-D65C3Pm~} zT`CP(b^b!1QX*8?CUI&=5hBexUWf#=m2$BT#)_I~s@Gy%fNiPqpI@6Hac%Hx-WRkr z#7cDVAir63Vu_RLcnK$lhq^8da^hY<3rjXHWLd57lPyaYS0zsG1Y^oj?jSBIt$>i^7fES< z4B`d-m3s7VVYU&Tb2q`~t{Moq>(DkomX10EwS@GFgkam%__&Ihmh)Ftoo$ks6S?f! zT8JM^oYi{>b(f>)4q|Q;N6s3RHcsL$@6fx1N4ASR2x0pa55ShaO?08tjiB2}NN1m4 z6qAmWQ>2Vl`nuqNpNEc<4xsRs@6$5GfM^f5)hB<@S66>+!2;29@)d?r+X8GP$a2)wT6E^J=3LZG`IJbj55 zve|oEc^#X*BeY3$C(xRB9kHHKWpS)$3BK%Whd}=ZEWWlJ4UD}nf$n+@eM{i-bZs