From 797226616e08899e14cd0ceb607c33d03563cb22 Mon Sep 17 00:00:00 2001 From: Nicolas Trimborn Date: Thu, 12 Aug 2021 14:07:23 +0200 Subject: [PATCH] position control added --- .../Motor_Master/Motor_Master/bldc.c | 53 +++++++----------- .../Motor_Master/Motor_Master/bldc.h | 2 +- .../Motor_Master/Motor_Master/main.c | 15 ++--- .../Motor_Master/motorparameters.h | 2 +- Twincat/MotorData/.vs/MotorData/v15/.suo | Bin 36864 -> 36864 bytes 5 files changed, 29 insertions(+), 43 deletions(-) diff --git a/2_Motor_Master/Motor_Master/Motor_Master/bldc.c b/2_Motor_Master/Motor_Master/Motor_Master/bldc.c index d52ad43..a72c789 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/bldc.c +++ b/2_Motor_Master/Motor_Master/Motor_Master/bldc.c @@ -35,7 +35,7 @@ void motor_StateMachine(BLDCMotor_t* const motor) case MOTOR_IDLE: //hri_tcc_write_PATTBUF_reg(motor->motor_param->pwm_desc->device.hw, DISABLE_PATTERN); motor->motor_state.previousstate = motor->motor_state.currentstate; - motor->motor_state.currentstate = MOTOR_VI_CTRL_STATE; + motor->motor_state.currentstate = MOTOR_PVI_CTRL_STATE; break; case MOTOR_OPEN_LOOP_STATE: BLDC_runOpenLoop(motor, *M1_Desired_dc); @@ -51,42 +51,31 @@ void motor_StateMachine(BLDCMotor_t* const motor) switch (motor->regulation_loop_count) { case 0: /* PWM FREQ / 25 - 1kHz */ case 5: case 10: case 15: case 20:/* PWM FREQ / 5 - 5kHz */ - calculate_motor_speed(motor); - BLDC_runSpeedCntl(motor, (float32_t)motor->motor_status.calc_rpm, (float32_t)motor->motor_setpoints.desired_speed); + calculate_motor_speed(motor); + BLDC_runSpeedCntl(motor, (float32_t)motor->motor_status.calc_rpm, (float32_t)motor->motor_setpoints.desired_speed); default: /* PWM FREQ - 25kHz */ - select_active_phase(motor); - BLDC_runCurrentCntl(motor,(float32_t)motor->Iphase_pu.Bus, (float32_t)motor->controllers.Pi_Idc.Ref_pu); + select_active_phase(motor); + BLDC_runCurrentCntl(motor,(float32_t)motor->Iphase_pu.Bus, (float32_t)motor->controllers.Pi_Idc.Ref_pu); break; } // end switch(regulation_loop_count) if(motor->regulation_loop_count > 23) motor->regulation_loop_count = 0; else motor->regulation_loop_count++; break; case MOTOR_PVI_CTRL_STATE: - switch (Motor1.regulation_loop_count) { + switch (motor->regulation_loop_count) { case 0: /* PWM FREQ / 25 - 1kHz */ - Motor1.timerflags.motor_telemetry_flag = true; // Update telemetry flag - BLDC_runPosCntl(&Motor1, Motor1.motor_status.Num_Steps, Motor1.motor_setpoints.desired_position); - BLDC_runPosCntl(&Motor2, Motor1.motor_status.Num_Steps, Motor1.motor_setpoints.desired_position); - + BLDC_runPosCntl(motor, motor->motor_status.Num_Steps, motor->motor_setpoints.desired_position); case 5: case 10: case 15: case 20:/* PWM FREQ / 5 - 5kHz */ - - //calculate_motor_speed(motor); - //calculate_motor_speed(); - - BLDC_runSpeedCntl(&Motor1, Motor1.motor_status.calc_rpm, Motor1.controllers.Pid_Speed.Ref_pu); - BLDC_runSpeedCntl(&Motor2, Motor1.motor_status.calc_rpm, Motor1.controllers.Pid_Speed.Ref_pu); - + calculate_motor_speed(motor); + BLDC_runSpeedCntl(motor, (float32_t)motor->motor_status.calc_rpm, (float32_t)motor->controllers.Pid_Speed.Ref_pu); default: /* PWM FREQ - 25kHz */ - select_active_phase(motor); - //select_active_phase(&Motor2, Motor2.motor_status.currentHallPattern); - - BLDC_runCurrentCntl(&Motor1, Motor1.Iphase_pu.Bus, Motor1.controllers.Pi_Idc.Ref_pu); - BLDC_runCurrentCntl(&Motor2, Motor1.Iphase_pu.Bus, Motor1.controllers.Pi_Idc.Ref_pu); - + select_active_phase(motor); + //select_active_phase(&Motor2, Motor2.motor_status.currentHallPattern); + BLDC_runCurrentCntl(motor, motor->Iphase_pu.Bus, motor->controllers.Pi_Idc.Ref_pu); break; } // end switch(regulation_loop_count) - if(Motor1.regulation_loop_count > 23) Motor1.regulation_loop_count = 0; - else Motor1.regulation_loop_count++; + if(motor->regulation_loop_count > 23) motor->regulation_loop_count = 0; + else motor->regulation_loop_count++; break; } //end switch (motor->motor_state.currentstate) @@ -220,13 +209,13 @@ 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_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 diff --git a/2_Motor_Master/Motor_Master/Motor_Master/bldc.h b/2_Motor_Master/Motor_Master/Motor_Master/bldc.h index e5ae0dc..c07cdb6 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/bldc.h +++ b/2_Motor_Master/Motor_Master/Motor_Master/bldc.h @@ -17,7 +17,7 @@ #include "statemachine.h" #define PWM_TOP (1000) -#define MAX_PWM (400) +#define MAX_PWM (300) //#define MAX_VEL 3800 #define CW (0) //CBA diff --git a/2_Motor_Master/Motor_Master/Motor_Master/main.c b/2_Motor_Master/Motor_Master/Motor_Master/main.c index 5ce61d2..2460a15 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/main.c +++ b/2_Motor_Master/Motor_Master/Motor_Master/main.c @@ -181,17 +181,14 @@ int main(void) /* Replace with your application code */ while (1) { - //delay_ms(100); - //*M1_Motor__hallState = readM1Hall(); - //*M2_Motor__hallState = readM2Hall(); - //x += 1; - //comms_check(); - //update_setpoints(); if (Motor1.timerflags.adc_readings_ready_tic) {process_currents();} - if (Motor1.timerflags.current_loop_tic) {APPLICATION_StateMachine();} + if (Motor1.timerflags.current_loop_tic) { + APPLICATION_StateMachine(); + exec_commutation(&Motor1); + exec_commutation(&Motor2); + } if (run_ECAT) {ECAT_STATE_MACHINE();} - exec_commutation(&Motor1); - exec_commutation(&Motor2); + } } diff --git a/2_Motor_Master/Motor_Master/Motor_Master/motorparameters.h b/2_Motor_Master/Motor_Master/Motor_Master/motorparameters.h index 668eaa3..e734f65 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/motorparameters.h +++ b/2_Motor_Master/Motor_Master/Motor_Master/motorparameters.h @@ -167,7 +167,7 @@ const static BLDCMotor_param_t FH_32mm24BXTR = { .motor_Max_Spd_RPM = 3000, .motor_MeasureRange_RPM = 3200, //(1.2f * MOTOR_MAX_SPD_RPM)f // give 20% headroom .motor_Max_Spd_ELEC = 12000, //(MOTOR_MAX_SPD_RPM/60)*MOTOR_POLEPAIRS - .motor_Max_Current_IDC_A = 1.2, + .motor_Max_Current_IDC_A = 1.0, }; #endif /* MOTORPARAMETERS_H_ */ \ No newline at end of file diff --git a/Twincat/MotorData/.vs/MotorData/v15/.suo b/Twincat/MotorData/.vs/MotorData/v15/.suo index 02fe54571a259ce61074cd7434a987e1cb5a344d..cdf1e0bf153873db4d61b8076527fa24476a95a2 100644 GIT binary patch delta 493 zcmZozz|^pSX+sVZ--QD2?S=i<8C4j7KxuLnQx#+T9JtM`>zpKCC6d zG?`7;YLf1OO?q^9F{9e%ucq~2_nC8x6L8<65TVHvGZ$=5NK4>cRKNjYS1jS3?9yhl q*`ZC2ZSuSxrO6FF4x4%%m?j_Sv6$?j!ZO*W-)6D_6UXF+eg^=YGOeiq delta 553 zcmZozz|^pSX+sVZ9|P0vse%318C4h<7!D{-u41ZUY@7U*shKf)awT&$W9#Ox%;ijc z4+K|x1OZhE;#9@1&a_#9>jC4$1eQ%aB}|OflS}#P8LK9L1(KDMOZn?L(}8-uSb)T2 zK>^{(0s=KW|Nj5~55j67!5IQC6kr0$K!Fq>P6gsLAO?wN0O`pK1RW-e2nbC6AlSl~ zG`Uo$o-uLqSE2trszAxZd_V#SCUMEYT_RS($f&ZpRGeFov2JpshCUwG3s8(@@)1QF0Dy_aEC2ui