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 ce8529c..265b758 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 @@ -839,8 +839,8 @@ drivers: eic_arch_asynch11: false eic_arch_asynch12: false eic_arch_asynch13: false - eic_arch_asynch14: false - eic_arch_asynch15: false + eic_arch_asynch14: true + eic_arch_asynch15: true eic_arch_asynch2: false eic_arch_asynch3: false eic_arch_asynch4: false @@ -872,8 +872,8 @@ drivers: eic_arch_enable_irq_setting11: false eic_arch_enable_irq_setting12: false eic_arch_enable_irq_setting13: false - eic_arch_enable_irq_setting14: false - eic_arch_enable_irq_setting15: false + eic_arch_enable_irq_setting14: true + eic_arch_enable_irq_setting15: true eic_arch_enable_irq_setting2: false eic_arch_enable_irq_setting3: false eic_arch_enable_irq_setting4: false @@ -926,8 +926,8 @@ drivers: eic_arch_sense11: No detection eic_arch_sense12: No detection eic_arch_sense13: No detection - eic_arch_sense14: No detection - eic_arch_sense15: No detection + eic_arch_sense14: Both-edges detection + eic_arch_sense15: Both-edges detection eic_arch_sense2: No detection eic_arch_sense3: No detection eic_arch_sense4: No detection diff --git a/2_Motor_Master/Motor_Master/Motor_Master/Config/hpl_eic_config.h b/2_Motor_Master/Motor_Master/Motor_Master/Config/hpl_eic_config.h index d10ee51..059b167 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/Config/hpl_eic_config.h +++ b/2_Motor_Master/Motor_Master/Motor_Master/Config/hpl_eic_config.h @@ -749,7 +749,7 @@ // Interrupt 14 Settings // eic_arch_enable_irq_setting14 #ifndef CONF_EIC_ENABLE_IRQ_SETTING14 -#define CONF_EIC_ENABLE_IRQ_SETTING14 0 +#define CONF_EIC_ENABLE_IRQ_SETTING14 1 #endif // External Interrupt 14 Filter Enable @@ -783,14 +783,14 @@ // This defines input sense trigger // eic_arch_sense14 #ifndef CONF_EIC_SENSE14 -#define CONF_EIC_SENSE14 EIC_NMICTRL_NMISENSE_NONE_Val +#define CONF_EIC_SENSE14 EIC_NMICTRL_NMISENSE_BOTH_Val #endif // External Interrupt 14 Asynchronous Edge Detection Mode // Indicates the external interrupt 14 detection mode operated synchronously or asynchronousl // eic_arch_asynch14 #ifndef CONF_EIC_ASYNCH14 -#define CONF_EIC_ASYNCH14 0 +#define CONF_EIC_ASYNCH14 1 #endif // @@ -798,7 +798,7 @@ // Interrupt 15 Settings // eic_arch_enable_irq_setting15 #ifndef CONF_EIC_ENABLE_IRQ_SETTING15 -#define CONF_EIC_ENABLE_IRQ_SETTING15 0 +#define CONF_EIC_ENABLE_IRQ_SETTING15 1 #endif // External Interrupt 15 Filter Enable @@ -832,14 +832,14 @@ // This defines input sense trigger // eic_arch_sense15 #ifndef CONF_EIC_SENSE15 -#define CONF_EIC_SENSE15 EIC_NMICTRL_NMISENSE_NONE_Val +#define CONF_EIC_SENSE15 EIC_NMICTRL_NMISENSE_BOTH_Val #endif // External Interrupt 15 Asynchronous Edge Detection Mode // Indicates the external interrupt 15 detection mode operated synchronously or asynchronousl // eic_arch_asynch15 #ifndef CONF_EIC_ASYNCH15 -#define CONF_EIC_ASYNCH15 0 +#define CONF_EIC_ASYNCH15 1 #endif // 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 7629170..ca22b89 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/EtherCAT_SlaveDef.h +++ b/2_Motor_Master/Motor_Master/Motor_Master/EtherCAT_SlaveDef.h @@ -167,7 +167,20 @@ static void update_telemetry(void) static void update_setpoints(void) { - //Motor1.motor_setpoints. = *desired_position; + Motor1.motor_setpoints.desired_position = *M1_Desired_pos; + Motor1.motor_setpoints.desired_speed = *M1_Desired_speed; + Motor1.motor_setpoints.desired_torque = *M1_Desired_current; + Motor1.motor_setpoints.max_current = *M1_Max_current; + Motor1.motor_setpoints.max_torque = *M1_Max_current; + Motor1.motor_setpoints.max_velocity = *M1_Max_velocity; + + Motor2.motor_setpoints.desired_position = *M2_Desired_pos; + Motor2.motor_setpoints.desired_speed = *M2_Desired_speed; + Motor2.motor_setpoints.desired_torque = *M2_Desired_current; + Motor2.motor_setpoints.max_current = *M2_Max_current; + Motor2.motor_setpoints.max_torque = *M2_Max_current; + Motor2.motor_setpoints.max_velocity = *M2_Max_velocity; + //volatile uint8_t a = *M1_Control_mode; //volatile uint8_t b = *M1_Control_set; //volatile int16_t c = *M1_Desired_pos; 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 7629170..ca22b89 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/Ethercat_SlaveDef.h +++ b/2_Motor_Master/Motor_Master/Motor_Master/Ethercat_SlaveDef.h @@ -167,7 +167,20 @@ static void update_telemetry(void) static void update_setpoints(void) { - //Motor1.motor_setpoints. = *desired_position; + Motor1.motor_setpoints.desired_position = *M1_Desired_pos; + Motor1.motor_setpoints.desired_speed = *M1_Desired_speed; + Motor1.motor_setpoints.desired_torque = *M1_Desired_current; + Motor1.motor_setpoints.max_current = *M1_Max_current; + Motor1.motor_setpoints.max_torque = *M1_Max_current; + Motor1.motor_setpoints.max_velocity = *M1_Max_velocity; + + Motor2.motor_setpoints.desired_position = *M2_Desired_pos; + Motor2.motor_setpoints.desired_speed = *M2_Desired_speed; + Motor2.motor_setpoints.desired_torque = *M2_Desired_current; + Motor2.motor_setpoints.max_current = *M2_Max_current; + Motor2.motor_setpoints.max_torque = *M2_Max_current; + Motor2.motor_setpoints.max_velocity = *M2_Max_velocity; + //volatile uint8_t a = *M1_Control_mode; //volatile uint8_t b = *M1_Control_set; //volatile int16_t c = *M1_Desired_pos; 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 eaf15a1..3096d08 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/Motor_Master.cproj +++ b/2_Motor_Master/Motor_Master/Motor_Master/Motor_Master.cproj @@ -211,7 +211,7 @@ - + @@ -387,7 +387,6 @@ %24(PackRepoDir)\arm\CMSIS\5.4.0\CMSIS\Core\Include\ - %24(PackRepoDir)\atmel\SAME51_DFP\1.1.139\include ../Config ../ ../examples @@ -412,12 +411,13 @@ ../hpl/tc ../hpl/tcc ../hri + %24(PackRepoDir)\atmel\SAME51_DFP\1.1.139\include True Maximum (-g3) True - -std=gnu11 -mfloat-abi=hard -mfpu=fpv4-sp-d16 + -std=gnu99 -mfloat-abi=hard -mfpu=fpv4-sp-d16 True @@ -438,7 +438,6 @@ %24(PackRepoDir)\arm\CMSIS\5.4.0\CMSIS\Core\Include\ - %24(PackRepoDir)\atmel\SAME51_DFP\1.1.139\include ../Config ../ ../examples @@ -463,13 +462,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,6 +493,7 @@ ../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 e097c42..d52ad43 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/bldc.c +++ b/2_Motor_Master/Motor_Master/Motor_Master/bldc.c @@ -33,15 +33,34 @@ void motor_StateMachine(BLDCMotor_t* const 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); + //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; + motor->motor_state.currentstate = MOTOR_VI_CTRL_STATE; break; case MOTOR_OPEN_LOOP_STATE: BLDC_runOpenLoop(motor, *M1_Desired_dc); calculate_motor_speed(motor); motor->motor_state.previousstate = motor->motor_state.currentstate; break; + case MOTOR_V_CTRL_STATE: + calculate_motor_speed(motor); + BLDC_runSpeedCntl(motor, (float32_t)motor->motor_status.calc_rpm, (float32_t)motor->motor_setpoints.desired_speed); + motor->motor_state.previousstate = motor->motor_state.currentstate; + break; + case MOTOR_VI_CTRL_STATE: + 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); + 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); + 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) { case 0: /* PWM FREQ / 25 - 1kHz */ @@ -100,8 +119,10 @@ void BldcInitStruct(BLDCMotor_t* const motor, BLDCMotor_param_t * const motor_pa motor->motor_status.cur_comm_step = 0; motor->motor_status.currentHallPattern = 1; //motor->motor_status.nextHallPattern = 3; - motor->motor_setpoints.directionOffset = 0; + motor->motor_status.speed_average = 0; + motor->motor_status.count =1; + // ---------------------------------------------------------------------- // Initialize Phase Current Struct: // ---------------------------------------------------------------------- @@ -146,6 +167,7 @@ void BldcInitStruct(BLDCMotor_t* const motor, BLDCMotor_param_t * const motor_pa motor->motor_setpoints.max_current = 0.0; motor->motor_setpoints.max_torque = 0.0; motor->motor_setpoints.max_velocity = 0; + motor->motor_setpoints.directionOffset = 0; //// ------------------------------------------------------------------------------ //// Initialize PI current control @@ -160,14 +182,20 @@ void BldcInitStruct(BLDCMotor_t* const motor, BLDCMotor_param_t * const motor_pa //// Initialize PD Vel control //// ------------------------------------------------------------------------------ PID_objectInit(&motor->controllers.Pid_Speed); + /* V Control Gains */ + //motor->controllers.Pid_Speed.Kp = 0.005f; + //motor->controllers.Pid_Speed.Ki = 0.00001f; + //motor->controllers.Pid_Speed.Kd = 0.001f; /* VI Control Gains */ - //motor->controllers.Pid_Speed.Kp = 0.0003f; - //motor->controllers.Pid_Speed.Ki = 0.001f; - motor->controllers.Pid_Speed.Kp = 0.0005f; - motor->controllers.Pid_Speed.Ki = 0.0f; - motor->controllers.Pid_Speed.Kd = 0.0001f; - //motor->controllers.Pid_Speed.Ki = 0.0001f; + motor->controllers.Pid_Speed.Kp = 0.0002f; + motor->controllers.Pid_Speed.Ki = 0.0000001f; + + //motor->controllers.Pid_Speed.Kp = 0.0005f; + //motor->controllers.Pid_Speed.Ki = 0.0f; + //motor->controllers.Pid_Speed.Kd = 0.0001f; + + /* Limits */ motor->controllers.Pid_Speed.OutMax_pu = (motor_param->motor_Max_Current_IDC_A); motor->controllers.Pid_Speed.OutMin_pu = -(motor_param->motor_Max_Current_IDC_A); @@ -280,7 +308,7 @@ void calculate_motor_speed(BLDCMotor_t* const motor) ////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 * motor->motor_param->motor_commutationStates); //temp_rpm = HZ_TO_RPM * period_after_capture; ////toc_port(DEBUG_3_PORT); @@ -297,32 +325,31 @@ void calculate_motor_speed(BLDCMotor_t* const motor) temp_rpm = temp_rpm; } - motor->motor_status.calc_rpm = (int16_t)temp_rpm; + //motor->motor_status.calc_rpm = (int16_t)temp_rpm; #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) - //{ - //count = 1; - //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(motor->motor_status.actualDirection == CCW) /* Changed from CCW */ - //{ - ////*motor_speed = -1* Motor1.calc_rpm; - //motor->motor_status.calc_rpm = -1* motor->motor_status.calc_rpm; - //} else { - ////*motor_speed = Motor1.calc_rpm; - //motor->motor_status.calc_rpm = motor->motor_status.calc_rpm; - //} - //return; - //} - //else count++; + // To avoid noise an average is realized on 8 samples + motor->motor_status.speed_average += temp_rpm; + if(motor->motor_status.count >= n_SAMPLE) + { + + motor->motor_status.count = 1; + motor->motor_status.calc_rpm = (motor->motor_status.speed_average >> 3); // divide by 8 + //Motor1.motor_status.calc_rpm = (speed_average); // divide by 8 + motor->motor_status.speed_average = 0; + //*Spare_byte1 = motorState.actualDirection; + if(motor->motor_status.actualDirection == CCW) /* Changed from CCW */ + { + //*motor_speed = -1* Motor1.calc_rpm; + motor->motor_status.calc_rpm = -1* motor->motor_status.calc_rpm; + } else { + //*motor_speed = Motor1.calc_rpm; + motor->motor_status.calc_rpm = motor->motor_status.calc_rpm; + } + return; + } + else motor->motor_status.count++; #endif motor->motor_status.calc_rpm = (int16_t)temp_rpm; @@ -371,7 +398,7 @@ void BLDC_runSpeedCntl(BLDCMotor_t *motor, const float32_t speedfbk, const float motor->controllers.Pid_Speed.Ref_pu = f_clamp(speedRef, -motor->motor_param->motor_Max_Spd_RPM, motor->motor_param->motor_Max_Spd_RPM); - if (applicationStatus.currentstate == MOTOR_V_CTRL_STATE) + if (motor->motor_state.currentstate == MOTOR_V_CTRL_STATE) { /* Output Pu in Volts (PWM %) */ motor->controllers.Pid_Speed.OutMax_pu = motor->VdcBus_pu; diff --git a/2_Motor_Master/Motor_Master/Motor_Master/bldc.h b/2_Motor_Master/Motor_Master/Motor_Master/bldc.h index c917e17..e5ae0dc 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 (600) +#define MAX_PWM (400) //#define MAX_VEL 3800 #define CW (0) //CBA @@ -29,7 +29,7 @@ #define INVALID_HALL_0 (0) #define INVALID_HALL_7 (7) -//#define AVERAGE_SPEED_MEASURE +//#define AVERAGE_SPEED_MEASURE 1 #define n_SAMPLE 8 // ---------------------------------------------------------------------- @@ -75,6 +75,7 @@ volatile BLDCMotor_t Motor2; volatile MOTOR_STATE_t Motor1_Status; volatile MOTOR_STATE_t Motor2_Status; + // ---------------------------------------------------------------------- // functions // ---------------------------------------------------------------------- @@ -92,6 +93,5 @@ static void BLDC_runPosCntl(BLDCMotor_t *motor, int16_t posfbk, int16_t posRef); static void BLDC_runOpenLoop(BLDCMotor_t *motor, int16_t duty); volatile uint8_t readHallSensorM1(void); volatile uint8_t readHallSensorM2(void); -volatile uint8_t readHallSensorM1v2(void); #endif /* BLDC_H_ */ \ No newline at end of file diff --git a/2_Motor_Master/Motor_Master/Motor_Master/bldc_types.h b/2_Motor_Master/Motor_Master/Motor_Master/bldc_types.h index d8b1b4c..db701a1 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/bldc_types.h +++ b/2_Motor_Master/Motor_Master/Motor_Master/bldc_types.h @@ -68,7 +68,7 @@ volatile typedef struct { volatile uint8_t actualDirection; //! The actual direction of rotation. volatile uint16_t duty_cycle; - volatile int16_t calc_rpm; + volatile int16_t calc_rpm; volatile int16_t Num_Steps; /* Hall States */ //volatile uint8_t prevHallPattern; @@ -77,6 +77,8 @@ volatile typedef struct /* Commutation State */ volatile uint8_t cur_comm_step; volatile uint8_t prev_comm_step; + volatile uint32_t speed_average; + volatile uint8_t count; } MOTOR_Status; // ---------------------------------------------------------------------- diff --git a/2_Motor_Master/Motor_Master/Motor_Master/interrupts.h b/2_Motor_Master/Motor_Master/Motor_Master/interrupts.h index 229a9c2..df2f689 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/interrupts.h +++ b/2_Motor_Master/Motor_Master/Motor_Master/interrupts.h @@ -47,6 +47,19 @@ void TC4_Handler(void) } +static void M1_RESET_BAR(void) +{ + volatile int x = 0; +} + +static void M2_RESET_BAR(void) +{ + volatile int x = 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 a91aa00..5ce61d2 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/main.c +++ b/2_Motor_Master/Motor_Master/Motor_Master/main.c @@ -55,6 +55,7 @@ static void One_ms_cycle_callback(const struct timer_task *const timer_task) } update_telemetry(); + update_setpoints(); //run_ECAT = true; } @@ -79,8 +80,9 @@ void One_ms_timer_init(void) void enable_NVIC_IRQ(void) { - //hri_tc_clear_INTEN_OVF_bit(TC2); - //hri_tc_clear_INTEN_OVF_bit(TC4); + + ext_irq_register(M1_RST_Bar, M1_RESET_BAR); + ext_irq_register(M2_RST_Bar, M2_RESET_BAR); NVIC_EnableIRQ(TC2_IRQn); // TC2: M1_Speed_Timer NVIC_EnableIRQ(TC4_IRQn); // TC4: M2_Speed_Timer NVIC_EnableIRQ(DMAC_0_IRQn);