251 lines
7.0 KiB
C
251 lines
7.0 KiB
C
#include <atmel_start.h>
|
|
#include <hal_gpio.h>
|
|
#include <hal_delay.h>
|
|
#include <arm_math.h>
|
|
|
|
// ----------------------------------------------------------------------
|
|
// Header Files
|
|
// ----------------------------------------------------------------------
|
|
#include "interrupt_handlers.h"
|
|
#include "ethercat/ethercat_e54.h"
|
|
#include "configuration.h"
|
|
#include "pins.h"
|
|
#include "statemachine.h"
|
|
#include "ethercat/ethercat_slave_def.h"
|
|
#include "utilities.h"
|
|
|
|
// ----------------------------------------------------------------------
|
|
// Defines
|
|
// ----------------------------------------------------------------------
|
|
//#define DBG_PIN1 GPIO(GPIO_PORTD, 0)
|
|
|
|
|
|
|
|
|
|
inline void CONTROLLER_StateMachine(void)
|
|
{
|
|
if (applicationStatus.fault)
|
|
{
|
|
applicationStatus.previousstate = applicationStatus.currentstate;
|
|
applicationStatus.currentstate = FAULT;
|
|
}
|
|
|
|
switch(applicationStatus.currentstate) /* process current motor state*/
|
|
{
|
|
case SYSTEM_INIT:
|
|
/* Toggle driver reset Latch */
|
|
gpio_set_pin_level(DRV_RESET, true);
|
|
gpio_set_pin_level(DRV_RESET, false);
|
|
/* Update State Variables */
|
|
applicationStatus.previousstate = applicationStatus.currentstate;
|
|
applicationStatus.currentstate = SYSTEM_IDLE;
|
|
break;
|
|
case SYSTEM_IDLE:
|
|
/* If received MOTOR START command, move to MOTOR_INIT */
|
|
//if(HostController.StartStopMotor == 0)
|
|
//{
|
|
applicationStatus.previousstate = applicationStatus.currentstate;
|
|
applicationStatus.currentstate = MOTOR_IDLE;
|
|
//}
|
|
break;
|
|
case MOTOR_IDLE:
|
|
applicationStatus.previousstate = applicationStatus.currentstate;
|
|
//applicationStatus.currentstate = MOTOR_PVI_CTRL_STATE;
|
|
applicationStatus.currentstate = MOTOR_V_CTRL_STATE;
|
|
break;
|
|
|
|
case MOTOR_I_CTRL_STATE:
|
|
break;
|
|
|
|
case MOTOR_V_CTRL_STATE:
|
|
switch (Motor1.regulation_loop_count) {
|
|
case 0: /* PWM FREQ / 25 - 1kHz */
|
|
Motor1.timerflags.motor_telemetry_flag = true;
|
|
/* Blank */
|
|
case 6: /* PWM FREQ / 6.25 - 4kHz */
|
|
calculate_motor_speed();
|
|
BLDC_runSpeedCntl(&Motor1, Motor1.motor_status.calc_rpm, Motor1.motor_setpoints.desired_speed);
|
|
//BLDC_runSpeedCntl(&Motor1, Motor1.motor_status.calc_rpm, 2000);
|
|
default: /* PWM FREQ - 25kHz */
|
|
select_active_phase(&Motor1, Motor1.motor_status.currentHallPattern); /* Still measure current */
|
|
break;
|
|
} // end switch(regulation_loop_count)
|
|
|
|
if(Motor1.regulation_loop_count > 23) Motor1.regulation_loop_count = 0;
|
|
else Motor1.regulation_loop_count++;
|
|
break;
|
|
|
|
case MOTOR_P_CTRL_STATE:
|
|
break;
|
|
|
|
case MOTOR_VI_CTRL_STATE:
|
|
switch (Motor1.regulation_loop_count) {
|
|
case 0: /* PWM FREQ / 25 - 1kHz */
|
|
Motor1.timerflags.motor_telemetry_flag = true;
|
|
/* Blank */
|
|
case 6: /* PWM FREQ / 6.25 - 4kHz */
|
|
calculate_motor_speed();
|
|
BLDC_runSpeedCntl(&Motor1, Motor1.motor_status.calc_rpm, Motor1.motor_setpoints.desired_speed);
|
|
default: /* PWM FREQ - 25kHz */
|
|
select_active_phase(&Motor1, Motor1.motor_status.currentHallPattern);
|
|
BLDC_runCurrentCntl(&Motor1, Motor1.Iphase_pu.Bus, Motor1.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++;
|
|
break;
|
|
|
|
case MOTOR_PVI_CTRL_STATE:
|
|
switch (Motor1.regulation_loop_count) {
|
|
case 0: /* PWM FREQ / 25 - 1kHz */
|
|
Motor1.timerflags.motor_telemetry_flag = true; // Update telemetry flag
|
|
//tic(DEBUG_4);
|
|
BLDC_runPosCntl(&Motor1, Motor1.motor_status.Num_Steps, Motor1.motor_setpoints.desired_position);
|
|
//toc(DEBUG_4);
|
|
case 5: case 10: case 15: case 20:/* PWM FREQ / 5 - 5kHz */
|
|
//tic(DEBUG_3);
|
|
//tic(DEBUG_4);
|
|
//tic(DEBUG_2);
|
|
calculate_motor_speed();
|
|
//toc(DEBUG_2);
|
|
//tic(DEBUG_3);
|
|
BLDC_runSpeedCntl(&Motor1, Motor1.motor_status.calc_rpm, Motor1.controllers.Pid_Speed.Ref_pu);
|
|
//toc(DEBUG_3);
|
|
//toc(DEBUG_4);
|
|
//toc(DEBUG_3);
|
|
default: /* PWM FREQ - 25kHz */
|
|
//tic(DEBUG_2);
|
|
//tic(DEBUG_3);
|
|
select_active_phase(&Motor1, Motor1.motor_status.currentHallPattern);
|
|
////toc(DEBUG_2);
|
|
////tic(DEBUG_3);
|
|
BLDC_runCurrentCntl(&Motor1, Motor1.Iphase_pu.Bus, Motor1.controllers.Pi_Idc.Ref_pu);
|
|
//toc(DEBUG_3);
|
|
//toc(DEBUG_2);
|
|
break;
|
|
} // end switch(regulation_loop_count)
|
|
|
|
|
|
if(Motor1.regulation_loop_count > 23) Motor1.regulation_loop_count = 0;
|
|
else Motor1.regulation_loop_count++;
|
|
break;
|
|
|
|
case MOTOR_STOP:
|
|
applicationStatus.previousstate = applicationStatus.currentstate;
|
|
applicationStatus.currentstate = SYSTEM_IDLE;
|
|
break;
|
|
case FAULT:
|
|
//DisableGateDrivers(&Motor1);
|
|
switch(applicationStatus.fault)
|
|
{
|
|
case NOFAULT:
|
|
break;
|
|
case HALLSENSORINVALID:
|
|
break;
|
|
case MOTOR_STALL:
|
|
break;
|
|
case VOLTAGE:
|
|
break;
|
|
case OVERCURRENT:
|
|
break;
|
|
case GATE_DRIVER:
|
|
break;
|
|
case UNKNOWN:
|
|
break;
|
|
default: break;
|
|
}// End switch(switch(applicationStatus.fault))
|
|
default: break;
|
|
} // End switch(applicationStatus.currentstate)
|
|
//gpio_set_pin_level(DRV_RST, true);
|
|
exec_commutation();
|
|
|
|
} // inline void CONTROLLER_StateMachine(void)
|
|
|
|
void enable_NVIC_IRQ(void)
|
|
{
|
|
NVIC_EnableIRQ(TC7_IRQn); // TC7: TC_ECAT
|
|
NVIC_EnableIRQ(TC0_IRQn); // TC0: TC_SPEED
|
|
NVIC_EnableIRQ(DMAC_0_IRQn);
|
|
NVIC_EnableIRQ(TCC1_0_IRQn);
|
|
NVIC_EnableIRQ(TCC1_0_IRQn);
|
|
NVIC_EnableIRQ(EIC_5_IRQn);
|
|
}
|
|
|
|
|
|
|
|
int main(void)
|
|
{
|
|
/* Initializes MCU, drivers and middleware */
|
|
//__disable_irq();
|
|
atmel_start_init();
|
|
BldcInitStruct(&Motor1);
|
|
BldcInitStruct(&Motor2);
|
|
BldcInitStruct(&Motor3);
|
|
BldcInitFunctions();
|
|
|
|
read_zero_current_offset_value();
|
|
configure_tcc_pwm();
|
|
config_pins();
|
|
configure_adc();
|
|
configure_ethercat_dma_descriptors();
|
|
ethercat_update();
|
|
custom_logic_enable();
|
|
configure_TC_CCL_SPEED();
|
|
|
|
ext_irq_register(nDRV_RESET, HW_current_limit_detect_callback);
|
|
enable_NVIC_IRQ();
|
|
|
|
/* Replace with your application code */
|
|
while (1) {
|
|
//tic(DEBUG_1);
|
|
//tic_port(DEBUG_3_PORT);
|
|
//toc(DEBUG_1);
|
|
//toc_port(DEBUG_3_PORT);
|
|
|
|
|
|
//tic(DEBUG_1_PORT);
|
|
//toc(DEBUG_1_PORT);
|
|
|
|
if(Motor1.timerflags.motor_telemetry_flag)
|
|
{
|
|
Motor1.timerflags.motor_telemetry_flag = false;
|
|
update_setpoints();
|
|
update_telemetry();
|
|
}
|
|
//{
|
|
//gpio_set_pin_level(DRV_RST, true);
|
|
//Motor1.timerflags.pwm_cycle_tic = false;
|
|
//tic(DEBUG_1);
|
|
//exec_commutation();
|
|
//toc(DEBUG_1);
|
|
//}
|
|
if(Motor1.timerflags.current_loop_tic) {
|
|
Motor1.timerflags.current_loop_tic = false;
|
|
tic_port(DEBUG_1_PORT);
|
|
CONTROLLER_StateMachine();
|
|
toc_port(DEBUG_1_PORT);
|
|
}
|
|
|
|
|
|
|
|
//do {
|
|
//delay_ms(10);
|
|
//} while (gpio_get_pin_level(SW0));
|
|
//
|
|
//if(Motor1.dir_hall_code < 14) {
|
|
//Motor1.dir_hall_code++;
|
|
//} else {
|
|
//Motor1.dir_hall_code=1;
|
|
//}
|
|
//
|
|
//do {
|
|
//delay_ms(10);
|
|
//} while (!gpio_get_pin_level(SW0));
|
|
}
|
|
}
|
|
|
|
|
|
|
|
|