From e367b994f2ebc60b665c8f00ee85f8cc8519f282 Mon Sep 17 00:00:00 2001 From: Nicolas Trimborn Date: Wed, 11 Aug 2021 20:41:58 +0200 Subject: [PATCH] got speed calc working. Still goes in wrong direction --- .../.atmelstart/atmel_start_config.atstart | 24 +- .../Motor_Master/Config/hpl_adc_config.h | 6 +- .../Motor_Master/Config/hpl_ccl_config.h | 2 +- .../Motor_Master/Config/hpl_evsys_config.h | 16 +- .../Motor_Master/EtherCAT_SlaveDef.h | 6 +- .../Motor_Master/Ethercat_SlaveDef.h | 6 +- .../Motor_Master/Motor_Master.cproj | 19 +- .../Motor_Master/Motor_Master/bldc.c | 160 +++--- .../Motor_Master/Motor_Master/bldc.h | 15 +- .../Motor_Master/Motor_Master/bldc_types.h | 18 +- .../Motor_Master/Motor_Master/configuration.h | 25 +- .../Motor_Master/Motor_Master/control.h | 4 +- .../Motor_Master/Motor_Master/driver_init.c | 2 + .../Motor_Master/Motor_Master/main.c | 45 +- .../Motor_Master/motorparameters.h | 32 +- BLDC_E54v2/BLCD_E54v2/BLCD_E54v2/bldc.h | 2 +- Twincat/MotorData/.vs/MotorData/v15/.suo | Bin 0 -> 36864 bytes Twincat/MotorData/MotorData.sln | 69 +++ Twincat/MotorData/MotorData/MotorData.tsproj | 455 ++++++++++++++++++ .../Motordata_PLC/GVLs/GVL_motor_data.TcGVL | 31 ++ .../Motordata_PLC/Motordata_PLC.plcproj | 74 +++ .../MotorData/Motordata_PLC/POUs/MAIN.TcPOU | 13 + .../MotorData/Motordata_PLC/PlcTask.TcTTO | 17 + .../TwinCAT Measurement Project1.tcmproj | 14 + .../XY Scope Project.tcscopex | 248 ++++++++++ 25 files changed, 1132 insertions(+), 171 deletions(-) create mode 100644 Twincat/MotorData/.vs/MotorData/v15/.suo create mode 100644 Twincat/MotorData/MotorData.sln create mode 100644 Twincat/MotorData/MotorData/MotorData.tsproj create mode 100644 Twincat/MotorData/MotorData/Motordata_PLC/GVLs/GVL_motor_data.TcGVL create mode 100644 Twincat/MotorData/MotorData/Motordata_PLC/Motordata_PLC.plcproj create mode 100644 Twincat/MotorData/MotorData/Motordata_PLC/POUs/MAIN.TcPOU create mode 100644 Twincat/MotorData/MotorData/Motordata_PLC/PlcTask.TcTTO create mode 100644 Twincat/MotorData/TwinCAT Measurement Project1/TwinCAT Measurement Project1.tcmproj create mode 100644 Twincat/MotorData/TwinCAT Measurement Project1/XY Scope Project.tcscopex diff --git a/2_Motor_Master/Motor_Master/Motor_Master/.atmelstart/atmel_start_config.atstart b/2_Motor_Master/Motor_Master/Motor_Master/.atmelstart/atmel_start_config.atstart index 05d550c..ce8529c 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/.atmelstart/atmel_start_config.atstart +++ b/2_Motor_Master/Motor_Master/Motor_Master/.atmelstart/atmel_start_config.atstart @@ -112,11 +112,11 @@ drivers: adc_arch_winmode: No window mode adc_arch_winmoneo: false adc_arch_winut: 0 - adc_differential_mode: false + adc_differential_mode: true adc_freerunning_mode: false - adc_pinmux_negative: Internal ground + adc_pinmux_negative: ADC AIN0 pin adc_pinmux_positive: ADC AIN6 pin - adc_prescaler: Peripheral clock divided by 16 + adc_prescaler: Peripheral clock divided by 8 adc_reference: External reference A adc_resolution: 12-bit optional_signals: @@ -206,7 +206,7 @@ drivers: ccl_arch_lutei_3: false ccl_arch_luteo_0: true ccl_arch_luteo_1: false - ccl_arch_luteo_2: false + ccl_arch_luteo_2: true ccl_arch_luteo_3: false ccl_arch_runstdby: false ccl_arch_seqsel_0: Sequential logic is disabled @@ -1014,9 +1014,9 @@ drivers: evsys_channel_43: No channel output selected evsys_channel_44: No channel output selected evsys_channel_45: No channel output selected - evsys_channel_46: No channel output selected + evsys_channel_46: Channel 1 evsys_channel_47: No channel output selected - evsys_channel_48: No channel output selected + evsys_channel_48: Channel 2 evsys_channel_49: No channel output selected evsys_channel_5: No channel output selected evsys_channel_50: No channel output selected @@ -1041,7 +1041,7 @@ drivers: evsys_channel_8: No channel output selected evsys_channel_9: No channel output selected evsys_channel_setting_0: true - evsys_channel_setting_1: false + evsys_channel_setting_1: true evsys_channel_setting_10: false evsys_channel_setting_11: false evsys_channel_setting_12: false @@ -1052,7 +1052,7 @@ drivers: evsys_channel_setting_17: false evsys_channel_setting_18: false evsys_channel_setting_19: false - evsys_channel_setting_2: false + evsys_channel_setting_2: true evsys_channel_setting_20: false evsys_channel_setting_21: false evsys_channel_setting_22: false @@ -1169,7 +1169,7 @@ drivers: evsys_evd_8: false evsys_evd_9: false evsys_evgen_0: TCC0 overflow - evsys_evgen_1: No event generator + evsys_evgen_1: CCL LUT output 0 evsys_evgen_10: No event generator evsys_evgen_11: No event generator evsys_evgen_12: No event generator @@ -1180,7 +1180,7 @@ drivers: evsys_evgen_17: No event generator evsys_evgen_18: No event generator evsys_evgen_19: No event generator - evsys_evgen_2: No event generator + evsys_evgen_2: CCL LUT output 2 evsys_evgen_20: No event generator evsys_evgen_21: No event generator evsys_evgen_22: No event generator @@ -1265,7 +1265,7 @@ drivers: evsys_ovr_8: false evsys_ovr_9: false evsys_path_0: Asynchronous path - evsys_path_1: Synchronous path + evsys_path_1: Asynchronous path evsys_path_10: Synchronous path evsys_path_11: Synchronous path evsys_path_12: Synchronous path @@ -1276,7 +1276,7 @@ drivers: evsys_path_17: Synchronous path evsys_path_18: Synchronous path evsys_path_19: Synchronous path - evsys_path_2: Synchronous path + evsys_path_2: Asynchronous path evsys_path_20: Synchronous path evsys_path_21: Synchronous path evsys_path_22: Synchronous path diff --git a/2_Motor_Master/Motor_Master/Motor_Master/Config/hpl_adc_config.h b/2_Motor_Master/Motor_Master/Motor_Master/Config/hpl_adc_config.h index a4ed186..82de8c4 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/Config/hpl_adc_config.h +++ b/2_Motor_Master/Motor_Master/Motor_Master/Config/hpl_adc_config.h @@ -339,7 +339,7 @@ // These bits define the ADC clock relative to the peripheral clock (PRESCALER) // adc_prescaler #ifndef CONF_ADC_1_PRESCALER -#define CONF_ADC_1_PRESCALER 0x3 +#define CONF_ADC_1_PRESCALER 0x2 #endif // Free Running Mode @@ -353,7 +353,7 @@ // In differential mode, the voltage difference between the MUXPOS and MUXNEG inputs will be converted by the ADC. (DIFFMODE) // adc_differential_mode #ifndef CONF_ADC_1_DIFFMODE -#define CONF_ADC_1_DIFFMODE 0 +#define CONF_ADC_1_DIFFMODE 1 #endif // Positive Mux Input Selection @@ -399,7 +399,7 @@ // These bits define the Mux selection for the negative ADC input. (MUXNEG) // adc_pinmux_negative #ifndef CONF_ADC_1_MUXNEG -#define CONF_ADC_1_MUXNEG 0x18 +#define CONF_ADC_1_MUXNEG 0x0 #endif // diff --git a/2_Motor_Master/Motor_Master/Motor_Master/Config/hpl_ccl_config.h b/2_Motor_Master/Motor_Master/Motor_Master/Config/hpl_ccl_config.h index 4b3f0f6..694e623 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/Config/hpl_ccl_config.h +++ b/2_Motor_Master/Motor_Master/Motor_Master/Config/hpl_ccl_config.h @@ -336,7 +336,7 @@ // Event output enable // ccl_arch_luteo_2 #ifndef CONF_CCL_LUTEO_2 -#define CONF_CCL_LUTEO_2 0 +#define CONF_CCL_LUTEO_2 1 #endif // Event input enable diff --git a/2_Motor_Master/Motor_Master/Motor_Master/Config/hpl_evsys_config.h b/2_Motor_Master/Motor_Master/Motor_Master/Config/hpl_evsys_config.h index f5bebaa..5d22b57 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/Config/hpl_evsys_config.h +++ b/2_Motor_Master/Motor_Master/Motor_Master/Config/hpl_evsys_config.h @@ -188,7 +188,7 @@ // Channel 1 settings // evsys_channel_setting_1 #ifndef CONF_EVSYS_CHANNEL_SETTINGS_1 -#define CONF_EVSYS_CHANNEL_SETTINGS_1 0 +#define CONF_EVSYS_CHANNEL_SETTINGS_1 1 #endif // Edge detection @@ -209,7 +209,7 @@ // Asynchronous path // evsys_path_1 #ifndef CONF_PATH_1 -#define CONF_PATH_1 EVSYS_CHANNEL_PATH_SYNCHRONOUS_Val +#define CONF_PATH_1 EVSYS_CHANNEL_PATH_ASYNCHRONOUS_Val #endif // Event generator @@ -333,7 +333,7 @@ // <0x77=>CCL LUT output 3 // evsys_evgen_1 #ifndef CONF_EVGEN_1 -#define CONF_EVGEN_1 0 +#define CONF_EVGEN_1 116 #endif // Overrun channel interrupt @@ -369,7 +369,7 @@ // Channel 2 settings // evsys_channel_setting_2 #ifndef CONF_EVSYS_CHANNEL_SETTINGS_2 -#define CONF_EVSYS_CHANNEL_SETTINGS_2 0 +#define CONF_EVSYS_CHANNEL_SETTINGS_2 1 #endif // Edge detection @@ -390,7 +390,7 @@ // Asynchronous path // evsys_path_2 #ifndef CONF_PATH_2 -#define CONF_PATH_2 EVSYS_CHANNEL_PATH_SYNCHRONOUS_Val +#define CONF_PATH_2 EVSYS_CHANNEL_PATH_ASYNCHRONOUS_Val #endif // Event generator @@ -514,7 +514,7 @@ // <0x77=>CCL LUT output 3 // evsys_evgen_2 #ifndef CONF_EVGEN_2 -#define CONF_EVGEN_2 0 +#define CONF_EVGEN_2 118 #endif // Overrun channel interrupt @@ -7544,7 +7544,7 @@ // evsys_channel_46 // Indicates which channel is chosen for user #ifndef CONF_CHANNEL_46 -#define CONF_CHANNEL_46 0 +#define CONF_CHANNEL_46 2 #endif // Channel selection for TC3 event @@ -7624,7 +7624,7 @@ // evsys_channel_48 // Indicates which channel is chosen for user #ifndef CONF_CHANNEL_48 -#define CONF_CHANNEL_48 0 +#define CONF_CHANNEL_48 3 #endif // Channel selection for TC5 event diff --git a/2_Motor_Master/Motor_Master/Motor_Master/EtherCAT_SlaveDef.h b/2_Motor_Master/Motor_Master/Motor_Master/EtherCAT_SlaveDef.h index e4e6c55..7629170 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/EtherCAT_SlaveDef.h +++ b/2_Motor_Master/Motor_Master/Motor_Master/EtherCAT_SlaveDef.h @@ -148,7 +148,8 @@ static void update_telemetry(void) *M1_Motor_currentPhC = convert_to_mA(Motor1.Iphase_pu.C); *M1_Motor__hallState = Motor1.motor_status.currentHallPattern; *M1_Motor_dutyCycle = Motor1.motor_status.duty_cycle; - // + *M1_Motor_speed = (int16_t)Motor1.motor_status.calc_rpm; + *M1_Joint_abs_position = Motor1.motor_status.actualDirection; /* Motor 2 */ *M2_Status = Motor2.motor_state.currentstate; *M2_Joint_rel_position = Motor2.motor_status.Num_Steps; @@ -160,7 +161,8 @@ static void update_telemetry(void) *M2_Motor_currentPhC = convert_to_mA(Motor2.Iphase_pu.C); *M2_Motor__hallState = Motor2.motor_status.currentHallPattern; *M2_Motor_dutyCycle = Motor2.motor_status.duty_cycle; - + *M2_Motor_speed = (int16_t)Motor2.motor_status.calc_rpm; + *M2_Joint_abs_position = Motor2.motor_status.actualDirection; } static void update_setpoints(void) diff --git a/2_Motor_Master/Motor_Master/Motor_Master/Ethercat_SlaveDef.h b/2_Motor_Master/Motor_Master/Motor_Master/Ethercat_SlaveDef.h index e4e6c55..7629170 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/Ethercat_SlaveDef.h +++ b/2_Motor_Master/Motor_Master/Motor_Master/Ethercat_SlaveDef.h @@ -148,7 +148,8 @@ static void update_telemetry(void) *M1_Motor_currentPhC = convert_to_mA(Motor1.Iphase_pu.C); *M1_Motor__hallState = Motor1.motor_status.currentHallPattern; *M1_Motor_dutyCycle = Motor1.motor_status.duty_cycle; - // + *M1_Motor_speed = (int16_t)Motor1.motor_status.calc_rpm; + *M1_Joint_abs_position = Motor1.motor_status.actualDirection; /* Motor 2 */ *M2_Status = Motor2.motor_state.currentstate; *M2_Joint_rel_position = Motor2.motor_status.Num_Steps; @@ -160,7 +161,8 @@ static void update_telemetry(void) *M2_Motor_currentPhC = convert_to_mA(Motor2.Iphase_pu.C); *M2_Motor__hallState = Motor2.motor_status.currentHallPattern; *M2_Motor_dutyCycle = Motor2.motor_status.duty_cycle; - + *M2_Motor_speed = (int16_t)Motor2.motor_status.calc_rpm; + *M2_Joint_abs_position = Motor2.motor_status.actualDirection; } static void update_setpoints(void) diff --git a/2_Motor_Master/Motor_Master/Motor_Master/Motor_Master.cproj b/2_Motor_Master/Motor_Master/Motor_Master/Motor_Master.cproj index 7fdad9a..eaf15a1 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/Motor_Master.cproj +++ b/2_Motor_Master/Motor_Master/Motor_Master/Motor_Master.cproj @@ -150,7 +150,7 @@ - + @@ -207,12 +207,12 @@ - - + + - + @@ -228,7 +228,7 @@ com.atmel.avrdbg.tool.atmelice - J42700013870 + J42700013287 0x61810302 SWD @@ -239,7 +239,7 @@ SWD com.atmel.avrdbg.tool.atmelice - J42700013870 + J42700013287 Atmel-ICE 2000000 @@ -387,6 +387,7 @@ %24(PackRepoDir)\arm\CMSIS\5.4.0\CMSIS\Core\Include\ + %24(PackRepoDir)\atmel\SAME51_DFP\1.1.139\include ../Config ../ ../examples @@ -411,10 +412,8 @@ ../hpl/tc ../hpl/tcc ../hri - %24(PackRepoDir)\atmel\SAME51_DFP\1.1.139\include - Optimize more (-O2) True Maximum (-g3) True @@ -439,6 +438,7 @@ %24(PackRepoDir)\arm\CMSIS\5.4.0\CMSIS\Core\Include\ + %24(PackRepoDir)\atmel\SAME51_DFP\1.1.139\include ../Config ../ ../examples @@ -463,13 +463,13 @@ ../hpl/tc ../hpl/tcc ../hri - %24(PackRepoDir)\atmel\SAME51_DFP\1.1.139\include Default (-g) %24(PackRepoDir)\arm\CMSIS\5.4.0\CMSIS\Core\Include\ + %24(PackRepoDir)\atmel\SAME51_DFP\1.1.139\include ../Config ../ ../examples @@ -494,7 +494,6 @@ ../hpl/tc ../hpl/tcc ../hri - %24(PackRepoDir)\atmel\SAME51_DFP\1.1.139\include Default (-Wa,-g) diff --git a/2_Motor_Master/Motor_Master/Motor_Master/bldc.c b/2_Motor_Master/Motor_Master/Motor_Master/bldc.c index 30a04f5..662768c 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/bldc.c +++ b/2_Motor_Master/Motor_Master/Motor_Master/bldc.c @@ -11,8 +11,7 @@ #include "utilities.h" #include "Ethercat_SlaveDef.h" - -void motor_StateMachine(BLDCMotor_t *motor) +void motor_StateMachine(BLDCMotor_t* const motor) { if (motor->motor_state.fault) @@ -34,11 +33,14 @@ void motor_StateMachine(BLDCMotor_t *motor) motor->motor_state.currentstate = MOTOR_IDLE; break; 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_OPEN_LOOP_STATE; break; case MOTOR_OPEN_LOOP_STATE: - BLDC_runOpenLoop(motor, *M1_Desired_dc); + BLDC_runOpenLoop(motor, *M1_Desired_dc); + calculate_motor_speed(motor); + motor->motor_state.previousstate = motor->motor_state.currentstate; break; case MOTOR_PVI_CTRL_STATE: switch (Motor1.regulation_loop_count) { @@ -49,7 +51,7 @@ void motor_StateMachine(BLDCMotor_t *motor) case 5: case 10: case 15: case 20:/* PWM FREQ / 5 - 5kHz */ - //calculate_motor_speed(); + //calculate_motor_speed(motor); //calculate_motor_speed(); BLDC_runSpeedCntl(&Motor1, Motor1.motor_status.calc_rpm, Motor1.controllers.Pid_Speed.Ref_pu); @@ -75,7 +77,7 @@ void motor_StateMachine(BLDCMotor_t *motor) } -void BldcInitStruct(BLDCMotor_t *motor, BLDCMotor_param_t *motor_param) +void BldcInitStruct(BLDCMotor_t* const motor, BLDCMotor_param_t * const motor_param) { // ---------------------------------------------------------------------- // Assign Motor Parameters: @@ -97,7 +99,7 @@ void BldcInitStruct(BLDCMotor_t *motor, BLDCMotor_param_t *motor_param) motor->motor_status.Num_Steps = 0; motor->motor_status.cur_comm_step = 0; motor->motor_status.currentHallPattern = 1; - motor->motor_status.nextHallPattern = 3; + //motor->motor_status.nextHallPattern = 3; motor->motor_setpoints.directionOffset = 0; // ---------------------------------------------------------------------- @@ -179,7 +181,7 @@ void BldcInitStruct(BLDCMotor_t *motor, BLDCMotor_param_t *motor_param) motor->controllers.Pi_Pos.OutMin_pu = -(motor_param->motor_Max_Spd_RPM); } -void exec_commutation(BLDCMotor_t *motor) +void exec_commutation(BLDCMotor_t* const motor) { // ---------------------------------------------------------------------- @@ -190,45 +192,41 @@ void exec_commutation(BLDCMotor_t *motor) volatile uint8_t currentHall = 1; currentHall = motor->readHall(); motor->motor_status.currentHallPattern = currentHall; - - - volatile uint8_t currentHallv2 = readHallSensorM1v2(); - - - + // ---------------------------------------------------------------------- // Set Pattern Buffers // ---------------------------------------------------------------------- - volatile uint16_t temp_M1 = COMMUTATION_PATTERN[motor->motor_status.currentHallPattern + + volatile uint16_t temp_M1 = COMMUTATION_PATTERN[currentHall + motor->motor_setpoints.directionOffset]; hri_tcc_write_PATTBUF_reg(motor->motor_param->pwm_desc->device.hw, temp_M1); // ---------------------------------------------------------------------- - // Set Calculated Duty Cycles + // Set Calculated Duty Cycle // ---------------------------------------------------------------------- hri_tcc_write_CCBUF_CCBUF_bf(motor->motor_param->pwm_desc->device.hw, 0, motor->motor_status.duty_cycle); - motor->motor_status.cur_comm_step = MOTOR_COMMUTATION_STEPS[motor->motor_status.currentHallPattern]; - volatile int8_t step_change1 = motor->motor_status.cur_comm_step - motor->motor_status.prev_comm_step; + volatile uint8_t cur_comm_step = MOTOR_COMMUTATION_STEPS[currentHall]; + motor->motor_status.cur_comm_step = cur_comm_step; + volatile int8_t step_change = (cur_comm_step - motor->motor_status.prev_comm_step); - switch(step_change1) + switch(step_change) { - case -1: case 5: - motor->motor_status.Num_Steps = motor->motor_status.Num_Steps+1; - motor->motor_status.actualDirection = CW; + case 1: case -5: + motor->motor_status.Num_Steps += 1; + motor->motor_status.actualDirection = CW; //Motor1.motor_setpoints.directionOffset = DIRECTION_CW_OFFSET; break; - case 1: case -5: - motor->motor_status.Num_Steps = motor->motor_status.Num_Steps-1; - motor->motor_status.actualDirection = CCW; + case -1: case 5: + motor->motor_status.Num_Steps -= 1; + motor->motor_status.actualDirection = CCW; //Motor1.motor_setpoints.directionOffset = DIRECTION_CCW_OFFSET; break; default: // do nothing break; } - motor->motor_status.prev_comm_step = motor->motor_status.cur_comm_step; + motor->motor_status.prev_comm_step = cur_comm_step; //toc_port(DEBUG_2_PORT); } @@ -238,7 +236,7 @@ void exec_commutation(BLDCMotor_t *motor) // CW -> Always positive current // CCW -> Always negative current //// ------------------------------------------------------------------------------ -void select_active_phase(BLDCMotor_t *Motor) +void select_active_phase(BLDCMotor_t* const Motor) { uint8_t hall_state = Motor->motor_status.currentHallPattern; volatile float32_t phase_current = 0; @@ -263,24 +261,27 @@ void select_active_phase(BLDCMotor_t *Motor) Motor->Iphase_pu.Bus = phase_current; } -void calculate_motor_speed(BLDCMotor_t *motor) +void calculate_motor_speed(BLDCMotor_t* const motor) { //tic_port(DEBUG_2_PORT); volatile uint32_t temp_rpm = 0; - hri_tccount32_read_CC_reg(TC0, 0); /* Read CC0 but throw away)*/ - volatile uint32_t period_after_capture = hri_tccount32_read_CC_reg(TC0, 1); - if((period_after_capture > UINT32_MAX)|| (period_after_capture > 10712046)) { - Motor1.motor_status.calc_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)) { + motor->motor_status.calc_rpm = 0; } else { - //uint32_t test = (SPEEDFACTOR, period_after_capture); - //temp_rpm = SPEEDFACTOR / period_after_capture; - //tic_port(DEBUG_3_PORT); - temp_rpm = HZ_TO_RPM / (period_after_capture*motor->motor_param->motor_commutationStates); + ////uint32_t test = (SPEEDFACTOR, period_after_capture); + ////temp_rpm = SPEEDFACTOR / period_after_capture; + ////tic_port(DEBUG_3_PORT); + temp_rpm = HZ_TO_RPM / (period_after_capture * 42); //temp_rpm = HZ_TO_RPM * period_after_capture; - //toc_port(DEBUG_3_PORT); + ////toc_port(DEBUG_3_PORT); + + //uint32_t muti = DEVICE_SPEEDTC_FREQUENCY_Hz*60; + //temp_rpm = (muti) / (period_after_capture*42); } - if(Motor1.motor_status.actualDirection == CCW) /* Changed from CCW */ + if(motor->motor_status.actualDirection == CCW) /* Changed from CCW */ { //*motor_speed = -1* Motor1.calc_rpm; temp_rpm = -1 * temp_rpm; @@ -289,33 +290,37 @@ void calculate_motor_speed(BLDCMotor_t *motor) temp_rpm = temp_rpm; } - Motor1.motor_status.calc_rpm = (int16_t)temp_rpm; + //motor->motor_status.calc_rpm = (int16_t)temp_rpm; //toc_port(DEBUG_2_PORT); //#ifdef AVERAGE_SPEED_MEASURE + //if(temp_rpm==0) + //#define n_SAMPLE 8 + //static uint32_t speed_average = 0; + //static uint8_t count = 1; //// To avoid noise an average is realized on 8 samples //speed_average += temp_rpm; //if(count >= n_SAMPLE) //{ //count = 1; - //Motor1.motor_status.calc_rpm = (speed_average >> 3); // divide by 32 - ////Motor1.motor_status.calc_rpm = (speed_average); // divide by 32 + //motor->motor_status.calc_rpm = (speed_average >> 3); // divide by 8 + ////Motor1.motor_status.calc_rpm = (speed_average); // divide by 8 //speed_average = 0; ////*Spare_byte1 = motorState.actualDirection; - //if(Motor1.motor_status.actualDirection == CCW) /* Changed from CCW */ + //if(motor->motor_status.actualDirection == CCW) /* Changed from CCW */ //{ ////*motor_speed = -1* Motor1.calc_rpm; - //Motor1.motor_status.calc_rpm = -1* Motor1.motor_status.calc_rpm; + //motor->motor_status.calc_rpm = -1* motor->motor_status.calc_rpm; //} else { ////*motor_speed = Motor1.calc_rpm; - //Motor1.motor_status.calc_rpm = Motor1.motor_status.calc_rpm; + //motor->motor_status.calc_rpm = motor->motor_status.calc_rpm; //} //return; //} //else count++; - //#else - //Motor1.motor_status.calc_rpm = (int16_t)temp_rpm; //#endif + + motor->motor_status.calc_rpm = (int16_t)temp_rpm; //toc_port(DEBUG_2_PORT); } @@ -418,36 +423,18 @@ volatile uint8_t readHallSensorM1(void) { volatile uint8_t motor_read = 0; - motor_read = (motor_read & M1_HALL_A_MASK) | (uint8_t)((PORT->Group[M1_HALL_A_GROUP].IN.reg & M1_HALL_A_PORT)>>(M1_HALL_A_LSR)); - motor_read = (motor_read & M1_HALL_B_MASK) | (uint8_t)((PORT->Group[M1_HALL_B_GROUP].IN.reg & M1_HALL_B_PORT)>>(M1_HALL_B_LSR)); - motor_read = (motor_read & M1_HALL_C_MASK) | (uint8_t)((PORT->Group[M1_HALL_C_GROUP].IN.reg & M1_HALL_C_PORT)>>(M1_HALL_C_LSR)); - - //volatile uint8_t a = gpio_get_pin_level(M1_HALL_A_PIN); - //volatile uint8_t b = gpio_get_pin_level(M1_HALL_B_PIN); - //volatile uint8_t c = gpio_get_pin_level(M1_HALL_C_PIN); - //volatile uint8_t motor_read2 = ((a << 2)|(b << 1)|(c << 0)); - //return ((a << 2) | - //(b << 1) | - //(c << 0)); + motor_read = (motor_read & M1_HALL_A_MASK) | (uint8_t)((PORT->Group[M1_HALL_A_GROUP].IN.reg & M1_HALL_A_PORT)>>(M1_HALL_A_LSR)); + motor_read = (motor_read & M1_HALL_B_MASK) | (uint8_t)((PORT->Group[M1_HALL_B_GROUP].IN.reg & M1_HALL_B_PORT)>>(M1_HALL_B_LSR)); + motor_read = (motor_read & M1_HALL_C_MASK) | (uint8_t)((PORT->Group[M1_HALL_C_GROUP].IN.reg & M1_HALL_C_PORT)>>(M1_HALL_C_LSR)); return motor_read; -} - -volatile uint8_t readHallSensorM1v2(void) -{ - volatile uint8_t motor_read = 0; - motor_read = (motor_read & M1_HALL_A_MASK) | (uint8_t)((PORT->Group[M1_HALL_A_GROUP].IN.reg & M1_HALL_A_PORT)>>(M1_HALL_A_LSR)); - motor_read = (motor_read & M1_HALL_B_MASK) | (uint8_t)((PORT->Group[M1_HALL_B_GROUP].IN.reg & M1_HALL_B_PORT)>>(M1_HALL_B_LSR)); - motor_read = (motor_read & M1_HALL_C_MASK) | (uint8_t)((PORT->Group[M1_HALL_C_GROUP].IN.reg & M1_HALL_C_PORT)>>(M1_HALL_C_LSR)); - //volatile uint8_t a = gpio_get_pin_level(M1_HALL_A_PIN); //volatile uint8_t b = gpio_get_pin_level(M1_HALL_B_PIN); //volatile uint8_t c = gpio_get_pin_level(M1_HALL_C_PIN); - //volatile uint8_t motor_read2 = ((a << 2)|(b << 1)|(c << 0)); //return ((a << 2) | - //(b << 1) | - //(c << 0)); - return motor_read; + //(b << 1) | + //(c << 0)); + } volatile uint8_t readHallSensorM2(void) @@ -458,14 +445,12 @@ 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)); - if(((motor_read == INVALID_HALL_0) || (motor_read == INVALID_HALL_7))) { - Motor2.motor_state.fault = MOTOR_HALLSENSORINVALID; - Motor2.motor_state.currentstate = MOTOR_FAULT; - //applicationStatus.currentstate = APP_FAULT; - } - - - + //if(((motor_read == INVALID_HALL_0) || (motor_read == INVALID_HALL_7))) { + //Motor2.motor_state.fault = MOTOR_HALLSENSORINVALID; + //Motor2.motor_state.currentstate = MOTOR_FAULT; + ////applicationStatus.currentstate = APP_FAULT; + //} + //volatile uint8_t a = gpio_get_pin_level(M2_HALL_A_PIN); //volatile uint8_t b = gpio_get_pin_level(M2_HALL_B_PIN); //volatile uint8_t c = gpio_get_pin_level(M2_HALL_C_PIN); @@ -493,7 +478,10 @@ void read_zero_current_offset_value(BLDCMotor_t *motor1, BLDCMotor_t *motor2) adc_sync_enable_channel(&ADC_1, 9); //adc_sync_enable_channel(&ADC_1, 0); - ADC1->INPUTCTRL.reg = 0x1809; + /* Single ended */ + //ADC1->INPUTCTRL.reg = 0x1809; + /* Differential */ + ADC1->INPUTCTRL.reg = 0x0089; while (ADC1->STATUS.bit.ADCBUSY) {}; /* Wait for bus synchronization. */ for (i=0; iVoffset_lsb.A = phase_A_zero_current_offset_temp/samples; - ADC1->INPUTCTRL.reg = 0x1808; + /* Single ended */ + //ADC1->INPUTCTRL.reg = 0x1808; + /* Differential */ + ADC1->INPUTCTRL.reg = 0x0088; while (ADC1->STATUS.bit.ADCBUSY) {}; /* Wait for bus synchronization. */ for (i=0; iINPUTCTRL.reg = 0x1807; + + /* Single ended */ + //ADC1->INPUTCTRL.reg = 0x1807; + /* Differential */ + ADC1->INPUTCTRL.reg = 0x0087; while (ADC1->STATUS.bit.ADCBUSY) {}; /* Wait for bus synchronization. */ for (i=0; iVoffset_lsb.A = phase_A_zero_current_offset_temp/samples; - ADC1->INPUTCTRL.reg = 0x1806; + /* Single ended */ + //ADC1->INPUTCTRL.reg = 0x1806; + /* Differential */ + ADC1->INPUTCTRL.reg = 0x0086; while (ADC1->STATUS.bit.ADCBUSY) {}; /* Wait for bus synchronization. */ for (i=0; iSatFlag = false; } -inline void PID_objectInit(volatile PID_t *pPiD_obj) +static inline void PID_objectInit(volatile PID_t *pPiD_obj) { PID_t *obj = (PID_t *)pPiD_obj; diff --git a/2_Motor_Master/Motor_Master/Motor_Master/driver_init.c b/2_Motor_Master/Motor_Master/Motor_Master/driver_init.c index 2e6da97..1c22140 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/driver_init.c +++ b/2_Motor_Master/Motor_Master/Motor_Master/driver_init.c @@ -178,6 +178,8 @@ void EXTERNAL_IRQ_0_init(void) void EVENT_SYSTEM_0_init(void) { hri_gclk_write_PCHCTRL_reg(GCLK, EVSYS_GCLK_ID_0, CONF_GCLK_EVSYS_CHANNEL_0_SRC | (1 << GCLK_PCHCTRL_CHEN_Pos)); + hri_gclk_write_PCHCTRL_reg(GCLK, EVSYS_GCLK_ID_1, CONF_GCLK_EVSYS_CHANNEL_1_SRC | (1 << GCLK_PCHCTRL_CHEN_Pos)); + hri_gclk_write_PCHCTRL_reg(GCLK, EVSYS_GCLK_ID_2, CONF_GCLK_EVSYS_CHANNEL_2_SRC | (1 << GCLK_PCHCTRL_CHEN_Pos)); hri_mclk_set_APBBMASK_EVSYS_bit(MCLK); diff --git a/2_Motor_Master/Motor_Master/Motor_Master/main.c b/2_Motor_Master/Motor_Master/Motor_Master/main.c index 7a4e768..3bf90e9 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/main.c +++ b/2_Motor_Master/Motor_Master/Motor_Master/main.c @@ -21,8 +21,8 @@ void process_currents() volatile int16_t phase_A_current_raw, phase_B_current_raw; /* Motor 1 */ - phase_A_current_raw = (adc_res[3] - Motor1.Voffset_lsb.A); - phase_B_current_raw = (adc_res[2] - Motor1.Voffset_lsb.B)*-1; + phase_A_current_raw = (adc_res[0] - Motor1.Voffset_lsb.A); + phase_B_current_raw = (adc_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; @@ -30,8 +30,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[1] - Motor2.Voffset_lsb.A); - phase_B_current_raw = (adc_res[0] - Motor2.Voffset_lsb.B)*-1; + phase_A_current_raw = (adc_res[2] - Motor2.Voffset_lsb.A); + phase_B_current_raw = (adc_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; @@ -69,6 +69,14 @@ void One_ms_timer_init(void) timer_start(&TIMER_0); } +//void speed_timer_init(void) +//{ + //hri_tc_write_WAVE_reg(TC_SPEED_M1.device.hw, 0x00); + //hri_tc_set_CTRLA_CAPTEN0_bit(TC_SPEED_M1.device.hw); + //hri_tc_set_CTRLA_CAPTEN1_bit(TC_SPEED_M1.device.hw); + //timer_start(&TC_SPEED_M1); +//} + void enable_NVIC_IRQ(void) { //NVIC_EnableIRQ(TC7_IRQn); // TC7: TC_ECAT @@ -79,7 +87,7 @@ void enable_NVIC_IRQ(void) //NVIC_EnableIRQ(EIC_5_IRQn); } -inline void APPLICATION_StateMachine(void) +void APPLICATION_StateMachine(void) { Motor1.timerflags.current_loop_tic = false; @@ -110,8 +118,6 @@ inline void APPLICATION_StateMachine(void) //applicationStatus.currentstate ; motor_StateMachine(&Motor1); motor_StateMachine(&Motor2); - exec_commutation(&Motor1); - exec_commutation(&Motor2); break; case APP_FAULT: //DisableGateDrivers(&Motor1); @@ -141,19 +147,37 @@ int main(void) /* Initializes MCU, drivers and middleware */ atmel_start_init(); //initMotorParameters(); + //configure_tcc_pwm(); + //BldcInitStruct(&Motor1, &FH_22mm24BXTR); + //BldcInitStruct(&Motor2, &FH_32mm24BXTR); + //Motor1.readHall = &readHallSensorM1; + //Motor2.readHall = &readHallSensorM2; + //read_zero_current_offset_value(&Motor1, &Motor2); + //adc_sync_enable_channel(&ADC_1, 6); + //adc_init_dma(); + //config_qspi(); + //custom_logic_enable(); + //enable_NVIC_IRQ(); + //ECAT_STATE_MACHINE(); + //One_ms_timer_init(); BldcInitStruct(&Motor1, &FH_22mm24BXTR); BldcInitStruct(&Motor2, &FH_32mm24BXTR); Motor1.readHall = &readHallSensorM1; Motor2.readHall = &readHallSensorM2; read_zero_current_offset_value(&Motor1, &Motor2); config_qspi(); - One_ms_timer_init(); configure_tcc_pwm(); adc_sync_enable_channel(&ADC_1, 6); adc_init_dma(); ECAT_STATE_MACHINE(); + One_ms_timer_init(); + custom_logic_enable(); + //speed_timer_init(); enable_NVIC_IRQ(); + + + /* Replace with your application code */ while (1) { //delay_ms(100); @@ -164,8 +188,9 @@ int main(void) //update_setpoints(); if (Motor1.timerflags.adc_readings_ready_tic) {process_currents();} if (Motor1.timerflags.current_loop_tic) {APPLICATION_StateMachine();} - if (run_ECAT) {ECAT_STATE_MACHINE();} - + 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 fe9aaf3..a688f0d 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/motorparameters.h +++ b/2_Motor_Master/Motor_Master/Motor_Master/motorparameters.h @@ -131,24 +131,27 @@ //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) + 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 + 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 */ @@ -177,6 +180,7 @@ 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; @@ -196,6 +200,7 @@ 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 @@ -212,6 +217,7 @@ const static BLDCMotor_param_t FH_22mm24BXTR = { /* 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 diff --git a/BLDC_E54v2/BLCD_E54v2/BLCD_E54v2/bldc.h b/BLDC_E54v2/BLCD_E54v2/BLCD_E54v2/bldc.h index cd6f548..fde6aab 100644 --- a/BLDC_E54v2/BLCD_E54v2/BLCD_E54v2/bldc.h +++ b/BLDC_E54v2/BLCD_E54v2/BLCD_E54v2/bldc.h @@ -150,7 +150,7 @@ volatile typedef struct BLDCmotor // ---------------------------------------------------------------------- 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}; +static const uint8_t MOTOR_COMMUTATION_STEPS[8] = {20, 1, 3, 2, 5, 6, 4, 20}; volatile BLDCMotor_t Motor1; volatile BLDCMotor_t Motor2; diff --git a/Twincat/MotorData/.vs/MotorData/v15/.suo b/Twincat/MotorData/.vs/MotorData/v15/.suo new file mode 100644 index 0000000000000000000000000000000000000000..c74cc05bdc5b1262b244b7054749f3ae297d6677 GIT binary patch literal 36864 zcmeHQYm8*aRlc+9_4*Y*NHE|e&e*Y&jmJIHkJ~TDcHH-N_w;-EJrA$h^!qXWnCa(C z?<@(BNC*i?q=<+}K|)IY@BM@4L6}YYS=Nb?&J;r|!J-qo>~egYP`^&%#J}UAQd#^u33L2M5xZ5Wl3| zKPd=5-@pIqd+)u+;rU-}U8eepRpa~K%wLRs(vdmb^Da4?~k;m1#dBL};C`02@aUq&+6R|y(^;@uM} zpr9e-05SAM3XtpPJMHp_kfAG{pUM;(r)% zjfM%?mB%;ntIg|25&wx+c;Nap(zRR8Z-$Tc(;mn52>|=>DZp!h4+A~|umVWWGXR$R zC0suW_(cHugLs+#aa>vFoeVbMumfHJd=~I3fVf}B^>ctX01kizkO5AB3*ZJk3%HZC z-79rp&iT*yBe*ipC;HciaGf;&DcTSnWn8ZUh~rL$#r&V#v%iWl__h8xa4a;KTyl})gU|yKG@iP!D^#KFG;p0(?*Ypw z;bYK0IHx(6*&k}^_}?BLU+ZUy18);}1JHGv(1c<_QPII#LPpVyn&YbysQ*wVQQw#r zKQ&yIVTc1y3_5R92!fKDU{I5%{{0B>6YmmAOpBiyF7+RVFm%6+kN|#aotie9Lt0DC zFyZ&vK7Q&~)8fw~udlCvI?>zg=_>G3|KC)!&@58B$5c5LO*nNTC9BZCsn2sQI4yq4 z7_G7xFjbV1E+JNf9>(81s3;({p=Xyf5{umC<_60AecvbN78z^fR7v0skv_iJRe+ zY(vSz{hz`3t9;i11zCj;S1~7s$7K~Wv<^zM3Vm&*hgj+BX#exLz6h8W|F0vjHveM4 zJ}rLsQw!QUxuS+rMbz`g9PR%S%5jat@Fq0=EZW^d>l(sa!ZO;a;^B%|eZn-(60QeF zdC@ip{=xoNsW{uOGVuMMS!;4)ywjLz1^iJ0Z7Qu~-{#QUlf&{H?WerpT9IKCKSwtQ z8g{W7ucNosnW*MH>GvA=k5gG&ubq@v2b!+zo@e(nQgK*8SeSdZ_8Poe#6i?*H} z#dXp>d0*rIlklq&C<`jG%6`Z=dMvW{vYhMae1|8)E7^i|F`iULMKT>22`|SCtiVniR)FqQ%T2(nMM)^#_9bU7jpj1%Ky}r-|AN|s%NVDKkX2l`5P18!&raJjGuegn*14F zaW#+IPJ6^CZKQ^4gTa9J@6s|7w0*L{XQ#!vf7V}qjjZ_MXm?xDa*3bzkcP4{f4|g$YkW8_q3r*+ z0Wz#C*N=A$nuv2fVS+uVgKH1>qdN`Ev#zYXmW3$tP~T@-8uLC2nREldy#P%X+(1lS zm2jtMFdugdI8VBeWn4Xs=8WSBR}$(9?mAZcw0LrEX%g=Gyv;vj?+e1a|3010avFh@ zWdw$K<^uTq5#jUy+e`oWH(&PtMMC<|?asFzV?~6&b^nX+hd=m=_xIk=JzMMHuWiP2TQw1RMVCe%qU31# z2PbEFDGqD07aaN76GnQN8d1&aMAqpGg_$*s`i}uJJhWcZX^s+d1u>Y)8@C@J|B%|ex}Uc^@~$<2#pf>i=_PfB z;3E9jQbrMed~vhoCka1ps19zks(HBWw2TsWQuwt-gdZ0->Ue1^dwpOG$E6KV9kzUV zXey)H(b<|nCT8^Lh2cNl@%{MNa49{i67Z5SVos=oorno9Jo*#I%5VLZ=R5!Khd=u8 zw>Ke7VuCIIk+*;P_OpR+eKY$X*k%OqN4 zr%txYE}dw%>veX!sMi^t2A9QVcgjYi`PNae-l{j8saEQ`63ZfXBOH*2?^gFJnY~86 zd?deeJ<-fHn%4_CleLrUU3bo~wsFK_pOka&>kZ^Q#Q+B&WEBDTV zm?;&EUI{(NF>K(s4T-Jk#`N&unvq@{$kP(wG zOlJHs&4|1P#O)|&L0%E1WZ@Rx4p}hhFkV&bAH?cAcdWP9-A53qs(zr!j}6d6ap!oelI& z?z_Ob_Z@ZK3V?5_9CV5E;KJaAQ#l_ep8tHF)SI(%V ztpWEYq*Cg-AiErjBsM`hGfhTL5iz48t@Vok4fjJepdXMH*RI^#;2s0l`dXTH<@R5%l$=O`5h@+UB`G!F^iVNocE~&0-nw9N>9?Rv+z%p8|Xua1HPoz)OG? zz{>y~U=^?ipgLdx7y%{#d&CT|0O*HE>n&gXgZ_W7;r?~N=Kya28~_O*1DpUCzztXj zP+ocg%AE!OZ9RBl|L2VR8MOZ|eFDtGckZ_Fvk==nHq+n{Xca zPkBvUfnl`&=dk}C%l>J{f7t%Ju>bM=k*4oxw$B;te~KPU9eLXR$ClGQWB(iFKh@KK zes=0fCV_;z zDSNrYGjSgLOTf#u3_}P~ZXFm}(1iCe*VQv$UeHkM_drZ}%H1)Z&T3*MkQ+E7_sSgl zFY#*lgIGNzz(J$7=4I%CCFp^Zyzu7nzc~W_ud@>nRR3f4JomNO#@xsnK+SjBFa576 zPdKL2@_!0>^?m$F^k^JCO`VtCJ_qOtzK1m<$Z2@Bbn3sHPuTXI8vo~!SK~jv@9A&X zK+990;dZYNuY@$e1kE=XRVV@Dzw{hpF94=(|DgUqY84%v4`K926Jw%T|A#qW#ZUV` zc}bgtYU=o3()vAi8o~wbHs93ugO1mcAMM$#BmK+zBX_FO z?aOMb#{aU6Hvd}Py1F;sFF))2-!P|y%}a0p+bsA=orZrrrPDpt#(rA<8?2S{_Wf3z z!5rh`TR-BPZ~pKCc3*~1pfrVL?4Z!_V*n5EcU;iMe_Wn%&nO2eJC-29zK`pRG}D9| z^?a$4d-1%jG_>2uj`(sgmuj{fIof}gfvcR$v<%m`wwGg>`d&^;S#4#SygxXab@Ke> z4DI-!y#KjpI4%DV&j0)U{lAd^_qY6q>T|lE$-@8RG}n>efDEA+A>67Wd955;U7jL?w0?_Q^39O|5*5c%;&pvTYG%J|H7;m z_q*ktGM%+OfO&GheovD;7UuCnTKIpQ=1nszH`3;RvEzN4!s$4Y7U`StaX6d*hot!8 znP>FGJ5Q|RWXL>ofSwxX+b`oNoX`JBm30@@i?=yz7Eip3r*1mFS0(XzQg`Ph@kLTO zA1A7E?jq@&l^gFSh0}4QDv3o%QWyNtMEIY{u8 zYJm*5BQ7I_2P&dU3Di@NFfUX{e>*>`tN5?>^h z^Kqgo=Pr`YS-J6UQaBw)J_6m(0ZTS5raX5lz`YNvv1 zZpmuZ>)kqoWOD07m(ihe5OKH9A4!ry zXEvMdI#F`EbPl7#tqOCjZZ#PUx^yZd=5l$X#gaGQQnDMS&hA`v`$-e}@!`{m*M@JG-#k87xjJ;N@~y8f z-E(IszyF}-xiaF=qTFiFR@KN+DJgA4yL$h6q3%Ek#S%qVqJWshTSr88Z1bA8t=Wr= zIJQEusNU;nG{uZ1!h~ook}x2*d{FLoY}=6p6MY$1aVOJtIGoKOZmmv7CfnIxX8}(& zmWWD8XE9t(dJj^DP}Ch3V+W3GD3&p72J4ZUzg>-3w-Sd1ebCyj#=Y)RN$!L*fp(+0 zmDfuhDe98NwxcQqqmrv8AGFNDNXfG0OM0`Blq=|p9C)m?tx$WrzTa927);i($}GqnK2io2B$9(Ib3QrBjbgPm^N=HE2c#oTVq<1~13(Q?ul40g79 z>)ExsR1R!xSxQZ3qqmZ4X8f_0gd?8ZI0$BHC4JdzYF1l5TQ%borB1C9sG2?blw{ni zIUT0Twx@d-^ffEx%1U{y8nPOCh4zlW65Py5`fj{ziN9)xJdWNyPpK-ev+^AZ8?!8PWXew{7MAi#7*P7Ml$=a>% zq(87$wiGMf?V8D-z+qt9zF|`~dbh%+wb0H=CX;Izb8=Je7)`yys+{tyXUx8~p(n~> zr=D$e>h`r((VgF}xq9t`_HMxF_vHi0ZZg-Y6_Q~o6kT!aE7iK+u@l%g7>m(^e8{$u z$UAo}$#fU~ExAa^5X`py2b-0K`=FUBTdcWm$s02zHV@Vtt;EW9I+Tbs!aLD`dwVSu zmU^3#+m{fdzNjbN*bhWg8|9T~OxkMLvz;}ssg*6Zv+>=$eJ_{4FzJ4D-fBBg_xEC#ZM&hl?QOaoD;?3Wn`o|!d%M2qPTo>DEc$Jt)LwC0r1pkW3QCsrUUw^M z%XRn>{Yz`zKYj(5KLw&rRYYv zEgd$SUW;EcZf@kQZn5RtH(Peg5nnXC-z#-Iva98BMZ|PnTGxy2ioDX1!;Pe53Yjz8 zmKD)&*mD?Neyi=E+1gFZhNf-BSr3aoUv%5zaYWaeo5sA)mZ`au2iamh4-=(QZ`;*L?L}nC81=91bo`dI-s_5QHWJO6sa+{G zqK2Z|Vrt_RZIYacd@o}Uq$ACW#k9FzOnb^1z0GHkDu%U=EZTzEo%T+z<4nm}n_D0D ziQDm=Y%4d9S=mtdFeOD3Q|5@zJ@ETWE-^4+YQaP_ey;{2jE zPY%2zTzcm6JC~Pq4}9awgTgniK8SwQdQL5=^{il2V@)F;|~T)|OXlw&0oi)f?IuXmv26K6+FWyA@PEirUyF;^P-w ztaz@&RsB;rpFP1gTTzZy4JZCsj}2D?avD&YWp4?OiL@Z(fnWWejP})R{&%AuQnhmo za{oEc7v}Kw_{Tv7ze&I^!#F{ojJOW8vu`EDM5Q0u5(#nkk2(f!w3#&Mk(Z-oR;YLa zwK1OwPu#e=@kFPzkNw8lh=+5SJ*?J7yC~b|z!Q!F+vEV;5}tf%a6eH!4aN3k2HLZZ z_V5(ZqrXgEi=ICAaDOcsVwCr3Y^f&)qBU2ur%F z$J6;Lrt=b$9y(5XNSWaeW0nK>=#k?`k7$pW{OED=qy1#1qR05`L;QLCIK$(E<(@cB zeqylPlPvNSaSnb@kCJ1$a9-yRZvO|1+0GA~&-sJd&wpsWGw=Bkt^8T93;&Okzl$>1 zFXQZ@^Vxb9{vZ5LAN-ghzx}CwCu%wy&%*!X + + + 424dd8020000000000003600000028000000100000000e0000000100180000000000a2020000120b0000120b000000000000000000001306e31306e3190ce42b1fe62b1fe61306e31f13e5190ce42519e51306e31306e3190ce42f24e7190ce41306e31306e31306e31306e35f56ec645ced645ced4137e91f13e5473de95f57ec3227e71306e3473de95a51ec271be61306e31306e31409ca524cc68e8ad74f48c1615cc82218d03e36bf716bce746fce453dc01307ce3931ba7d78d27671d1150cb21409ca1712801b1d1d1b1d1d1b1d1d1b1d1d120b891b1d1d1b1d1d1b1d1d1b1d1d120b891b1d1d1b1d1d1b1d1d1b1d1d1712801712807f8080d4d5d5d4d5d5383939120b89545656d4d5d5d4d5d5626464130c89292b2bd4d5d5d4d5d56264641915801712804647471b1d1daaaaaad4d5d5130e82383939292b2b717272d4d5d5151183d4d5d57f80801b1d1d7172721e1c81191580464747d4d5d5d4d5d51b1d1d19158a292b2bd4d5d5d4d5d5292b2b1b1b8ad4d5d56264641b1d1d1b1d1d2427821e1d81d4d5d54647476264643839391e208bd4d5d57f8080464747545656242a8bd4d5d59b9c9c292b2baaaaaa2d3683252882464747d4d5d5d4d5d51b1d1d272d85292b2bd4d5d5d4d5d5292b2b2e37861b1d1dd4d5d5d4d5d5464747394484323bb52324812122822426822526824554c0323883292b822a2d83353c84424cbf3238843940842e32834853865d6ebb5262eb3e43e83334e74147e94349e9535feb4d56ea5662eb484dea545deb636fed545aea5a63ec6671ed8ca0f290a5f2748aef6b7bee5d68ec6874ed788aef8397f17684ef7986ef8c9ff2818ff1818ef08e9df18a97f18791f19ba9f3b0c0f691a4f291a2f28390f192a1f29cacf3a3b3f498a6f3a4b3f4aebdf5b0bef59ea8f3a3adf4bbc7f7c4d1f8cad7f8ced9f9b4c4f6b8c8f6acb8f59aa3f3b6c1f6c5d2f8c2cdf8ccd7f9d2ddf9d5e0fad2daf9d5dcf9dfe7fbe2e9fbe5ebfbe8eefb0000 + + + + + + PlcTask + + + + + + + Motordata_PLC Instance + {08500001-0000-0000-F000-000000000064} + + PlcTask Inputs + + GVL_motor_data.Status + + BYTE + + + GVL_motor_data.Mode + BYTE + + + GVL_motor_data.Com_State + BYTE + + + GVL_motor_data.act_pos + INT + + + GVL_motor_data.PH_A + INT + + + GVL_motor_data.PH_B + INT + + + GVL_motor_data.PH_C + INT + + + GVL_motor_data.Error + INT + + + GVL_motor_data.Phase_Current + INT + + + GVL_motor_data.Speed + INT + + + GVL_motor_data.Duty + INT + + + + PlcTask Outputs + + GVL_motor_data.op_Mode + + BYTE + + + GVL_motor_data.des_pos + INT + + + GVL_motor_data.des_max_current + INT + + + GVL_motor_data.des_current + INT + + + GVL_motor_data.des_speed + INT + + + GVL_motor_data.I_kp + INT + + + GVL_motor_data.I_ki + INT + + + GVL_motor_data.V_kp + INT + + + GVL_motor_data.V_kd + INT + + + + + 0 + PlcTask + + #x02010030 + + 20 + 10000000 + + + + + + + + + + + Device 2 (EtherCAT) + + + Ethernet (TwinCAT-Intel PCI Ethernet Adapter (Gigab + \DEVICE\{B319FB6B-ED15-445B-9318-290BECEEFA76} + 4c5262a1d9ac + + + + Image + + + Box 1 (LAN9252-2_motor_Master) + 1000 + + 001140000400010003000000000000000000001104010000 + 001878000000010004000000000000000000001800010000 + 0000000000000000001100020100000001000000000000000000000000000000 + 0000000000000000001800010100000002000000000000000000000000000000 + + + BYTE + + + BYTE + + + INT + + + INT + + + INT + + + INT + + + INT + + + INT + + + INT + + + INT + + + INT + + + BYTE + + + BYTE + + + INT + + + INT + + + INT + + + INT + + + INT + + + INT + + + INT + + + INT + + + INT + + + INT + + + INT + + + INT + + + INT + + + INT + + + INT + + + INT + + + INT + + + BYTE + + + BYTE + + + INT + + + INT + + + INT + + + INT + + + INT + + + INT + + + INT + + + INT + + + INT + + + BYTE + + + BYTE + + + INT + + + INT + + + INT + + + INT + + + INT + + + INT + + + INT + + + INT + + + INT + + + INT + + + INT + + + INT + + + INT + + + INT + + + INT + + + INT + + + INT + + + INT + + + INT + + + INT + + + INT + + + + + BYTE + + + BYTE + + + INT + + + INT + + + INT + + + INT + + + INT + + + INT + + + INT + + + BYTE + + + BYTE + + + INT + + + INT + + + INT + + + INT + + + INT + + + INT + + + INT + + + BYTE + + + BYTE + + + INT + + + INT + + + INT + + + INT + + + INT + + + INT + + + INT + + + BYTE + + + BYTE + + + INT + + + INT + + + INT + + + INT + + + INT + + + INT + + + INT + + + + + + + + + diff --git a/Twincat/MotorData/MotorData/Motordata_PLC/GVLs/GVL_motor_data.TcGVL b/Twincat/MotorData/MotorData/Motordata_PLC/GVLs/GVL_motor_data.TcGVL new file mode 100644 index 0000000..ec6f260 --- /dev/null +++ b/Twincat/MotorData/MotorData/Motordata_PLC/GVLs/GVL_motor_data.TcGVL @@ -0,0 +1,31 @@ + + + + + + \ No newline at end of file diff --git a/Twincat/MotorData/MotorData/Motordata_PLC/Motordata_PLC.plcproj b/Twincat/MotorData/MotorData/Motordata_PLC/Motordata_PLC.plcproj new file mode 100644 index 0000000..32ccfde --- /dev/null +++ b/Twincat/MotorData/MotorData/Motordata_PLC/Motordata_PLC.plcproj @@ -0,0 +1,74 @@ + + + 1.0.0.0 + 2.0 + {4ae346cd-c7ce-4990-9940-2d1e689dc225} + True + true + true + false + Motordata_PLC + 3.1.4023.0 + {8cb30f18-2c9e-4d81-a3ca-322069701ffa} + {491a06a0-e15f-449f-8b38-ee9ac154eb22} + {53ab81c7-8b4b-43c1-b5ab-73cba09cb336} + {81fa6db4-0105-41b4-88c2-51344db29290} + {83c85c54-db23-4354-9cba-3d70fd632e8d} + {7999646c-51c9-45ae-9085-f34e5ecc5ec7} + + + + Code + true + + + Code + + + Code + + + + + + + + + + + Tc2_Standard, * (Beckhoff Automation GmbH) + Tc2_Standard + + + Tc2_System, * (Beckhoff Automation GmbH) + Tc2_System + + + Tc3_Module, * (Beckhoff Automation GmbH) + Tc3_Module + + + + + Content + + + + + + + + "<ProjectRoot>" + + + + + + System.Collections.Hashtable + {54dd0eac-a6d8-46f2-8c27-2f43c7e49861} + System.String + + + + + \ No newline at end of file diff --git a/Twincat/MotorData/MotorData/Motordata_PLC/POUs/MAIN.TcPOU b/Twincat/MotorData/MotorData/Motordata_PLC/POUs/MAIN.TcPOU new file mode 100644 index 0000000..9e1ec61 --- /dev/null +++ b/Twincat/MotorData/MotorData/Motordata_PLC/POUs/MAIN.TcPOU @@ -0,0 +1,13 @@ + + + + + + + + + + \ No newline at end of file diff --git a/Twincat/MotorData/MotorData/Motordata_PLC/PlcTask.TcTTO b/Twincat/MotorData/MotorData/Motordata_PLC/PlcTask.TcTTO new file mode 100644 index 0000000..dc67fd4 --- /dev/null +++ b/Twincat/MotorData/MotorData/Motordata_PLC/PlcTask.TcTTO @@ -0,0 +1,17 @@ + + + + + 10000 + 20 + + MAIN + + {e97dcd9b-40b9-4d2d-be20-7f79184b4b83} + {3b074a47-04a4-4446-8c36-6845d19068a7} + {5c5f5f84-e7f6-4c9f-8e6b-25d1b89465a8} + {07b2fc7d-725f-4928-b7bb-1c1ddcb9a703} + {49bd0358-5e57-44dd-9076-193288aeb9f7} + + + \ No newline at end of file diff --git a/Twincat/MotorData/TwinCAT Measurement Project1/TwinCAT Measurement Project1.tcmproj b/Twincat/MotorData/TwinCAT Measurement Project1/TwinCAT Measurement Project1.tcmproj new file mode 100644 index 0000000..ace6e19 --- /dev/null +++ b/Twincat/MotorData/TwinCAT Measurement Project1/TwinCAT Measurement Project1.tcmproj @@ -0,0 +1,14 @@ + + + + {8fb7700f-1b3f-4e2a-b3df-c74f2d60ba45} + TwinCAT Measurement Project1 + TwinCAT Measurement Project1 + TwinCAT Measurement Project1 + + + + Content + + + \ No newline at end of file diff --git a/Twincat/MotorData/TwinCAT Measurement Project1/XY Scope Project.tcscopex b/Twincat/MotorData/TwinCAT Measurement Project1/XY Scope Project.tcscopex new file mode 100644 index 0000000..6344a31 --- /dev/null +++ b/Twincat/MotorData/TwinCAT Measurement Project1/XY Scope Project.tcscopex @@ -0,0 +1,248 @@ + + + 0 + Disabled + 0 + false + {SCOPE}_AutoSave_{HH_mm_ss} + $ScopeProject$\AutoSave + + Black + 0f4c9db4-7dc8-4a6e-bab0-d9526c2fdc12 + + 00000000-0000-0000-0000-000000000000 + true + false + + 127.0.0.1.1.1 + Scope Project + 6000000000 + 100 + AutoStop + + + + Black + 4d0f043f-99fd-48e3-88a4-eaed2ee9275c + false + DataPool + 0 + + .svdp + DataPool_34 + + + 0 + true + + 100000000 + -986896 + -16777216 + d0b9ac70-9337-4fea-96b3-e8ac396fe683 + false + 00000000-0000-0000-0000-000000000000 + 120000 + XY Chart + false + 10 + + + + -1 + true + 41ce791f-bcf1-4d09-9cb2-ec79e2806d83 + false + Axis Group + 10 + + + + Black + aa35b9c6-4a80-424a-bd18-4077de755c99 + false + false + false + 0.5 + -0.5 + Value Axis + X + AutoGrowOnly + 100 + + + + -16777216 + true + -16777216 + 10 + 1 + c942fb84-ebbf-478f-9b4d-a361581ae2f5 + false + 1 + Axis Style + 6 + false + 100 + false + 5 + .svstyle + AxisStyle_38 + true + + + .svaxis + Value Axis + + + + Black + 6dcf7932-cbd2-4ea2-91b4-ffb32c161f58 + false + false + false + 0.5 + -0.5 + Value Axis + Y + AutoGrowOnly + 100 + + + + -16777216 + true + -16777216 + 10 + 1 + bc0d4a81-c886-4ac1-a954-f68cfcc15f22 + false + 1 + Axis Style + 6 + false + 100 + false + 5 + .svstyle + AxisStyle_40 + true + + + .svaxis + Value Axis + + + 1:1 + + Black + f3f6f2ea-0c83-4c7b-8976-5c0ad3437a1a + false + false + AspectRatioScalingStyle_41 + X + 100 + .svstyle + AspectRatioScalingStyle_41 + + + + Black + a1bafadb-b657-4f03-8d16-7da5ba9e80de + false + Marker Container + 100 + + .svmc + MarkerContainer_42 + + + .svagroup + AxisGroup_36 + + + + -1 + -16777216 + cd2eca7a-022e-4ee1-9aac-22d1443cc230 + false + Overview Chart + false + 100 + .svochart + OverviewChart_43 + + + + Black + 1 + a3c60b44-98d1-4dc6-96dd-28ec04b43949 + false + Chart Style + false + 100 + false + + + + Black + 1ec177ce-162d-4027-80af-1bea9563a820 + false + Chartzoom Style + true + 100 + .svstyle + ChartZoomStyle_45 + true + + + + 16 + 16 + + + Black + RunButton PauseButton Splitter1 DisplaywidthTextBox Splitter2 ScrollBackBig ScrollBackSmall ScrollForwardSmall ScrollForwardBig Splitter3 PositionTextBox Splitter4 UndoButton RedoButton Splitter5 PanXYButton ZoomXYButton UnzoomButton ZoomOutMaxButton OverviewButton CopyToClipboard + All + 7fea54c9-d498-4be2-b2aa-5e6dfc5c5525 + false + Chartmenu Style + 100 + .svstyle + 8.25 + ChartMenuStyle_46 + 8.25 + true + true + true + + + .svstyle + ChartStyle_44 + true + + + .svchart + XYChart_35 + + + + Black + c29c24e8-ee01-443d-9a31-927d4d04d726 + false + Trigger + 100 + + .svtm + TriggerModule_47 + + + .tcscopex + Default + + ScopeProject_3 + false + true + true + 1.0.0.0 + ExtendedXYOnly + \ No newline at end of file