diff --git a/2_Motor_Master/Motor_Master/Motor_Master/.atmelstart/AtmelStart.gpdsc b/2_Motor_Master/Motor_Master/Motor_Master/.atmelstart/AtmelStart.gpdsc index bdbf8e2..41de0f3 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/.atmelstart/AtmelStart.gpdsc +++ b/2_Motor_Master/Motor_Master/Motor_Master/.atmelstart/AtmelStart.gpdsc @@ -50,7 +50,6 @@ - @@ -158,7 +157,6 @@ - @@ -177,7 +175,6 @@ - @@ -200,8 +197,6 @@ - - @@ -221,7 +216,6 @@ - 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 6222e30..e9e208e 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 @@ -236,7 +236,7 @@ drivers: functionality: System api: HAL:HPL:DMAC configuration: - dmac_beatsize_0: 8-bit bus transfer + dmac_beatsize_0: 32-bit bus transfer dmac_beatsize_1: 16-bit bus transfer dmac_beatsize_10: 8-bit bus transfer dmac_beatsize_11: 8-bit bus transfer @@ -259,7 +259,7 @@ drivers: dmac_beatsize_27: 8-bit bus transfer dmac_beatsize_28: 8-bit bus transfer dmac_beatsize_29: 8-bit bus transfer - dmac_beatsize_3: 8-bit bus transfer + dmac_beatsize_3: 32-bit bus transfer dmac_beatsize_30: 8-bit bus transfer dmac_beatsize_31: 8-bit bus transfer dmac_beatsize_4: 8-bit bus transfer @@ -558,9 +558,9 @@ drivers: dmac_lvl_8: Channel priority 0 dmac_lvl_9: Channel priority 0 dmac_lvlen0: true - dmac_lvlen1: true - dmac_lvlen2: true - dmac_lvlen3: true + dmac_lvlen1: false + dmac_lvlen2: false + dmac_lvlen3: false dmac_lvlpri0: 0 dmac_lvlpri1: 0 dmac_lvlpri2: 0 @@ -923,7 +923,7 @@ drivers: api: HAL:Driver:Event_system configuration: evsys_channel_0: No channel output selected - evsys_channel_1: No channel output selected + evsys_channel_1: Channel 3 evsys_channel_10: No channel output selected evsys_channel_11: No channel output selected evsys_channel_12: No channel output selected @@ -1008,7 +1008,7 @@ drivers: evsys_channel_setting_27: false evsys_channel_setting_28: false evsys_channel_setting_29: false - evsys_channel_setting_3: false + evsys_channel_setting_3: true evsys_channel_setting_30: false evsys_channel_setting_31: false evsys_channel_setting_4: false @@ -1136,7 +1136,7 @@ drivers: evsys_evgen_27: No event generator evsys_evgen_28: No event generator evsys_evgen_29: No event generator - evsys_evgen_3: No event generator + evsys_evgen_3: TC0 match/capture 0 evsys_evgen_30: No event generator evsys_evgen_31: No event generator evsys_evgen_4: No event generator @@ -1232,7 +1232,7 @@ drivers: evsys_path_27: Synchronous path evsys_path_28: Synchronous path evsys_path_29: Synchronous path - evsys_path_3: Synchronous path + evsys_path_3: Asynchronous path evsys_path_30: Synchronous path evsys_path_31: Synchronous path evsys_path_4: Synchronous path @@ -1642,7 +1642,7 @@ drivers: functionality: System api: HAL:HPL:PORT configuration: - enable_port_input_event_0: false + enable_port_input_event_0: true enable_port_input_event_1: false enable_port_input_event_2: false enable_port_input_event_3: false @@ -1658,15 +1658,15 @@ drivers: porta_input_event_enable_1: false porta_input_event_enable_2: false porta_input_event_enable_3: false - portb_event_action_0: Output register of pin will be set to level of event + portb_event_action_0: Clear output register of pin on event portb_event_action_1: Output register of pin will be set to level of event portb_event_action_2: Output register of pin will be set to level of event portb_event_action_3: Output register of pin will be set to level of event - portb_event_pin_identifier_0: 0 + portb_event_pin_identifier_0: 22 portb_event_pin_identifier_1: 0 portb_event_pin_identifier_2: 0 portb_event_pin_identifier_3: 0 - portb_input_event_enable_0: false + portb_input_event_enable_0: true portb_input_event_enable_1: false portb_input_event_enable_2: false portb_input_event_enable_3: false @@ -1882,24 +1882,53 @@ drivers: slow_gclk_selection: Generic clock generator 3 TIMER_0: user_label: TIMER_0 - definition: Atmel:SAME51_Drivers:0.0.1::SAME51J19A-MF::TC0::driver_config_definition::Timer::HAL:Driver:Timer + definition: Atmel:SAME51_Drivers:0.0.1::SAME51J19A-MF::TC0::driver_config_definition::16-bit.Counter.Mode::Lite:TC:Timer functionality: Timer - api: HAL:Driver:Timer + api: Lite:TC:Timer configuration: - tc_arch_dbgrun: false - tc_arch_evact: Event action disabled - tc_arch_mceo0: false - tc_arch_mceo1: false - tc_arch_ondemand: false - tc_arch_ovfeo: false - tc_arch_presync: Reload or reset counter on next GCLK - tc_arch_runstdby: false - tc_arch_tcei: false - tc_arch_tcinv: false - timer_advanced_configuration: true - timer_event_control: false - timer_prescaler: Divide by 64 - timer_tick: 1000 + cc_cc0: 1874 + cc_cc1: 0 + cc_control: true + count_control: false + count_count: 0 + ctrla_alock: false + ctrla_capten0: false + ctrla_capten1: false + ctrla_captmode0: DEFAULT + ctrla_captmode1: DEFAULT + ctrla_control: true + ctrla_copen0: false + ctrla_copen1: false + ctrla_enable: true + ctrla_mode: 0 + ctrla_ondemand: false + ctrla_prescaler: DIV64 + ctrla_prescsync: GCLK + ctrla_runstdby: false + ctrlbset_cmd: NONE + ctrlbset_control: false + ctrlbset_dir: false + ctrlbset_lupd: false + ctrlbset_oneshot: false + ctrlc_inven0: false + ctrlc_inven1: false + dbgctrl_control: false + dbgctrl_dbgrun: false + drvctrl_control: false + evctrl_control: true + evctrl_evact: 'OFF' + evctrl_mceo0: true + evctrl_mceo1: false + evctrl_ovfeo: false + evctrl_tcei: false + evctrl_tcinv: false + intenset_control: true + intenset_err: false + intenset_mc0: true + intenset_mc1: false + intenset_ovf: false + wave_control: true + wave_wavegen: MFRQ optional_signals: [] variant: null clocks: diff --git a/2_Motor_Master/Motor_Master/Motor_Master/Config/hpl_dmac_config.h b/2_Motor_Master/Motor_Master/Motor_Master/Config/hpl_dmac_config.h index 78d6df5..98903a7 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/Config/hpl_dmac_config.h +++ b/2_Motor_Master/Motor_Master/Motor_Master/Config/hpl_dmac_config.h @@ -36,7 +36,7 @@ // Indicates whether Priority Level 1 is enabled or not // dmac_lvlen1 #ifndef CONF_DMAC_LVLEN1 -#define CONF_DMAC_LVLEN1 1 +#define CONF_DMAC_LVLEN1 0 #endif // Level 1 Round-Robin Arbitration @@ -57,7 +57,7 @@ // Indicates whether Priority Level 2 is enabled or not // dmac_lvlen2 #ifndef CONF_DMAC_LVLEN2 -#define CONF_DMAC_LVLEN2 1 +#define CONF_DMAC_LVLEN2 0 #endif // Level 2 Round-Robin Arbitration @@ -78,7 +78,7 @@ // Indicates whether Priority Level 3 is enabled or not // dmac_lvlen3 #ifndef CONF_DMAC_LVLEN3 -#define CONF_DMAC_LVLEN3 1 +#define CONF_DMAC_LVLEN3 0 #endif // Level 3 Round-Robin Arbitration @@ -301,7 +301,7 @@ // Defines the size of one beat // dmac_beatsize_0 #ifndef CONF_DMAC_BEATSIZE_0 -#define CONF_DMAC_BEATSIZE_0 0 +#define CONF_DMAC_BEATSIZE_0 2 #endif // Block Action @@ -973,7 +973,7 @@ // Defines the size of one beat // dmac_beatsize_3 #ifndef CONF_DMAC_BEATSIZE_3 -#define CONF_DMAC_BEATSIZE_3 0 +#define CONF_DMAC_BEATSIZE_3 2 #endif // Block Action 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 5d22b57..88f75b3 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 @@ -550,7 +550,7 @@ // Channel 3 settings // evsys_channel_setting_3 #ifndef CONF_EVSYS_CHANNEL_SETTINGS_3 -#define CONF_EVSYS_CHANNEL_SETTINGS_3 0 +#define CONF_EVSYS_CHANNEL_SETTINGS_3 1 #endif // Edge detection @@ -571,7 +571,7 @@ // Asynchronous path // evsys_path_3 #ifndef CONF_PATH_3 -#define CONF_PATH_3 EVSYS_CHANNEL_PATH_SYNCHRONOUS_Val +#define CONF_PATH_3 EVSYS_CHANNEL_PATH_ASYNCHRONOUS_Val #endif // Event generator @@ -695,7 +695,7 @@ // <0x77=>CCL LUT output 3 // evsys_evgen_3 #ifndef CONF_EVGEN_3 -#define CONF_EVGEN_3 0 +#define CONF_EVGEN_3 74 #endif // Overrun channel interrupt @@ -5880,7 +5880,7 @@ // evsys_channel_1 // Indicates which channel is chosen for user #ifndef CONF_CHANNEL_1 -#define CONF_CHANNEL_1 0 +#define CONF_CHANNEL_1 4 #endif // Channel selection for PORT event 1 diff --git a/2_Motor_Master/Motor_Master/Motor_Master/Config/hpl_port_config.h b/2_Motor_Master/Motor_Master/Motor_Master/Config/hpl_port_config.h index 1efce33..79a9242 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/Config/hpl_port_config.h +++ b/2_Motor_Master/Motor_Master/Motor_Master/Config/hpl_port_config.h @@ -7,7 +7,7 @@ // PORT Input Event 0 configuration // enable_port_input_event_0 #ifndef CONF_PORT_EVCTRL_PORT_0 -#define CONF_PORT_EVCTRL_PORT_0 0 +#define CONF_PORT_EVCTRL_PORT_0 1 #endif // PORT Input Event 0 configuration on PORT A @@ -44,14 +44,14 @@ // The event action will be triggered on any incoming event if PORT B Input Event 0 configuration is enabled // portb_input_event_enable_0 #ifndef CONF_PORTB_EVCTRL_PORTEI_0 -#define CONF_PORTB_EVCTRL_PORTEI_0 0x0 +#define CONF_PORTB_EVCTRL_PORTEI_0 0x1 #endif // PORTB Event 0 Pin Identifier <0x00-0x1F> // These bits define the I/O pin from port B on which the event action will be performed // portb_event_pin_identifier_0 #ifndef CONF_PORTB_EVCTRL_PID_0 -#define CONF_PORTB_EVCTRL_PID_0 0x0 +#define CONF_PORTB_EVCTRL_PID_0 0x16 #endif // PORTB Event 0 Action @@ -62,7 +62,7 @@ // These bits define the event action the PORT B will perform on event input 0 // portb_event_action_0 #ifndef CONF_PORTB_EVCTRL_EVACT_0 -#define CONF_PORTB_EVCTRL_EVACT_0 0 +#define CONF_PORTB_EVCTRL_EVACT_0 2 #endif // diff --git a/2_Motor_Master/Motor_Master/Motor_Master/Config/hpl_tc_config.h b/2_Motor_Master/Motor_Master/Motor_Master/Config/hpl_tc_config.h deleted file mode 100644 index bfabf21..0000000 --- a/2_Motor_Master/Motor_Master/Motor_Master/Config/hpl_tc_config.h +++ /dev/null @@ -1,180 +0,0 @@ -/* Auto-generated config file hpl_tc_config.h */ -#ifndef HPL_TC_CONFIG_H -#define HPL_TC_CONFIG_H - -// <<< Use Configuration Wizard in Context Menu >>> - -#ifndef CONF_TC0_ENABLE -#define CONF_TC0_ENABLE 1 -#endif - -#include "peripheral_clk_config.h" - -// Basic configuration - -// Prescaler -// <0x0=> No division -// <0x1=> Divide by 2 -// <0x2=> Divide by 4 -// <0x3=> Divide by 8 -// <0x4=> Divide by 16 -// <0x5=> Divide by 64 -// <0x6=> Divide by 256 -// <0x7=> Divide by 1024 -// This defines the prescaler value -// timer_prescaler -#ifndef CONF_TC0_PRESCALER -#define CONF_TC0_PRESCALER 0x5 -#endif - -// Length of one timer tick in uS <0-4294967295> -// timer_tick -#ifndef CONF_TC0_TIMER_TICK -#define CONF_TC0_TIMER_TICK 1000 -#endif -// - -// Advanced configuration -// timer_advanced_configuration -#ifndef CONF_TC0__ADVANCED_CONFIGURATION_ENABLE -#define CONF_TC0__ADVANCED_CONFIGURATION_ENABLE 1 -#endif - -// Prescaler and Counter Synchronization Selection -// Reload or reset counter on next GCLK -// Reload or reset counter on next prescaler clock -// Reload or reset counter on next GCLK and reset prescaler counter -// These bits select if on retrigger event, the Counter should be cleared or reloaded on the next GCLK_TCx clock or on the next prescaled GCLK_TCx clock. -// tc_arch_presync -#ifndef CONF_TC0_PRESCSYNC -#define CONF_TC0_PRESCSYNC TC_CTRLA_PRESCSYNC_GCLK_Val -#endif - -// Run in standby -// Indicates whether the module will continue to run in standby sleep mode -// tc_arch_runstdby -#ifndef CONF_TC0_RUNSTDBY -#define CONF_TC0_RUNSTDBY 0 -#endif - -// Run in debug mode -// Indicates whether the module will run in debug mode -// tc_arch_dbgrun -#ifndef CONF_TC0_DBGRUN -#define CONF_TC0_DBGRUN 0 -#endif - -// Run on demand -// Run if requested by some other peripheral in the device -// tc_arch_ondemand -#ifndef CONF_TC0_ONDEMAND -#define CONF_TC0_ONDEMAND 0 -#endif - -// - -// Event control -// timer_event_control -#ifndef CONF_TC0_EVENT_CONTROL_ENABLE -#define CONF_TC0_EVENT_CONTROL_ENABLE 0 -#endif - -// Output Event On Match or Capture on Channel 0 -// Enable output of event on timer tick -// tc_arch_mceo0 -#ifndef CONF_TC0_MCEO0 -#define CONF_TC0_MCEO0 0 -#endif - -// Output Event On Match or Capture on Channel 1 -// Enable output of event on timer tick -// tc_arch_mceo1 -#ifndef CONF_TC0_MCEO1 -#define CONF_TC0_MCEO1 0 -#endif - -// Output Event On Timer Tick -// Enable output of event on timer tick -// tc_arch_ovfeo -#ifndef CONF_TC0_OVFEO -#define CONF_TC0_OVFEO 0 -#endif - -// Event Input -// Enable asynchronous input events -// tc_arch_tcei -#ifndef CONF_TC0_TCEI -#define CONF_TC0_TCEI 0 -#endif - -// Inverted Event Input -// Invert the asynchronous input events -// tc_arch_tcinv -#ifndef CONF_TC0_TCINV -#define CONF_TC0_TCINV 0 -#endif - -// Event action -// <0=> Event action disabled -// <1=> Start, restart or re-trigger TC on event -// <2=> Count on event -// <3=> Start on event -// <4=> Time stamp capture -// <5=> Period captured in CC0, pulse width in CC1 -// <6=> Period captured in CC1, pulse width in CC0 -// <7=> Pulse width capture -// Event which will be performed on an event -// tc_arch_evact -#ifndef CONF_TC0_EVACT -#define CONF_TC0_EVACT 0 -#endif -// - -// Default values which the driver needs in order to work correctly - -// Mode set to 32-bit -#ifndef CONF_TC0_MODE -#define CONF_TC0_MODE TC_CTRLA_MODE_COUNT32_Val -#endif - -// CC 1 register set to 0 -#ifndef CONF_TC0_CC1 -#define CONF_TC0_CC1 0 -#endif - -#ifndef CONF_TC0_ALOCK -#define CONF_TC0_ALOCK 0 -#endif - -// Not used in 32-bit mode -#define CONF_TC0_PER 0 - -// Calculating correct top value based on requested tick interval. -#define CONF_TC0_PRESCALE (1 << CONF_TC0_PRESCALER) - -// Prescaler set to 64 -#if CONF_TC0_PRESCALER > 0x4 -#undef CONF_TC0_PRESCALE -#define CONF_TC0_PRESCALE 64 -#endif - -// Prescaler set to 256 -#if CONF_TC0_PRESCALER > 0x5 -#undef CONF_TC0_PRESCALE -#define CONF_TC0_PRESCALE 256 -#endif - -// Prescaler set to 1024 -#if CONF_TC0_PRESCALER > 0x6 -#undef CONF_TC0_PRESCALE -#define CONF_TC0_PRESCALE 1024 -#endif - -#ifndef CONF_TC0_CC0 -#define CONF_TC0_CC0 \ - (uint32_t)(((float)CONF_TC0_TIMER_TICK / 1000000.f) / (1.f / (CONF_GCLK_TC0_FREQUENCY / CONF_TC0_PRESCALE))) -#endif - -// <<< end of configuration section >>> - -#endif // HPL_TC_CONFIG_H 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 0dc043f..dc31bd6 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/EtherCAT_SlaveDef.h +++ b/2_Motor_Master/Motor_Master/Motor_Master/EtherCAT_SlaveDef.h @@ -163,7 +163,8 @@ static void update_telemetry(void) //*M1_Mode = 0; /* Motor 1 */ - *M1_Status = Motor1.motor_state.currentstate; + *M1_Status = Motor1.motor_state.fault; + *M1_Mode = Motor1.motor_state.currentstate; *M1_Joint_rel_position = Motor1.motor_status.Num_Steps; *M1_Joint_abs_position = Motor1.motor_status.abs_position; //*M1_Motor_speed = (((int16_t *)&QSPI_tx_buffer[1])+1); @@ -176,7 +177,8 @@ static void update_telemetry(void) *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_Status = Motor2.motor_state.fault; + *M2_Mode = Motor2.motor_state.currentstate; *M2_Joint_rel_position = Motor2.motor_status.Num_Steps; *M2_Joint_abs_position = Motor2.motor_status.abs_position; //*M1_Motor_speed = (((int16_t *)&QSPI_tx_buffer[1])+1); 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 0dc043f..dc31bd6 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/Ethercat_SlaveDef.h +++ b/2_Motor_Master/Motor_Master/Motor_Master/Ethercat_SlaveDef.h @@ -163,7 +163,8 @@ static void update_telemetry(void) //*M1_Mode = 0; /* Motor 1 */ - *M1_Status = Motor1.motor_state.currentstate; + *M1_Status = Motor1.motor_state.fault; + *M1_Mode = Motor1.motor_state.currentstate; *M1_Joint_rel_position = Motor1.motor_status.Num_Steps; *M1_Joint_abs_position = Motor1.motor_status.abs_position; //*M1_Motor_speed = (((int16_t *)&QSPI_tx_buffer[1])+1); @@ -176,7 +177,8 @@ static void update_telemetry(void) *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_Status = Motor2.motor_state.fault; + *M2_Mode = Motor2.motor_state.currentstate; *M2_Joint_rel_position = Motor2.motor_status.Num_Steps; *M2_Joint_abs_position = Motor2.motor_status.abs_position; //*M1_Motor_speed = (((int16_t *)&QSPI_tx_buffer[1])+1); 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 8d6c7c3..3d20ce8 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/Motor_Master.cproj +++ b/2_Motor_Master/Motor_Master/Motor_Master/Motor_Master.cproj @@ -150,14 +150,13 @@ - - + + - - + + - @@ -176,7 +175,6 @@ - @@ -199,10 +197,8 @@ - - - - + + @@ -210,17 +206,16 @@ - + - + - + - @@ -579,9 +574,6 @@ compile - - compile - compile @@ -663,9 +655,6 @@ compile - - compile - compile @@ -822,9 +811,6 @@ compile - - compile - compile @@ -936,12 +922,6 @@ compile - - compile - - - compile - compile @@ -1142,9 +1122,6 @@ compile - - compile - compile diff --git a/2_Motor_Master/Motor_Master/Motor_Master/bldc.c b/2_Motor_Master/Motor_Master/Motor_Master/bldc.c index 3ca9616..68afdd3 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/bldc.c +++ b/2_Motor_Master/Motor_Master/Motor_Master/bldc.c @@ -38,7 +38,7 @@ void motor_StateMachine(BLDCMotor_t* const motor) motor->motor_state.currentstate = MOTOR_PVI_CTRL_STATE; break; case MOTOR_OPEN_LOOP_STATE: - BLDC_runOpenLoop(motor, *M1_Desired_dc); + BLDC_runOpenLoop(motor, 100); calculate_motor_speed(motor); motor->motor_state.previousstate = motor->motor_state.currentstate; break; @@ -77,6 +77,9 @@ void motor_StateMachine(BLDCMotor_t* const motor) if(motor->regulation_loop_count > 23) motor->regulation_loop_count = 0; else motor->regulation_loop_count++; break; + case MOTOR_FAULT: + disable_phases(motor); + break; } //end switch (motor->motor_state.currentstate) // ---------------------------------------------------------------------- @@ -357,6 +360,12 @@ void calculate_motor_speed(BLDCMotor_t* const motor) } +void disable_phases(BLDCMotor_t* const motor) +{ + Tcc * tmp = (Tcc *)motor->motor_param->pwm_desc->device.hw; + tmp->PATTBUF.reg = DISABLE_PATTERN; +} + //------------------------------------------------------------------------------ // pi current control //------------------------------------------------------------------------------ @@ -509,7 +518,10 @@ void read_zero_current_offset_value(BLDCMotor_t *motor1, BLDCMotor_t *motor2) uint8_t samples = 32; uint8_t i; - // ------------------------- Motor 1 --------------------------------- + // ------------------------------------------------------------------ + // Motor 1 + // ------------------------------------------------------------------- + adc_sync_enable_channel(&ADC_1, 9); //adc_sync_enable_channel(&ADC_1, 0); @@ -560,6 +572,17 @@ void read_zero_current_offset_value(BLDCMotor_t *motor1, BLDCMotor_t *motor2) adc_sync_enable_channel(&ADC_1, 7); //adc_sync_enable_channel(&ADC_1, 0); + if ((abs(motor1->Voffset_lsb.A) > MAX_CUR_SENSE_OFFSET) || (abs(motor1->Voffset_lsb.B) > MAX_CUR_SENSE_OFFSET)) + { + motor1->motor_state.currentstate = MOTOR_FAULT; + motor1->motor_state.fault = MOTOR_CURRENTS_SENSOR; + + } + + // ------------------------------------------------------------------ + // Motor 2 + // ------------------------------------------------------------------- + phase_A_zero_current_offset_temp = 0; phase_B_zero_current_offset_temp = 0; @@ -607,4 +630,9 @@ void read_zero_current_offset_value(BLDCMotor_t *motor1, BLDCMotor_t *motor2) motor2->Voffset_lsb.B = phase_B_zero_current_offset_temp/samples; adc_sync_disable_channel(&ADC_1, 6); //adc_sync_disable_channel(&ADC_1, 0); + if ((abs(motor2->Voffset_lsb.A) > MAX_CUR_SENSE_OFFSET) || (abs(motor2->Voffset_lsb.B) > MAX_CUR_SENSE_OFFSET)) + { + motor2->motor_state.currentstate = MOTOR_FAULT; + motor2->motor_state.fault = MOTOR_CURRENTS_SENSOR; + } } \ No newline at end of file diff --git a/2_Motor_Master/Motor_Master/Motor_Master/bldc.h b/2_Motor_Master/Motor_Master/Motor_Master/bldc.h index 55b82cc..4f48ce7 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/bldc.h +++ b/2_Motor_Master/Motor_Master/Motor_Master/bldc.h @@ -35,7 +35,7 @@ // ---------------------------------------------------------------------- // ADC Parameters // ---------------------------------------------------------------------- -#define ADC_VOLTAGE_REFERENCE (3.3f) +#define ADC_VOLTAGE_REFERENCE (3.0f) #define ADC_RESOLUTION (12) #define ADC_MAX_COUNTS (1<SPI.CTRLC.bit.DATA32B = true; + SERCOM1->SPI.CTRLC.bit.ICSPACE = 5; + SERCOM1->SPI.CTRLC.bit.DATA32B= true; gpio_set_pin_level(SPI1_CS, true); spi_m_dma_enable(&SPI_1_MSIF); } @@ -169,12 +173,12 @@ void init_spi_master_dma_descriptors() _dma_set_source_address(DMAC_CHANNEL_CONF_SERCOM_1_RECEIVE, (uint32_t *)&(((SercomSpi *)(SPI_1_MSIF.dev.prvt))->DATA.reg)); _dma_set_destination_address(DMAC_CHANNEL_CONF_SERCOM_1_RECEIVE, &QSPI_tx_buffer[16]); - _dma_set_data_amount(DMAC_CHANNEL_CONF_SERCOM_1_RECEIVE, MASTER_BUFFER_SIZE); + _dma_set_data_amount(DMAC_CHANNEL_CONF_SERCOM_1_RECEIVE, MASTER_BUFFER_SIZE_LONG); _dma_set_source_address(DMAC_CHANNEL_CONF_SERCOM_1_TRANSMIT, &QSPI_rx_buffer[16]); _dma_set_destination_address(DMAC_CHANNEL_CONF_SERCOM_1_TRANSMIT, (uint32_t *)&(((SercomSpi *)(SPI_1_MSIF.dev.prvt))->DATA.reg)); - _dma_set_data_amount(DMAC_CHANNEL_CONF_SERCOM_1_TRANSMIT, MASTER_BUFFER_SIZE); + _dma_set_data_amount(DMAC_CHANNEL_CONF_SERCOM_1_TRANSMIT, MASTER_BUFFER_SIZE_LONG); hri_dmacdescriptor_set_BTCTRL_VALID_bit(&_descriptor_section[DMAC_CHANNEL_CONF_SERCOM_1_RECEIVE]); hri_dmacdescriptor_set_BTCTRL_VALID_bit(&_descriptor_section[DMAC_CHANNEL_CONF_SERCOM_1_TRANSMIT]); @@ -186,7 +190,7 @@ void init_spi_master_dma_descriptors() //resource_tx->dma_cb.transfer_done = b2bTransferComplete_cb; /* Enable DMA transfer complete interrupt */ - //_dma_set_irq_state(CONF_SERCOM_1_RECEIVE_DMA_CHANNEL, DMA_TRANSFER_COMPLETE_CB, true); + //_dma_set_irq_state(DMAC_CHANNEL_CONF_SERCOM_1_RECEIVE, DMA_TRANSFER_COMPLETE_CB, true); } 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 c1fd65b..a77295b 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/driver_init.c +++ b/2_Motor_Master/Motor_Master/Motor_Master/driver_init.c @@ -15,7 +15,6 @@ struct spi_m_sync_descriptor SPI_2; struct spi_m_sync_descriptor SPI_3; -struct timer_descriptor TIMER_0; struct adc_sync_descriptor ADC_1; @@ -163,6 +162,7 @@ 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_gclk_write_PCHCTRL_reg(GCLK, EVSYS_GCLK_ID_3, CONF_GCLK_EVSYS_CHANNEL_3_SRC | (1 << GCLK_PCHCTRL_CHEN_Pos)); hri_mclk_set_APBBMASK_EVSYS_bit(MCLK); @@ -561,17 +561,11 @@ void SPI_3_init(void) SPI_3_PORT_init(); } -/** - * \brief Timer initialization function - * - * Enables Timer peripheral, clocks and initializes Timer driver - */ -static void TIMER_0_init(void) +void TIMER_0_CLOCK_init(void) { hri_mclk_set_APBAMASK_TC0_bit(MCLK); - hri_gclk_write_PCHCTRL_reg(GCLK, TC0_GCLK_ID, CONF_GCLK_TC0_SRC | (1 << GCLK_PCHCTRL_CHEN_Pos)); - timer_init(&TIMER_0, TC0, _tc_get_timer()); + hri_gclk_write_PCHCTRL_reg(GCLK, TC0_GCLK_ID, CONF_GCLK_TC0_SRC | (1 << GCLK_PCHCTRL_CHEN_Pos)); } void TC_SPEED_M1_CLOCK_init(void) @@ -761,7 +755,10 @@ void system_init(void) SPI_3_init(); + TIMER_0_CLOCK_init(); + TIMER_0_init(); + TC_SPEED_M1_CLOCK_init(); TC_SPEED_M1_init(); diff --git a/2_Motor_Master/Motor_Master/Motor_Master/driver_init.h b/2_Motor_Master/Motor_Master/Motor_Master/driver_init.h index 0922178..74140b9 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/driver_init.h +++ b/2_Motor_Master/Motor_Master/Motor_Master/driver_init.h @@ -34,8 +34,7 @@ extern "C" { #include #include #include -#include -#include +#include #include #include @@ -52,7 +51,6 @@ extern struct qspi_dma_descriptor ECAT_QSPI; extern struct spi_m_dma_descriptor SPI_1_MSIF; extern struct spi_m_sync_descriptor SPI_2; extern struct spi_m_sync_descriptor SPI_3; -extern struct timer_descriptor TIMER_0; extern struct pwm_descriptor PWM_0; @@ -82,6 +80,10 @@ void SPI_3_PORT_init(void); void SPI_3_CLOCK_init(void); void SPI_3_init(void); +void TIMER_0_CLOCK_init(void); + +int8_t TIMER_0_init(void); + void TC_SPEED_M1_CLOCK_init(void); int8_t TC_SPEED_M1_init(void); diff --git a/2_Motor_Master/Motor_Master/Motor_Master/examples/driver_examples.c b/2_Motor_Master/Motor_Master/Motor_Master/examples/driver_examples.c index e5619cb..f9ab5c1 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/examples/driver_examples.c +++ b/2_Motor_Master/Motor_Master/Motor_Master/examples/driver_examples.c @@ -145,33 +145,6 @@ void SPI_3_example(void) io_write(io, example_SPI_3, 12); } -static struct timer_task TIMER_0_task1, TIMER_0_task2; - -/** - * Example of using TIMER_0. - */ -static void TIMER_0_task1_cb(const struct timer_task *const timer_task) -{ -} - -static void TIMER_0_task2_cb(const struct timer_task *const timer_task) -{ -} - -void TIMER_0_example(void) -{ - TIMER_0_task1.interval = 100; - TIMER_0_task1.cb = TIMER_0_task1_cb; - TIMER_0_task1.mode = TIMER_TASK_REPEAT; - TIMER_0_task2.interval = 200; - TIMER_0_task2.cb = TIMER_0_task2_cb; - TIMER_0_task2.mode = TIMER_TASK_REPEAT; - - timer_add_task(&TIMER_0, &TIMER_0_task1); - timer_add_task(&TIMER_0, &TIMER_0_task2); - timer_start(&TIMER_0); -} - /** * Example of using PWM_0. */ diff --git a/2_Motor_Master/Motor_Master/Motor_Master/examples/driver_examples.h b/2_Motor_Master/Motor_Master/Motor_Master/examples/driver_examples.h index 70830e6..ebc8056 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/examples/driver_examples.h +++ b/2_Motor_Master/Motor_Master/Motor_Master/examples/driver_examples.h @@ -22,8 +22,6 @@ void ECAT_QSPI_example(void); void SPI_1_MSIF_example(void); -void TIMER_0_example(void); - void PWM_0_example(void); void PWM_1_example(void); diff --git a/2_Motor_Master/Motor_Master/Motor_Master/hal/documentation/timer.rst b/2_Motor_Master/Motor_Master/Motor_Master/hal/documentation/timer.rst deleted file mode 100644 index c5ca63d..0000000 --- a/2_Motor_Master/Motor_Master/Motor_Master/hal/documentation/timer.rst +++ /dev/null @@ -1,52 +0,0 @@ -============================ -The Timer driver (bare-bone) -============================ - -The Timer driver provides means for delayed and periodical function invocation. - -A timer task is a piece of code (function) executed at a specific time or periodically by the timer after the task has -been added to the timers task queue. The execution delay or period is set in ticks, where one tick is defined as a -configurable number of clock cycles in the hardware timer. Changing the number of clock cycles in a tick automatically -changes execution delays and periods for all tasks in the timers task queue. - -A task has two operation modes, single-shot or repeating mode. In single-shot mode the task is removed from the task queue -and then is executed once, in repeating mode the task reschedules itself automatically after it has executed based on -the period set in the task configuration. -In single-shot mode a task is removed from the task queue before its callback is invoked. It allows an application to -reuse the memory of expired task in the callback. - -Each instance of the Timer driver supports infinite amount of timer tasks, only limited by the amount of RAM available. - -Features --------- -* Initialization and de-initialization -* Starting and stopping -* Timer tasks - periodical invocation of functions -* Changing and obtaining of the period of a timer - -Applications ------------- -* Delayed and periodical function execution for middle-ware stacks and applications. - -Dependencies ------------- -* Each instance of the driver requires separate hardware timer capable of generating periodic interrupt. - -Concurrency ------------ -The Timer driver is an interrupt driven driver.This means that the interrupt that triggers a task may occur during -the process of adding or removing a task via the driver's API. In such case the interrupt processing is postponed -until the task adding or removing is complete. - -The task queue is not protected from the access by interrupts not used by the driver. Due to this -it is not recommended to add or remove a task from such interrupts: in case if a higher priority interrupt supersedes -the driver's interrupt, adding or removing a task may cause unpredictable behavior of the driver. - -Limitations ------------ -* The driver is designed to work outside of an operating system environment, the task queue is therefore processed in interrupt context which may delay execution of other interrupts. -* If there are a lot of frequently called interrupts with the priority higher than the driver's one, it may cause delay for triggering of a task. - -Knows issues and workarounds ----------------------------- -Not applicable diff --git a/2_Motor_Master/Motor_Master/Motor_Master/hal/include/hal_timer.h b/2_Motor_Master/Motor_Master/Motor_Master/hal/include/hal_timer.h deleted file mode 100644 index 43a1ff4..0000000 --- a/2_Motor_Master/Motor_Master/Motor_Master/hal/include/hal_timer.h +++ /dev/null @@ -1,206 +0,0 @@ -/** - * \file - * - * \brief Timer task functionality declaration. - * - * Copyright (c) 2014-2018 Microchip Technology Inc. and its subsidiaries. - * - * \asf_license_start - * - * \page License - * - * Subject to your compliance with these terms, you may use Microchip - * software and any derivatives exclusively with Microchip products. - * It is your responsibility to comply with third party license terms applicable - * to your use of third party software (including open source software) that - * may accompany Microchip software. - * - * THIS SOFTWARE IS SUPPLIED BY MICROCHIP "AS IS". NO WARRANTIES, - * WHETHER EXPRESS, IMPLIED OR STATUTORY, APPLY TO THIS SOFTWARE, - * INCLUDING ANY IMPLIED WARRANTIES OF NON-INFRINGEMENT, MERCHANTABILITY, - * AND FITNESS FOR A PARTICULAR PURPOSE. IN NO EVENT WILL MICROCHIP BE - * LIABLE FOR ANY INDIRECT, SPECIAL, PUNITIVE, INCIDENTAL OR CONSEQUENTIAL - * LOSS, DAMAGE, COST OR EXPENSE OF ANY KIND WHATSOEVER RELATED TO THE - * SOFTWARE, HOWEVER CAUSED, EVEN IF MICROCHIP HAS BEEN ADVISED OF THE - * POSSIBILITY OR THE DAMAGES ARE FORESEEABLE. TO THE FULLEST EXTENT - * ALLOWED BY LAW, MICROCHIP'S TOTAL LIABILITY ON ALL CLAIMS IN ANY WAY - * RELATED TO THIS SOFTWARE WILL NOT EXCEED THE AMOUNT OF FEES, IF ANY, - * THAT YOU HAVE PAID DIRECTLY TO MICROCHIP FOR THIS SOFTWARE. - * - * \asf_license_stop - * - */ - -#ifndef _HAL_TIMER_H_INCLUDED -#define _HAL_TIMER_H_INCLUDED - -#include -#include - -#ifdef __cplusplus -extern "C" { -#endif - -/** - * \addtogroup doc_driver_hal_timer - * - * @{ - */ - -/** - * \brief Timer mode type - */ -enum timer_task_mode { TIMER_TASK_ONE_SHOT, TIMER_TASK_REPEAT }; - -/** - * \brief Timer task descriptor - * - * The timer task descriptor forward declaration. - */ -struct timer_task; - -/** - * \brief Timer task callback function type - */ -typedef void (*timer_cb_t)(const struct timer_task *const timer_task); - -/** - * \brief Timer task structure - */ -struct timer_task { - struct list_element elem; /*! List element. */ - uint32_t time_label; /*! Absolute timer start time. */ - - uint32_t interval; /*! Number of timer ticks before calling the task. */ - timer_cb_t cb; /*! Function pointer to the task. */ - enum timer_task_mode mode; /*! Task mode: one shot or repeat. */ -}; - -/** - * \brief Timer structure - */ -struct timer_descriptor { - struct _timer_device device; - uint32_t time; - struct list_descriptor tasks; /*! Timer tasks list. */ - volatile uint8_t flags; -}; - -/** - * \brief Initialize timer - * - * This function initializes the given timer. - * It checks if the given hardware is not initialized and if the given hardware - * is permitted to be initialized. - * - * \param[out] descr A timer descriptor to initialize - * \param[in] hw The pointer to the hardware instance - * \param[in] func The pointer to a set of function pointers - * - * \return Initialization status. - */ -int32_t timer_init(struct timer_descriptor *const descr, void *const hw, struct _timer_hpl_interface *const func); - -/** - * \brief Deinitialize timer - * - * This function deinitializes the given timer. - * It checks if the given hardware is initialized and if the given hardware is - * permitted to be deinitialized. - * - * \param[in] descr A timer descriptor to deinitialize - * - * \return De-initialization status. - */ -int32_t timer_deinit(struct timer_descriptor *const descr); - -/** - * \brief Start timer - * - * This function starts the given timer. - * It checks if the given hardware is initialized. - * - * \param[in] descr The timer descriptor of a timer to start - * - * \return Timer starting status. - */ -int32_t timer_start(struct timer_descriptor *const descr); - -/** - * \brief Stop timer - * - * This function stops the given timer. - * It checks if the given hardware is initialized. - * - * \param[in] descr The timer descriptor of a timer to stop - * - * \return Timer stopping status. - */ -int32_t timer_stop(struct timer_descriptor *const descr); - -/** - * \brief Set amount of clock cycles per timer tick - * - * This function sets the amount of clock cycles per timer tick for the given timer. - * It checks if the given hardware is initialized. - * - * \param[in] descr The timer descriptor of a timer to stop - * \param[in] clock_cycles The amount of clock cycles per tick to set - * - * \return Setting clock cycles amount status. - */ -int32_t timer_set_clock_cycles_per_tick(struct timer_descriptor *const descr, const uint32_t clock_cycles); - -/** - * \brief Retrieve the amount of clock cycles in a tick - * - * This function retrieves how many clock cycles there are in a single timer tick. - * It checks if the given hardware is initialized. - * - * \param[in] descr The timer descriptor of a timer to convert ticks to - * clock cycles - * \param[out] cycles The amount of clock cycles - * - * \return The status of clock cycles retrieving. - */ -int32_t timer_get_clock_cycles_in_tick(const struct timer_descriptor *const descr, uint32_t *const cycles); - -/** - * \brief Add timer task - * - * This function adds the given timer task to the given timer. - * It checks if the given hardware is initialized. - * - * \param[in] descr The timer descriptor of a timer to add task to - * \param[in] task A task to add - * - * \return Timer's task adding status. - */ -int32_t timer_add_task(struct timer_descriptor *const descr, struct timer_task *const task); - -/** - * \brief Remove timer task - * - * This function removes the given timer task from the given timer. - * It checks if the given hardware is initialized. - * - * \param[in] descr The timer descriptor of a timer to remove task from - * \param[in] task A task to remove - * - * \return Timer's task removing status. - */ -int32_t timer_remove_task(struct timer_descriptor *const descr, const struct timer_task *const task); - -/** - * \brief Retrieve the current driver version - * - * \return Current driver version. - */ -uint32_t timer_get_version(void); -/**@}*/ - -#ifdef __cplusplus -} -#endif - -#endif /* _HAL_TIMER_H_INCLUDED */ diff --git a/2_Motor_Master/Motor_Master/Motor_Master/hal/src/hal_timer.c b/2_Motor_Master/Motor_Master/Motor_Master/hal/src/hal_timer.c deleted file mode 100644 index 565c6db..0000000 --- a/2_Motor_Master/Motor_Master/Motor_Master/hal/src/hal_timer.c +++ /dev/null @@ -1,250 +0,0 @@ -/** - * \file - * - * \brief Timer functionality implementation. - * - * Copyright (c) 2014-2018 Microchip Technology Inc. and its subsidiaries. - * - * \asf_license_start - * - * \page License - * - * Subject to your compliance with these terms, you may use Microchip - * software and any derivatives exclusively with Microchip products. - * It is your responsibility to comply with third party license terms applicable - * to your use of third party software (including open source software) that - * may accompany Microchip software. - * - * THIS SOFTWARE IS SUPPLIED BY MICROCHIP "AS IS". NO WARRANTIES, - * WHETHER EXPRESS, IMPLIED OR STATUTORY, APPLY TO THIS SOFTWARE, - * INCLUDING ANY IMPLIED WARRANTIES OF NON-INFRINGEMENT, MERCHANTABILITY, - * AND FITNESS FOR A PARTICULAR PURPOSE. IN NO EVENT WILL MICROCHIP BE - * LIABLE FOR ANY INDIRECT, SPECIAL, PUNITIVE, INCIDENTAL OR CONSEQUENTIAL - * LOSS, DAMAGE, COST OR EXPENSE OF ANY KIND WHATSOEVER RELATED TO THE - * SOFTWARE, HOWEVER CAUSED, EVEN IF MICROCHIP HAS BEEN ADVISED OF THE - * POSSIBILITY OR THE DAMAGES ARE FORESEEABLE. TO THE FULLEST EXTENT - * ALLOWED BY LAW, MICROCHIP'S TOTAL LIABILITY ON ALL CLAIMS IN ANY WAY - * RELATED TO THIS SOFTWARE WILL NOT EXCEED THE AMOUNT OF FEES, IF ANY, - * THAT YOU HAVE PAID DIRECTLY TO MICROCHIP FOR THIS SOFTWARE. - * - * \asf_license_stop - * - */ - -#include "hal_timer.h" -#include -#include -#include -#include - -/** - * \brief Driver version - */ -#define DRIVER_VERSION 0x00000001u - -/** - * \brief Timer flags - */ -#define TIMER_FLAG_QUEUE_IS_TAKEN 1 -#define TIMER_FLAG_INTERRUPT_TRIGERRED 2 - -static void timer_add_timer_task(struct list_descriptor *list, struct timer_task *const new_task, const uint32_t time); -static void timer_process_counted(struct _timer_device *device); - -/** - * \brief Initialize timer - */ -int32_t timer_init(struct timer_descriptor *const descr, void *const hw, struct _timer_hpl_interface *const func) -{ - ASSERT(descr && hw); - _timer_init(&descr->device, hw); - descr->time = 0; - descr->device.timer_cb.period_expired = timer_process_counted; - - return ERR_NONE; -} - -/** - * \brief Deinitialize timer - */ -int32_t timer_deinit(struct timer_descriptor *const descr) -{ - ASSERT(descr); - _timer_deinit(&descr->device); - - return ERR_NONE; -} - -/** - * \brief Start timer - */ -int32_t timer_start(struct timer_descriptor *const descr) -{ - ASSERT(descr); - if (_timer_is_started(&descr->device)) { - return ERR_DENIED; - } - _timer_start(&descr->device); - - return ERR_NONE; -} - -/** - * \brief Stop timer - */ -int32_t timer_stop(struct timer_descriptor *const descr) -{ - ASSERT(descr); - if (!_timer_is_started(&descr->device)) { - return ERR_DENIED; - } - _timer_stop(&descr->device); - - return ERR_NONE; -} - -/** - * \brief Set amount of clock cycler per timer tick - */ -int32_t timer_set_clock_cycles_per_tick(struct timer_descriptor *const descr, const uint32_t clock_cycles) -{ - ASSERT(descr); - _timer_set_period(&descr->device, clock_cycles); - - return ERR_NONE; -} - -/** - * \brief Add timer task - */ -int32_t timer_add_task(struct timer_descriptor *const descr, struct timer_task *const task) -{ - ASSERT(descr && task); - - descr->flags |= TIMER_FLAG_QUEUE_IS_TAKEN; - if (is_list_element(&descr->tasks, task)) { - descr->flags &= ~TIMER_FLAG_QUEUE_IS_TAKEN; - ASSERT(false); - return ERR_ALREADY_INITIALIZED; - } - task->time_label = descr->time; - timer_add_timer_task(&descr->tasks, task, descr->time); - - descr->flags &= ~TIMER_FLAG_QUEUE_IS_TAKEN; - if (descr->flags & TIMER_FLAG_INTERRUPT_TRIGERRED) { - CRITICAL_SECTION_ENTER() - descr->flags &= ~TIMER_FLAG_INTERRUPT_TRIGERRED; - _timer_set_irq(&descr->device); - CRITICAL_SECTION_LEAVE() - } - - return ERR_NONE; -} - -/** - * \brief Remove timer task - */ -int32_t timer_remove_task(struct timer_descriptor *const descr, const struct timer_task *const task) -{ - ASSERT(descr && task); - - descr->flags |= TIMER_FLAG_QUEUE_IS_TAKEN; - if (!is_list_element(&descr->tasks, task)) { - descr->flags &= ~TIMER_FLAG_QUEUE_IS_TAKEN; - ASSERT(false); - return ERR_NOT_FOUND; - } - list_delete_element(&descr->tasks, task); - - descr->flags &= ~TIMER_FLAG_QUEUE_IS_TAKEN; - if (descr->flags & TIMER_FLAG_INTERRUPT_TRIGERRED) { - CRITICAL_SECTION_ENTER() - descr->flags &= ~TIMER_FLAG_INTERRUPT_TRIGERRED; - _timer_set_irq(&descr->device); - CRITICAL_SECTION_LEAVE() - } - - return ERR_NONE; -} - -/** - * \brief Retrieve the amount of clock cycles in a tick - */ -int32_t timer_get_clock_cycles_in_tick(const struct timer_descriptor *const descr, uint32_t *const cycles) -{ - ASSERT(descr && cycles); - *cycles = _timer_get_period(&descr->device); - return ERR_NONE; -} - -/** - * \brief Retrieve the current driver version - */ -uint32_t timer_get_version(void) -{ - return DRIVER_VERSION; -} - -/** - * \internal Insert a timer task into sorted timer's list - * - * \param[in] head The pointer to the head of timer task list - * \param[in] task The pointer to task to add - * \param[in] time Current timer time - */ -static void timer_add_timer_task(struct list_descriptor *list, struct timer_task *const new_task, const uint32_t time) -{ - struct timer_task *it, *prev = NULL, *head = (struct timer_task *)list_get_head(list); - - if (!head) { - list_insert_as_head(list, new_task); - return; - } - - for (it = head; it; it = (struct timer_task *)list_get_next_element(it)) { - uint32_t time_left; - - if (it->time_label <= time) { - time_left = it->interval - (time - it->time_label); - } else { - time_left = it->interval - (0xFFFFFFFF - it->time_label) - time; - } - if (time_left >= new_task->interval) - break; - prev = it; - } - - if (it == head) { - list_insert_as_head(list, new_task); - } else { - list_insert_after(prev, new_task); - } -} - -/** - * \internal Process interrupts - */ -static void timer_process_counted(struct _timer_device *device) -{ - struct timer_descriptor *timer = CONTAINER_OF(device, struct timer_descriptor, device); - struct timer_task * it = (struct timer_task *)list_get_head(&timer->tasks); - uint32_t time = ++timer->time; - - if ((timer->flags & TIMER_FLAG_QUEUE_IS_TAKEN) || (timer->flags & TIMER_FLAG_INTERRUPT_TRIGERRED)) { - timer->flags |= TIMER_FLAG_INTERRUPT_TRIGERRED; - return; - } - - while (it && ((time - it->time_label) >= it->interval)) { - struct timer_task *tmp = it; - - list_remove_head(&timer->tasks); - if (TIMER_TASK_REPEAT == tmp->mode) { - tmp->time_label = time; - timer_add_timer_task(&timer->tasks, tmp, time); - } - it = (struct timer_task *)list_get_head(&timer->tasks); - - tmp->cb(tmp); - } -} diff --git a/2_Motor_Master/Motor_Master/Motor_Master/hpl/dmac/hpl_dmac.c b/2_Motor_Master/Motor_Master/Motor_Master/hpl/dmac/hpl_dmac.c index 7dddc9d..2806d0c 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/hpl/dmac/hpl_dmac.c +++ b/2_Motor_Master/Motor_Master/Motor_Master/hpl/dmac/hpl_dmac.c @@ -215,10 +215,11 @@ static void _dmac_handler(void) uint8_t channel = hri_dmac_get_INTPEND_reg(DMAC, DMAC_INTPEND_ID_Msk); struct _dma_resource *tmp_resource = &_resources[channel]; - if (hri_dmac_get_INTPEND_TERR_bit(DMAC)) { + if (hri_dmac_get_CHINTFLAG_TERR_bit(DMAC, channel)) { hri_dmac_clear_CHINTFLAG_TERR_bit(DMAC, channel); tmp_resource->dma_cb.error(tmp_resource); - } else if (hri_dmac_get_INTPEND_TCMPL_bit(DMAC)) { + } else if (hri_dmac_get_CHINTFLAG_TCMPL_bit(DMAC, channel)) { + //hri_dmac_get_CHINTFLAG_TCMPL_bit(DMAC, channel); /********* ADDED **************/ hri_dmac_clear_CHINTFLAG_TCMPL_bit(DMAC, channel); tmp_resource->dma_cb.transfer_done(tmp_resource); } diff --git a/2_Motor_Master/Motor_Master/Motor_Master/hpl/tc/hpl_tc.c b/2_Motor_Master/Motor_Master/Motor_Master/hpl/tc/hpl_tc.c deleted file mode 100644 index 5b9eccc..0000000 --- a/2_Motor_Master/Motor_Master/Motor_Master/hpl/tc/hpl_tc.c +++ /dev/null @@ -1,357 +0,0 @@ - -/** - * \file - * - * \brief SAM TC - * - * Copyright (c) 2014-2018 Microchip Technology Inc. and its subsidiaries. - * - * \asf_license_start - * - * \page License - * - * Subject to your compliance with these terms, you may use Microchip - * software and any derivatives exclusively with Microchip products. - * It is your responsibility to comply with third party license terms applicable - * to your use of third party software (including open source software) that - * may accompany Microchip software. - * - * THIS SOFTWARE IS SUPPLIED BY MICROCHIP "AS IS". NO WARRANTIES, - * WHETHER EXPRESS, IMPLIED OR STATUTORY, APPLY TO THIS SOFTWARE, - * INCLUDING ANY IMPLIED WARRANTIES OF NON-INFRINGEMENT, MERCHANTABILITY, - * AND FITNESS FOR A PARTICULAR PURPOSE. IN NO EVENT WILL MICROCHIP BE - * LIABLE FOR ANY INDIRECT, SPECIAL, PUNITIVE, INCIDENTAL OR CONSEQUENTIAL - * LOSS, DAMAGE, COST OR EXPENSE OF ANY KIND WHATSOEVER RELATED TO THE - * SOFTWARE, HOWEVER CAUSED, EVEN IF MICROCHIP HAS BEEN ADVISED OF THE - * POSSIBILITY OR THE DAMAGES ARE FORESEEABLE. TO THE FULLEST EXTENT - * ALLOWED BY LAW, MICROCHIP'S TOTAL LIABILITY ON ALL CLAIMS IN ANY WAY - * RELATED TO THIS SOFTWARE WILL NOT EXCEED THE AMOUNT OF FEES, IF ANY, - * THAT YOU HAVE PAID DIRECTLY TO MICROCHIP FOR THIS SOFTWARE. - * - * \asf_license_stop - * - */ - -#include -#include -#include -#include -#include -#include - -#ifndef CONF_TC0_ENABLE -#define CONF_TC0_ENABLE 0 -#endif -#ifndef CONF_TC1_ENABLE -#define CONF_TC1_ENABLE 0 -#endif -#ifndef CONF_TC2_ENABLE -#define CONF_TC2_ENABLE 0 -#endif -#ifndef CONF_TC3_ENABLE -#define CONF_TC3_ENABLE 0 -#endif -#ifndef CONF_TC4_ENABLE -#define CONF_TC4_ENABLE 0 -#endif -#ifndef CONF_TC5_ENABLE -#define CONF_TC5_ENABLE 0 -#endif -#ifndef CONF_TC6_ENABLE -#define CONF_TC6_ENABLE 0 -#endif -#ifndef CONF_TC7_ENABLE -#define CONF_TC7_ENABLE 0 -#endif - -/** - * \brief Macro is used to fill usart configuration structure based on its - * number - * - * \param[in] n The number of structures - */ -#define TC_CONFIGURATION(n) \ - { \ - n, TC##n##_IRQn, \ - TC_CTRLA_MODE(CONF_TC##n##_MODE) | TC_CTRLA_PRESCSYNC(CONF_TC##n##_PRESCSYNC) \ - | (CONF_TC##n##_RUNSTDBY << TC_CTRLA_RUNSTDBY_Pos) | (CONF_TC##n##_ONDEMAND << TC_CTRLA_ONDEMAND_Pos) \ - | TC_CTRLA_PRESCALER(CONF_TC##n##_PRESCALER) | (CONF_TC##n##_ALOCK << TC_CTRLA_ALOCK_Pos), \ - (CONF_TC##n##_OVFEO << TC_EVCTRL_OVFEO_Pos) | (CONF_TC##n##_TCEI << TC_EVCTRL_TCEI_Pos) \ - | (CONF_TC##n##_TCINV << TC_EVCTRL_TCINV_Pos) | (CONF_TC##n##_EVACT << TC_EVCTRL_EVACT_Pos) \ - | (CONF_TC##n##_MCEO0 << TC_EVCTRL_MCEO0_Pos) | (CONF_TC##n##_MCEO1 << TC_EVCTRL_MCEO1_Pos), \ - (CONF_TC##n##_DBGRUN << TC_DBGCTRL_DBGRUN_Pos), CONF_TC##n##_PER, CONF_TC##n##_CC0, CONF_TC##n##_CC1, \ - } -/** - * \brief TC configuration type - */ -struct tc_configuration { - uint8_t number; - IRQn_Type irq; - hri_tc_ctrla_reg_t ctrl_a; - hri_tc_evctrl_reg_t event_ctrl; - hri_tc_dbgctrl_reg_t dbg_ctrl; - hri_tccount8_per_reg_t per; - hri_tccount32_cc_reg_t cc0; - hri_tccount32_cc_reg_t cc1; -}; - -/** - * \brief Array of TC configurations - */ -static struct tc_configuration _tcs[] = { -#if CONF_TC0_ENABLE == 1 - TC_CONFIGURATION(0), -#endif -#if CONF_TC1_ENABLE == 1 - TC_CONFIGURATION(1), -#endif -#if CONF_TC2_ENABLE == 1 - TC_CONFIGURATION(2), -#endif -#if CONF_TC3_ENABLE == 1 - TC_CONFIGURATION(3), -#endif -#if CONF_TC4_ENABLE == 1 - TC_CONFIGURATION(4), -#endif -#if CONF_TC5_ENABLE == 1 - TC_CONFIGURATION(5), -#endif -#if CONF_TC6_ENABLE == 1 - TC_CONFIGURATION(6), -#endif -#if CONF_TC7_ENABLE == 1 - TC_CONFIGURATION(7), -#endif -}; - -static struct _timer_device *_tc0_dev = NULL; - -static struct _pwm_device *_tc2_dev = NULL; - -static struct _pwm_device *_tc4_dev = NULL; - -static int8_t get_tc_index(const void *const hw); -static void _tc_init_irq_param(const void *const hw, void *dev); -static inline uint8_t _get_hardware_offset(const void *const hw); -/** - * \brief Initialize TC - */ -int32_t _timer_init(struct _timer_device *const device, void *const hw) -{ - int8_t i = get_tc_index(hw); - - device->hw = hw; - ASSERT(ARRAY_SIZE(_tcs)); - - if (!hri_tc_is_syncing(hw, TC_SYNCBUSY_SWRST)) { - if (hri_tc_get_CTRLA_reg(hw, TC_CTRLA_ENABLE)) { - hri_tc_clear_CTRLA_ENABLE_bit(hw); - hri_tc_wait_for_sync(hw, TC_SYNCBUSY_ENABLE); - } - hri_tc_write_CTRLA_reg(hw, TC_CTRLA_SWRST); - } - hri_tc_wait_for_sync(hw, TC_SYNCBUSY_SWRST); - - hri_tc_write_CTRLA_reg(hw, _tcs[i].ctrl_a); - hri_tc_write_DBGCTRL_reg(hw, _tcs[i].dbg_ctrl); - hri_tc_write_EVCTRL_reg(hw, _tcs[i].event_ctrl); - hri_tc_write_WAVE_reg(hw, TC_WAVE_WAVEGEN_MFRQ); - - if ((_tcs[i].ctrl_a & TC_CTRLA_MODE_Msk) == TC_CTRLA_MODE_COUNT32) { - hri_tccount32_write_CC_reg(hw, 0, _tcs[i].cc0); - hri_tccount32_write_CC_reg(hw, 1, _tcs[i].cc1); - - } else if ((_tcs[i].ctrl_a & TC_CTRLA_MODE_Msk) == TC_CTRLA_MODE_COUNT16) { - hri_tccount16_write_CC_reg(hw, 0, (uint16_t)_tcs[i].cc0); - hri_tccount16_write_CC_reg(hw, 1, (uint16_t)_tcs[i].cc1); - - } else if ((_tcs[i].ctrl_a & TC_CTRLA_MODE_Msk) == TC_CTRLA_MODE_COUNT8) { - hri_tccount8_write_CC_reg(hw, 0, (uint8_t)_tcs[i].cc0); - hri_tccount8_write_CC_reg(hw, 1, (uint8_t)_tcs[i].cc1); - hri_tccount8_write_PER_reg(hw, _tcs[i].per); - } - hri_tc_set_INTEN_OVF_bit(hw); - - _tc_init_irq_param(hw, (void *)device); - NVIC_DisableIRQ(_tcs[i].irq); - NVIC_ClearPendingIRQ(_tcs[i].irq); - NVIC_EnableIRQ(_tcs[i].irq); - - return ERR_NONE; -} -/** - * \brief De-initialize TC - */ -void _timer_deinit(struct _timer_device *const device) -{ - void *const hw = device->hw; - int8_t i = get_tc_index(hw); - ASSERT(ARRAY_SIZE(_tcs)); - - NVIC_DisableIRQ(_tcs[i].irq); - - hri_tc_clear_CTRLA_ENABLE_bit(hw); - hri_tc_set_CTRLA_SWRST_bit(hw); -} -/** - * \brief Start hardware timer - */ -void _timer_start(struct _timer_device *const device) -{ - hri_tc_set_CTRLA_ENABLE_bit(device->hw); -} -/** - * \brief Stop hardware timer - */ -void _timer_stop(struct _timer_device *const device) -{ - hri_tc_clear_CTRLA_ENABLE_bit(device->hw); -} -/** - * \brief Set timer period - */ -void _timer_set_period(struct _timer_device *const device, const uint32_t clock_cycles) -{ - void *const hw = device->hw; - - if (TC_CTRLA_MODE_COUNT32_Val == hri_tc_read_CTRLA_MODE_bf(hw)) { - hri_tccount32_write_CC_reg(hw, 0, clock_cycles); - } else if (TC_CTRLA_MODE_COUNT16_Val == hri_tc_read_CTRLA_MODE_bf(hw)) { - hri_tccount16_write_CC_reg(hw, 0, (uint16_t)clock_cycles); - } else if (TC_CTRLA_MODE_COUNT8_Val == hri_tc_read_CTRLA_MODE_bf(hw)) { - hri_tccount8_write_PER_reg(hw, clock_cycles); - } -} -/** - * \brief Retrieve timer period - */ -uint32_t _timer_get_period(const struct _timer_device *const device) -{ - void *const hw = device->hw; - - if (TC_CTRLA_MODE_COUNT32_Val == hri_tc_read_CTRLA_MODE_bf(hw)) { - return hri_tccount32_read_CC_reg(hw, 0); - } else if (TC_CTRLA_MODE_COUNT16_Val == hri_tc_read_CTRLA_MODE_bf(hw)) { - return hri_tccount16_read_CC_reg(hw, 0); - } else if (TC_CTRLA_MODE_COUNT8_Val == hri_tc_read_CTRLA_MODE_bf(hw)) { - return hri_tccount8_read_PER_reg(hw); - } - - return 0; -} -/** - * \brief Check if timer is running - */ -bool _timer_is_started(const struct _timer_device *const device) -{ - return hri_tc_get_CTRLA_ENABLE_bit(device->hw); -} - -/** - * \brief Retrieve timer helper functions - */ -struct _timer_hpl_interface *_tc_get_timer(void) -{ - return NULL; -} - -/** - * \brief Retrieve pwm helper functions - */ -struct _pwm_hpl_interface *_tc_get_pwm(void) -{ - return NULL; -} -/** - * \brief Set timer IRQ - * - * \param[in] hw The pointer to hardware instance - */ -void _timer_set_irq(struct _timer_device *const device) -{ - void *const hw = device->hw; - int8_t i = get_tc_index(hw); - ASSERT(ARRAY_SIZE(_tcs)); - - _irq_set(_tcs[i].irq); -} -/** - * \internal TC interrupt handler for Timer - * - * \param[in] instance TC instance number - */ -static void tc_interrupt_handler(struct _timer_device *device) -{ - void *const hw = device->hw; - - if (hri_tc_get_interrupt_OVF_bit(hw)) { - hri_tc_clear_interrupt_OVF_bit(hw); - device->timer_cb.period_expired(device); - } -} - -/** - * \brief TC interrupt handler - */ -void TC0_Handler(void) -{ - tc_interrupt_handler(_tc0_dev); -} - -/** - * \internal Retrieve TC index - * - * \param[in] hw The pointer to hardware instance - * - * \return The index of TC configuration - */ -static int8_t get_tc_index(const void *const hw) -{ - uint8_t index = _get_hardware_offset(hw); - uint8_t i; - - for (i = 0; i < ARRAY_SIZE(_tcs); i++) { - if (_tcs[i].number == index) { - return i; - } - } - - ASSERT(false); - return -1; -} - -/** - * \brief Init irq param with the given tc hardware instance - */ -static void _tc_init_irq_param(const void *const hw, void *dev) -{ - if (hw == TC0) { - _tc0_dev = (struct _timer_device *)dev; - } - if (hw == TC2) { - _tc2_dev = (struct _pwm_device *)dev; - } - if (hw == TC4) { - _tc4_dev = (struct _pwm_device *)dev; - } -} - -/** - * \internal Retrieve TC hardware index - * - * \param[in] hw The pointer to hardware instance - */ -static inline uint8_t _get_hardware_offset(const void *const hw) -{ - /* List of available TC modules. */ - Tc *const tc_modules[TC_INST_NUM] = TC_INSTS; - - /* Find index for TC instance. */ - for (uint32_t i = 0; i < TC_INST_NUM; i++) { - if ((uint32_t)hw == (uint32_t)tc_modules[i]) { - return i; - } - } - return 0; -} diff --git a/2_Motor_Master/Motor_Master/Motor_Master/hpl/tc/hpl_tc_base.h b/2_Motor_Master/Motor_Master/Motor_Master/hpl/tc/hpl_tc_base.h deleted file mode 100644 index ae77c90..0000000 --- a/2_Motor_Master/Motor_Master/Motor_Master/hpl/tc/hpl_tc_base.h +++ /dev/null @@ -1,77 +0,0 @@ -/** - * \file - * - * \brief SAM Timer/Counter - * - * Copyright (c) 2014-2018 Microchip Technology Inc. and its subsidiaries. - * - * \asf_license_start - * - * \page License - * - * Subject to your compliance with these terms, you may use Microchip - * software and any derivatives exclusively with Microchip products. - * It is your responsibility to comply with third party license terms applicable - * to your use of third party software (including open source software) that - * may accompany Microchip software. - * - * THIS SOFTWARE IS SUPPLIED BY MICROCHIP "AS IS". NO WARRANTIES, - * WHETHER EXPRESS, IMPLIED OR STATUTORY, APPLY TO THIS SOFTWARE, - * INCLUDING ANY IMPLIED WARRANTIES OF NON-INFRINGEMENT, MERCHANTABILITY, - * AND FITNESS FOR A PARTICULAR PURPOSE. IN NO EVENT WILL MICROCHIP BE - * LIABLE FOR ANY INDIRECT, SPECIAL, PUNITIVE, INCIDENTAL OR CONSEQUENTIAL - * LOSS, DAMAGE, COST OR EXPENSE OF ANY KIND WHATSOEVER RELATED TO THE - * SOFTWARE, HOWEVER CAUSED, EVEN IF MICROCHIP HAS BEEN ADVISED OF THE - * POSSIBILITY OR THE DAMAGES ARE FORESEEABLE. TO THE FULLEST EXTENT - * ALLOWED BY LAW, MICROCHIP'S TOTAL LIABILITY ON ALL CLAIMS IN ANY WAY - * RELATED TO THIS SOFTWARE WILL NOT EXCEED THE AMOUNT OF FEES, IF ANY, - * THAT YOU HAVE PAID DIRECTLY TO MICROCHIP FOR THIS SOFTWARE. - * - * \asf_license_stop - */ - -#ifndef _HPL_TC_BASE_H_INCLUDED -#define _HPL_TC_BASE_H_INCLUDED - -#include -#include - -#ifdef __cplusplus -extern "C" { -#endif - -/** - * \addtogroup tc_group TC Hardware Proxy Layer - * - * \section tc_hpl_rev Revision History - * - v0.0.0.1 Initial Commit - * - *@{ - */ - -/** - * \name HPL functions - */ -//@{ - -/** - * \brief Retrieve timer helper functions - * - * \return A pointer to set of timer helper functions - */ -struct _timer_hpl_interface *_tc_get_timer(void); - -/** - * \brief Retrieve pwm helper functions - * - * \return A pointer to set of pwm helper functions - */ -struct _pwm_hpl_interface *_tc_get_pwm(void); - -//@} -/**@}*/ - -#ifdef __cplusplus -} -#endif -#endif /* _HPL_TC_BASE_H_INCLUDED */ diff --git a/2_Motor_Master/Motor_Master/Motor_Master/hpl/tc/tc_lite.c b/2_Motor_Master/Motor_Master/Motor_Master/hpl/tc/tc_lite.c index 90d6d1a..9409c3e 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/hpl/tc/tc_lite.c +++ b/2_Motor_Master/Motor_Master/Motor_Master/hpl/tc/tc_lite.c @@ -34,6 +34,74 @@ #include "tc_lite.h" +/** + * \brief Initialize TC interface + */ +int8_t TIMER_0_init() +{ + + if (!hri_tc_is_syncing(TC0, TC_SYNCBUSY_SWRST)) { + if (hri_tc_get_CTRLA_reg(TC0, TC_CTRLA_ENABLE)) { + hri_tc_clear_CTRLA_ENABLE_bit(TC0); + hri_tc_wait_for_sync(TC0, TC_SYNCBUSY_ENABLE); + } + hri_tc_write_CTRLA_reg(TC0, TC_CTRLA_SWRST); + } + hri_tc_wait_for_sync(TC0, TC_SYNCBUSY_SWRST); + + hri_tc_write_CTRLA_reg(TC0, + 0 << TC_CTRLA_CAPTMODE0_Pos /* Capture mode Channel 0: 0 */ + | 0 << TC_CTRLA_CAPTMODE1_Pos /* Capture mode Channel 1: 0 */ + | 0 << TC_CTRLA_COPEN0_Pos /* Capture Pin 0 Enable: disabled */ + | 0 << TC_CTRLA_COPEN1_Pos /* Capture Pin 1 Enable: disabled */ + | 0 << TC_CTRLA_CAPTEN0_Pos /* Capture Channel 0 Enable: disabled */ + | 0 << TC_CTRLA_CAPTEN1_Pos /* Capture Channel 1 Enable: disabled */ + | 0 << TC_CTRLA_ALOCK_Pos /* Auto Lock: disabled */ + | 0 << TC_CTRLA_PRESCSYNC_Pos /* Prescaler and Counter Synchronization: 0 */ + | 0 << TC_CTRLA_ONDEMAND_Pos /* Clock On Demand: disabled */ + | 0 << TC_CTRLA_RUNSTDBY_Pos /* Run in Standby: disabled */ + | 5 << TC_CTRLA_PRESCALER_Pos /* Setting: 5 */ + | 0x0 << TC_CTRLA_MODE_Pos); /* Operating Mode: 0x0 */ + + hri_tc_write_CTRLB_reg(TC0, + 0 << TC_CTRLBSET_CMD_Pos /* Command: 0 */ + | 0 << TC_CTRLBSET_ONESHOT_Pos /* One-Shot: disabled */ + | 0 << TC_CTRLBCLR_LUPD_Pos /* Setting: disabled */ + | 0 << TC_CTRLBSET_DIR_Pos); /* Counter Direction: disabled */ + + hri_tc_write_WAVE_reg(TC0,1); /* Waveform Generation Mode: 0 */ + + // hri_tc_write_DRVCTRL_reg(TC0,0 << TC_DRVCTRL_INVEN1_Pos /* Output Waveform 1 Invert Enable: disabled */ + // | 0 << TC_DRVCTRL_INVEN0_Pos); /* Output Waveform 0 Invert Enable: disabled */ + + // hri_tc_write_DBGCTRL_reg(TC0,0); /* Run in debug: 0 */ + + hri_tccount16_write_CC_reg(TC0, 0, 0x752); /* Compare/Capture Value: 0x752 */ + + // hri_tccount16_write_CC_reg(TC0, 1 ,0x0); /* Compare/Capture Value: 0x0 */ + + // hri_tccount16_write_COUNT_reg(TC0,0x0); /* Counter Value: 0x0 */ + + hri_tc_write_EVCTRL_reg( + TC0, + 1 << TC_EVCTRL_MCEO0_Pos /* Match or Capture Channel 0 Event Output Enable: enabled */ + | 0 << TC_EVCTRL_MCEO1_Pos /* Match or Capture Channel 1 Event Output Enable: disabled */ + | 0 << TC_EVCTRL_OVFEO_Pos /* Overflow/Underflow Event Output Enable: disabled */ + | 0 << TC_EVCTRL_TCEI_Pos /* TC Event Input: disabled */ + | 0 << TC_EVCTRL_TCINV_Pos /* TC Inverted Event Input: disabled */ + | 0); /* Event Action: 0 */ + + hri_tc_write_INTEN_reg(TC0, + 1 << TC_INTENSET_MC0_Pos /* Match or Capture Channel 0 Interrupt Enable: enabled */ + | 0 << TC_INTENSET_MC1_Pos /* Match or Capture Channel 1 Interrupt Enable: disabled */ + | 0 << TC_INTENSET_ERR_Pos /* Error Interrupt Enable: disabled */ + | 0 << TC_INTENSET_OVF_Pos); /* Overflow Interrupt enable: disabled */ + + hri_tc_write_CTRLA_ENABLE_bit(TC0, 1 << TC_CTRLA_ENABLE_Pos); /* Enable: enabled */ + + return 0; +} + /** * \brief Initialize TC interface */ diff --git a/2_Motor_Master/Motor_Master/Motor_Master/hpl/tc/tc_lite.h b/2_Motor_Master/Motor_Master/Motor_Master/hpl/tc/tc_lite.h index 1a02bba..e72aa75 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/hpl/tc/tc_lite.h +++ b/2_Motor_Master/Motor_Master/Motor_Master/hpl/tc/tc_lite.h @@ -51,6 +51,12 @@ extern "C" { #endif +/** + * \brief Initialize tc interface + * \return Initialization status. + */ +int8_t TIMER_0_init(); + /** * \brief Initialize tc interface * \return Initialization status. diff --git a/2_Motor_Master/Motor_Master/Motor_Master/interrupts.h b/2_Motor_Master/Motor_Master/Motor_Master/interrupts.h index b3df411..3e04cf8 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/interrupts.h +++ b/2_Motor_Master/Motor_Master/Motor_Master/interrupts.h @@ -37,7 +37,7 @@ static void b2bTransferComplete_cb(struct _dma_resource *resource) { PORT->Group[1].OUTSET.reg = (1<Group[1].OUTSET.reg = (1<Group[GPIO_PORTB].OUTCLR.reg = (1<SS_pin); //gpio_set_pin_level(SPI1_CS, true); } diff --git a/2_Motor_Master/Motor_Master/Motor_Master/main.c b/2_Motor_Master/Motor_Master/Motor_Master/main.c index c502d57..92c7597 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/main.c +++ b/2_Motor_Master/Motor_Master/Motor_Master/main.c @@ -48,19 +48,13 @@ void process_currents() Motor2.timerflags.current_loop_tic = true; } -/** - * Example of using TIMER_0. - */ - -static struct timer_task Onems_task; - -void One_ms_timer_init(void) -{ - Onems_task.interval = 1; - Onems_task.cb = One_ms_cycle_callback; - Onems_task.mode = TIMER_TASK_REPEAT; - timer_add_task(&TIMER_0, &Onems_task); - timer_start(&TIMER_0); +void TC0_Handler( void ){ + if (TC0->COUNT16.INTFLAG.bit.MC0 == 0x01){ + TC0->COUNT16.INTFLAG.bit.MC0 = 0x01; + Motor1.timerflags.motor_telemetry_flag = true; + + + } } void enable_NVIC_IRQ(void) @@ -72,11 +66,15 @@ void enable_NVIC_IRQ(void) //NVIC_EnableIRQ(TC4_IRQn); // TC4: M2_Speed_Timer NVIC_EnableIRQ(DMAC_0_IRQn); NVIC_EnableIRQ(DMAC_1_IRQn); - NVIC_SetPriority(DMAC_0_IRQn, 1); - NVIC_SetPriority(ADC1_0_IRQn, 2); + NVIC_SetPriority(DMAC_0_IRQn, 2); + NVIC_SetPriority(ADC1_0_IRQn, 3); NVIC_EnableIRQ(TCC0_0_IRQn); NVIC_EnableIRQ(TCC1_0_IRQn); NVIC_EnableIRQ(EIC_2_IRQn); + NVIC_EnableIRQ(SERCOM1_1_IRQn); + NVIC_SetPriority(SERCOM1_1_IRQn, 1); + NVIC_EnableIRQ(TC0_IRQn); + //NVIC_EnableIRQ(TC0_IRQn); //NVIC_SetPriority(EIC_2_IRQn, 3); //NVIC_SetPriority(TCC0_0_IRQn, 3); //NVIC_EnableIRQ(EIC_5_IRQn); @@ -165,26 +163,31 @@ int main(void) adc_init_dma(); ECAT_STATE_MACHINE(); - One_ms_timer_init(); custom_logic_enable(); //angle_sensor_init(); //initialize_ads(); /* External IRQ Config */ - __enable_irq(); enable_NVIC_IRQ(); + __enable_irq(); + //ext_irq_register(GPIO_PIN(ADS_DATA_RDY), ADS1299_dataReadyISR); //ADS1299_START(); /* Replace with your application code */ while (1) { - if (Motor1.timerflags.adc_readings_ready_tic) {process_currents();} + //if (Motor1.timerflags.adc_readings_ready_tic) {process_currents();} if (Motor1.timerflags.motor_telemetry_flag) { Motor1.timerflags.motor_telemetry_flag = false; update_telemetry(); update_setpoints(); - PORT->Group[1].OUTCLR.reg = (1<Channel[DMAC_CHANNEL_CONF_SERCOM_1_TRANSMIT].CHSTATUS.bit.PEND == true) + { + volatile int x = 0; + } + //PORT->Group[1].OUTCLR.reg = (1<Channel[DMAC_CHANNEL_CONF_SERCOM_1_RECEIVE].CHCTRLA.reg |= DMAC_CHCTRLA_ENABLE; DMAC->Channel[DMAC_CHANNEL_CONF_SERCOM_1_TRANSMIT].CHCTRLA.reg |= DMAC_CHCTRLA_ENABLE; //_dma_enable_transaction(DMAC_CHANNEL_CONF_SERCOM_1_RECEIVE, false); @@ -205,10 +208,11 @@ int main(void) } if (Motor1.timerflags.current_loop_tic) { - Motor1.timerflags.current_loop_tic = false; - APPLICATION_StateMachine(); - exec_commutation(&Motor1); - exec_commutation(&Motor2); + + Motor1.timerflags.current_loop_tic = false; + APPLICATION_StateMachine(); + exec_commutation(&Motor1); + exec_commutation(&Motor2); } if (ADS1299.data_ReadyFlag){ diff --git a/2_Motor_Master/Motor_Master/Motor_Master/motorparameters.h b/2_Motor_Master/Motor_Master/Motor_Master/motorparameters.h index 8491f2d..a58297e 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/motorparameters.h +++ b/2_Motor_Master/Motor_Master/Motor_Master/motorparameters.h @@ -150,12 +150,12 @@ const static BLDCMotor_param_t FH_22mm24BXTR = { .motor_LD_H = 0.003150, .motor_LQ_H = 0.003150, .motor_Flux_WB = 0.001575, - .motor_Max_Spd_RPM = 3000, + .motor_Max_Spd_RPM = 2000, .motor_MeasureRange_RPM = 3000 * 1.2, //(1.2f * MOTOR_MAX_SPD_RPM)f // give 20% headroom .motor_Max_Spd_ELEC = (3000/60)*7.0, //(MOTOR_MAX_SPD_RPM/60)*MOTOR_POLEPAIRS //.motor_Max_Current_IDC_A = 0.368, .motor_Max_Current_IDC_A = 0.180, - .controller_param.Pid_Speed.Kp = 0.00008f, + .controller_param.Pid_Speed.Kp = 0.00004f, .controller_param.Pid_Speed.Ki = 0.0000001f, //.controller_param.Pid_Speed.Ki = 0.0000001f, .controller_param.Pi_Pos.Kp = 50.0f, diff --git a/2_Motor_Master/Motor_Master/Motor_Master/statemachine.h b/2_Motor_Master/Motor_Master/Motor_Master/statemachine.h index 3bbb186..7cfc98d 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/statemachine.h +++ b/2_Motor_Master/Motor_Master/Motor_Master/statemachine.h @@ -59,9 +59,10 @@ typedef enum typedef enum { - MOTOR_NOFAULT = 0xE1, - MOTOR_HALLSENSORINVALID = 0xE2, - MOTOR_DRIVER_OVER_CURRENT = 0xE3, + MOTOR_NOFAULT = 0x0E, + MOTOR_HALLSENSORINVALID = 0xE1, + MOTOR_DRIVER_OVER_CURRENT = 0xE2, + MOTOR_CURRENTS_SENSOR = 0xE3, } MOTOR_FAULTS_t; typedef struct MOTOR_STATE diff --git a/2_Motor_Slave/Motor_Slave/Motor_Slave/.atmelstart/atmel_start_config.atstart b/2_Motor_Slave/Motor_Slave/Motor_Slave/.atmelstart/atmel_start_config.atstart index 4d78ae5..cb991df 100644 --- a/2_Motor_Slave/Motor_Slave/Motor_Slave/.atmelstart/atmel_start_config.atstart +++ b/2_Motor_Slave/Motor_Slave/Motor_Slave/.atmelstart/atmel_start_config.atstart @@ -308,7 +308,7 @@ drivers: functionality: System api: HAL:HPL:DMAC configuration: - dmac_beatsize_0: 8-bit bus transfer + dmac_beatsize_0: 32-bit bus transfer dmac_beatsize_1: 16-bit bus transfer dmac_beatsize_10: 8-bit bus transfer dmac_beatsize_11: 8-bit bus transfer @@ -331,7 +331,7 @@ drivers: dmac_beatsize_27: 8-bit bus transfer dmac_beatsize_28: 8-bit bus transfer dmac_beatsize_29: 8-bit bus transfer - dmac_beatsize_3: 8-bit bus transfer + dmac_beatsize_3: 32-bit bus transfer dmac_beatsize_30: 8-bit bus transfer dmac_beatsize_31: 8-bit bus transfer dmac_beatsize_4: 8-bit bus transfer diff --git a/2_Motor_Slave/Motor_Slave/Motor_Slave/Config/hpl_dmac_config.h b/2_Motor_Slave/Motor_Slave/Motor_Slave/Config/hpl_dmac_config.h index 2cf78b5..9a457ec 100644 --- a/2_Motor_Slave/Motor_Slave/Motor_Slave/Config/hpl_dmac_config.h +++ b/2_Motor_Slave/Motor_Slave/Motor_Slave/Config/hpl_dmac_config.h @@ -301,7 +301,7 @@ // Defines the size of one beat // dmac_beatsize_0 #ifndef CONF_DMAC_BEATSIZE_0 -#define CONF_DMAC_BEATSIZE_0 0 +#define CONF_DMAC_BEATSIZE_0 2 #endif // Block Action @@ -973,7 +973,7 @@ // Defines the size of one beat // dmac_beatsize_3 #ifndef CONF_DMAC_BEATSIZE_3 -#define CONF_DMAC_BEATSIZE_3 0 +#define CONF_DMAC_BEATSIZE_3 2 #endif // Block Action diff --git a/2_Motor_Slave/Motor_Slave/Motor_Slave/Ethercat_QSPI.c b/2_Motor_Slave/Motor_Slave/Motor_Slave/Ethercat_QSPI.c deleted file mode 100644 index f254423..0000000 --- a/2_Motor_Slave/Motor_Slave/Motor_Slave/Ethercat_QSPI.c +++ /dev/null @@ -1,218 +0,0 @@ -/* - * Ethercat_QSPI.c - * - * Created: 31/07/2021 17:52:21 - * Author: Nick-XMG - */ - -#include "Ethercat_QSPI.h" -#include -#include - -#define SQI_READ (0x0B<<16) #define SQI_WRITE (0x02<<16) #define SQI_INC (0x40<<8) #define SQI_DEC (0x80<<8) -#define ECAT_PRAM_RD_DATA 0x0000 #define ECAT_PRAM_WR_DATA 0x0020 #define HW_CFG 0x0074 #define BYTE_TEST 0x0064 #define ECAT_PRAM_RD_ADDR_LEN 0x0308 #define ECAT_PRAM_RD_CMD 0x030C #define ECAT_PRAM_WR_ADDR_LEN 0x0310 #define ECAT_PRAM_WR_CMD 0x0314 #define LAN9252_RDY (1<<27) #define PDRAM_RD_ADDRESS 0x1100 #define PDRAM_RD_LENGTH ECAT_SIZE_RD //do the math later to automatize. fixed to 64 for now. #define PDRAM_WR_ADDRESS 0x1800 #define PDRAM_WR_LENGTH ECAT_SIZE_WR #define PDRAM_LENGTH_MAX 64 -#define PDRAM_LENGTH_SHORT 32 -#define FIFO_DEPTH 16 -#define PDRAM_REG_MAX PDRAM_LENGTH_SHORT/4 -#define PRAM_X_ABORT (1<<30) -#define CSR_BUSY 0x80000000 - -COMPILER_ALIGNED(16) -DmacDescriptor dummy_rx_descriptor; -DmacDescriptor dummy_tx_descriptor; - -volatile uint32_t QSPI_tx_buffer[buffer_size]={0}; -volatile uint32_t QSPI_rx_buffer[buffer_size]={0}; - -volatile enum ecat_states ecat_state = wait; -volatile enum ecat_states next_ecat_state = wait; - -volatile uint8_t wr_cnt = 0; -volatile uint8_t rd_cnt = 0; - -static uint8_t sync_rx_buffer[2*2*buffer_size/3]={0}; -static uint8_t sync_tx_buffer[2*2*buffer_size/3]={0xED}; - - -static uint32_t QSPI_cmds[]={0,PRAM_X_ABORT,0,PRAM_X_ABORT, - ((PDRAM_LENGTH_MAX)<<16)+PDRAM_RD_ADDRESS, CSR_BUSY,(PDRAM_LENGTH_MAX<<16)+PDRAM_WR_ADDRESS,CSR_BUSY, - ((PDRAM_LENGTH_MAX)<<16)+PDRAM_RD_ADDRESS+PDRAM_LENGTH_MAX, CSR_BUSY,(PDRAM_LENGTH_MAX<<16)+PDRAM_WR_ADDRESS+PDRAM_LENGTH_MAX,CSR_BUSY, - ((PDRAM_LENGTH_MAX)<<16)+PDRAM_RD_ADDRESS+PDRAM_LENGTH_MAX*2, CSR_BUSY,(PDRAM_LENGTH_MAX<<16)+PDRAM_WR_ADDRESS+PDRAM_LENGTH_MAX*2,CSR_BUSY, - }; - -static uint32_t zero[FIFO_DEPTH]={0}; - -volatile uint32_t status = 0; - - struct _qspi_command rd_cmd = { - .inst_frame.bits.width = QSPI_INST4_ADDR4_DATA4, - .inst_frame.bits.inst_en = 0, - .inst_frame.bits.data_en = 1, - .inst_frame.bits.addr_en = 1, - .inst_frame.bits.dummy_cycles = 6, - .inst_frame.bits.tfr_type = QSPI_READMEM_ACCESS, - }; - - struct _qspi_command wr_cmd = { - .inst_frame.bits.width = QSPI_INST4_ADDR4_DATA4, - .inst_frame.bits.inst_en = 0, - .inst_frame.bits.data_en = 1, - .inst_frame.bits.addr_en = 1, - .inst_frame.bits.dummy_cycles = 0, - .inst_frame.bits.tfr_type = QSPI_WRITEMEM_ACCESS, - }; - - struct _qspi_command qspi_cmd = { - .inst_frame.bits.width = QSPI_INST1_ADDR1_DATA1, - .inst_frame.bits.inst_en = 1, - .inst_frame.bits.data_en = 0, - .inst_frame.bits.addr_en = 0, - .inst_frame.bits.dummy_cycles = 0, - .instruction = 0x38, - .inst_frame.bits.tfr_type = QSPI_WRITE_ACCESS, - }; - -struct _qspi_command *spi_cmd= &wr_cmd; -volatile uint8_t tx_complete =3; -volatile uint32_t *ECAT_BYTE_TEST= QSPI_AHB+BYTE_TEST+SQI_READ; -volatile uint32_t *ECAT_FIFO_RD_RD= QSPI_AHB+SQI_READ +ECAT_PRAM_RD_DATA; -//volatile uint32_t *ECAT_FIFO_RD_RD= QSPI_AHB+SQI_READ +SQI_INC+ECAT_PRAM_RD_DATA; - -volatile uint32_t *ECAT_FIFO_WR_WR= QSPI_AHB+SQI_WRITE+ECAT_PRAM_WR_DATA; -//volatile uint32_t *ECAT_FIFO_WR_WR= QSPI_AHB+SQI_WRITE+SQI_INC+ECAT_PRAM_WR_DATA; - -volatile uint32_t *ECAT_HW_CFG_RD= QSPI_AHB+SQI_READ+HW_CFG; -volatile uint32_t *ECAT_FIFO_RD_ADLEN_WR= QSPI_AHB+SQI_WRITE+SQI_INC+ECAT_PRAM_RD_ADDR_LEN; -volatile uint32_t *ECAT_FIFO_WR_ADLEN_WR= QSPI_AHB+SQI_WRITE+SQI_INC+ECAT_PRAM_WR_ADDR_LEN; -volatile uint32_t *ECAT_FIFO_WR_ADLEN_RD= QSPI_AHB+SQI_READ+SQI_INC+ECAT_PRAM_WR_ADDR_LEN; -volatile uint32_t *ECAT_FIFO_WR_CMD_RD= QSPI_AHB+SQI_READ+ECAT_PRAM_WR_CMD; -volatile uint32_t *ECAT_FIFO_WR_CMD_WR= QSPI_AHB+SQI_WRITE+ECAT_PRAM_WR_CMD; - -volatile uint32_t *INPUT_ADDRESS ; -volatile uint32_t *OUTPUT_ADDRESS; -volatile uint32_t read_buffer =0; -volatile uint8_t ecat_length= 0; - -volatile bool run_ECAT = false; -volatile bool synced = false; - -void ECAT_STATE_MACHINE(void){ - if ((ecat_state != wait)&(ecat_state != wait2)){ - run_ECAT = false; - switch(ecat_state){ - case en_SQI: - spi_cmd = &qspi_cmd; - QSPI->INSTRCTRL.bit.INSTR=spi_cmd->instruction; - OUTPUT_ADDRESS = ECAT_FIFO_RD_ADLEN_WR; - INPUT_ADDRESS = &QSPI_cmds[0]; - ecat_length = 0; - next_ecat_state = rd_rdy; - - break; - case rd_rdy: - if (QSPI_rx_buffer[0]==LAN9252_RDY){ - next_ecat_state = abort_fifo; - } - else if (QSPI_rx_buffer[0]== 0xFFFFFFFF){ - next_ecat_state = en_SQI; - QSPI_rx_buffer[0] = 0; - } - else{ - spi_cmd = &rd_cmd; - OUTPUT_ADDRESS = &QSPI_rx_buffer[0]; - INPUT_ADDRESS = ECAT_HW_CFG_RD; - ecat_length = 1; - wr_cnt=0; - } - - break; - case abort_fifo: - spi_cmd = &wr_cmd; - OUTPUT_ADDRESS = ECAT_FIFO_WR_CMD_WR; - INPUT_ADDRESS = &QSPI_cmds[1]; - ecat_length = 1; - next_ecat_state = dlt_rdram; - wr_cnt=0; - rd_cnt=0; - break; - case dlt_rdram: - spi_cmd = &wr_cmd; - OUTPUT_ADDRESS = ECAT_FIFO_WR_WR; - INPUT_ADDRESS = &zero[0]; - //if (wr_cnt >= 1) ecat_length = FIFO_DEPTH/2; - //else ecat_length = FIFO_DEPTH; - ecat_length = FIFO_DEPTH; - next_ecat_state = cf_dlt_rdram; - break; - case cf_dlt_rdram: - spi_cmd = &wr_cmd; - OUTPUT_ADDRESS = ECAT_FIFO_WR_ADLEN_WR; - INPUT_ADDRESS = &QSPI_cmds[4*(wr_cnt+1)]; - ecat_length = 2; - if (wr_cnt >= 2) { - next_ecat_state = wait; - wr_cnt =0; - } else { - next_ecat_state = dlt_rdram; - wr_cnt++; - } - - break; - case write_fifo: - spi_cmd = &wr_cmd; - OUTPUT_ADDRESS = ECAT_FIFO_WR_WR; - INPUT_ADDRESS = &QSPI_tx_buffer[wr_cnt*FIFO_DEPTH]; - /*if (wr_cnt >= 1) ecat_length = FIFO_DEPTH/2; - else ecat_length = FIFO_DEPTH;*/ - ecat_length = FIFO_DEPTH; - next_ecat_state = config_fifo; - break; - case config_fifo: - spi_cmd = &wr_cmd; - OUTPUT_ADDRESS = ECAT_FIFO_RD_ADLEN_WR; - INPUT_ADDRESS = &QSPI_cmds[4*(wr_cnt+1)]; - ecat_length = 4; - next_ecat_state = read_fifo; - break; - case read_fifo: - spi_cmd = &rd_cmd; - OUTPUT_ADDRESS = &QSPI_rx_buffer[wr_cnt*FIFO_DEPTH]; - INPUT_ADDRESS = ECAT_FIFO_RD_RD; - ecat_length = FIFO_DEPTH; - if (wr_cnt >= 2) { -// ecat_length = FIFO_DEPTH/2; - next_ecat_state = wait2; - wr_cnt=0; - } - else { -// ecat_length = FIFO_DEPTH; - next_ecat_state = write_fifo; - wr_cnt++; - } - break; - } - qspi_dma_enable(&ECAT_QSPI); - QSPI->INSTRFRAME.reg = ((*spi_cmd).inst_frame.word); - for (uint8_t i=0;iCTRLA.bit.LASTXFER = 1; - ecat_state = next_ecat_state; - } -} - -void config_qspi() -{ - QSPI->INTENSET.bit.CSRISE = 1; - NVIC_EnableIRQ(QSPI_IRQn); - ecat_state =en_SQI; -} - -QSPI_Handler(){ - if (QSPI->INTFLAG.bit.CSRISE == 1){ - QSPI->INTFLAG.bit.CSRISE = 1; - run_ECAT =true; - } -} - diff --git a/2_Motor_Slave/Motor_Slave/Motor_Slave/Ethercat_QSPI.h b/2_Motor_Slave/Motor_Slave/Motor_Slave/Ethercat_QSPI.h deleted file mode 100644 index d64c935..0000000 --- a/2_Motor_Slave/Motor_Slave/Motor_Slave/Ethercat_QSPI.h +++ /dev/null @@ -1,38 +0,0 @@ -/* - * EtherCAT_QSPI.h - * - * Created: 31/07/2021 17:51:24 - * Author: Nick-XMG - */ -#ifndef ETHERCAT_QSPI_H_ -#define ETHERCAT_QSPI_H_ - -#include "atmel_start.h" - - -#define ECAT_SIZE_WR 64 //max fifo size -#define ECAT_SIZE_RD ECAT_SIZE_WR -#define ECAT_SIZE_WR_REG ECAT_SIZE_WR/4 -#define ECAT_SIZE_RD_REG ECAT_SIZE_WR/4 - -#define buffer_size 3*ECAT_SIZE_WR_REG //changed to double -#define motor_buffer_size buffer_size/3 - - -extern enum ecat_states {abort_fifo,dlt_rdram,cf_dlt_rdram,write_fifo,config_fifo,read_fifo,wait,wait2,en_SQI,temp,rd_rdy}; -extern volatile enum ecat_states ecat_state; -extern volatile enum ecat_states next_ecat_state; - -volatile uint32_t QSPI_tx_buffer[buffer_size]; -volatile uint32_t QSPI_rx_buffer[buffer_size]; - -//static uint32_t QSPI_tx_buffer[buffer_size] = {47,46,45,44,43,42,41,40,39,38,37,36,35,34,33,32, - //31,30,29,28,27,26,25,24,23,22,21,20,19,18,17,16, - //15,14,13,12,11,10, 9,8,7,6,5,4,3,2,1,0}; -//static uint32_t QSPI_rx_buffer[buffer_size] = {0}; -extern volatile bool run_ECAT; -void config_qspi(void); - - - -#endif /* ETHERCAT_QSPI_H_ */ \ No newline at end of file diff --git a/2_Motor_Slave/Motor_Slave/Motor_Slave/Ethercat_SlaveDef.h b/2_Motor_Slave/Motor_Slave/Motor_Slave/Ethercat_SlaveDef.h deleted file mode 100644 index ca22b89..0000000 --- a/2_Motor_Slave/Motor_Slave/Motor_Slave/Ethercat_SlaveDef.h +++ /dev/null @@ -1,297 +0,0 @@ -/* - * EtherCAT_SlaveDef.h - * - * Created: 01/08/2021 12:56:16 - * Author: Nick-XMG - */ - - -#ifndef ETHERCAT_SLAVEDEF_H_ -#define ETHERCAT_SLAVEDEF_H_ - -#include "Ethercat_QSPI.h" - -//Write To Ecat Total Bytes (XX bytes) -/* Motor 1*/ -static volatile uint8_t *M1_Status = (uint8_t *)&QSPI_tx_buffer[0]; -static volatile uint8_t *M1_Mode = (((uint8_t *)&QSPI_tx_buffer[0])+1); -static volatile int16_t *M1_Joint_rel_position = (((int16_t *)&QSPI_tx_buffer[0])+1); -static volatile int16_t *M1_Joint_abs_position = ((int16_t *)&QSPI_tx_buffer[1]); -static volatile int16_t *M1_Motor_speed = (((int16_t *)&QSPI_tx_buffer[1])+1); -static volatile int16_t *M1_Motor_current_bus = ((int16_t *)&QSPI_tx_buffer[2]); -static volatile int16_t *M1_Motor_currentPhA = (((int16_t *)&QSPI_tx_buffer[2])+1); -static volatile int16_t *M1_Motor_currentPhB = ((int16_t *)&QSPI_tx_buffer[3]); -static volatile int16_t *M1_Motor_currentPhC = (((int16_t *)&QSPI_tx_buffer[3])+1); -static volatile int16_t *M1_Motor__hallState = ((int16_t *)&QSPI_tx_buffer[4]); -static volatile int16_t *M1_Motor_dutyCycle = (((int16_t *)&QSPI_tx_buffer[4])+1); -/* Motor 2*/ -static volatile uint8_t *M2_Status = (uint8_t *)&QSPI_tx_buffer[5]; -static volatile uint8_t *M2_Mode = (((uint8_t *)&QSPI_tx_buffer[5])+1); -static volatile int16_t *M2_Joint_rel_position = (((int16_t *)&QSPI_tx_buffer[5])+1); -static volatile int16_t *M2_Joint_abs_position = ((int16_t *)&QSPI_tx_buffer[6]); -static volatile int16_t *M2_Motor_speed = (((int16_t *)&QSPI_tx_buffer[6])+1); -static volatile int16_t *M2_Motor_current_bus = ((int16_t *)&QSPI_tx_buffer[7]); -static volatile int16_t *M2_Motor_currentPhA = (((int16_t *)&QSPI_tx_buffer[7])+1); -static volatile int16_t *M2_Motor_currentPhB = ((int16_t *)&QSPI_tx_buffer[8]); -static volatile int16_t *M2_Motor_currentPhC = (((int16_t *)&QSPI_tx_buffer[8])+1); -static volatile int16_t *M2_Motor__hallState = ((int16_t *)&QSPI_tx_buffer[9]); -static volatile int16_t *M2_Motor_dutyCycle = (((int16_t *)&QSPI_tx_buffer[9])+1); -/* EMG */ -static volatile int16_t *EMG_CH1 = (((int16_t *)&QSPI_tx_buffer[10])); -static volatile int16_t *EMG_CH2 = (((int16_t *)&QSPI_tx_buffer[10])+1); -static volatile int16_t *EMG_CH3 = (((int16_t *)&QSPI_tx_buffer[11])); -static volatile int16_t *EMG_CH4 = (((int16_t *)&QSPI_tx_buffer[11])+1); -static volatile int16_t *EMG_CH5 = (((int16_t *)&QSPI_tx_buffer[12])); -static volatile int16_t *EMG_CH6 = (((int16_t *)&QSPI_tx_buffer[12])+1); -static volatile int16_t *EMG_CH7 = (((int16_t *)&QSPI_tx_buffer[13])); -static volatile int16_t *EMG_CH8 = (((int16_t *)&QSPI_tx_buffer[13])+1); -/* Motor 3*/ -static volatile uint8_t *M3_Status = (uint8_t *)&QSPI_tx_buffer[14]; -static volatile uint8_t *M3_Mode = (((uint8_t *)&QSPI_tx_buffer[14])+1); -static volatile int16_t *M3_Joint_rel_position = (((int16_t *)&QSPI_tx_buffer[14])+1); -static volatile int16_t *M3_Joint_abs_position = ((int16_t *)&QSPI_tx_buffer[15]); -static volatile int16_t *M3_Motor_speed = (((int16_t *)&QSPI_tx_buffer[15])+1); -static volatile int16_t *M3_Motor_current_bus = ((int16_t *)&QSPI_tx_buffer[16]); -static volatile int16_t *M3_Motor_currentPhA = (((int16_t *)&QSPI_tx_buffer[16])+1); -static volatile int16_t *M3_Motor_currentPhB = ((int16_t *)&QSPI_tx_buffer[17]); -static volatile int16_t *M3_Motor_currentPhC = (((int16_t *)&QSPI_tx_buffer[17])+1); -static volatile int16_t *M3_Motor__hallState = ((int16_t *)&QSPI_tx_buffer[18]); -static volatile int16_t *M3_Motor_dutyCycle = (((int16_t *)&QSPI_tx_buffer[18])+1); -/* Motor 4*/ -static volatile uint8_t *M4_Status = (uint8_t *)&QSPI_tx_buffer[19]; -static volatile uint8_t *M4_Mode = (((uint8_t *)&QSPI_tx_buffer[19])+1); -static volatile int16_t *M4_Joint_rel_position = (((int16_t *)&QSPI_tx_buffer[19])+1); -static volatile int16_t *M4_Joint_abs_position = ((int16_t *)&QSPI_tx_buffer[20]); -static volatile int16_t *M4_Motor_speed = (((int16_t *)&QSPI_tx_buffer[20])+1); -static volatile int16_t *M4_Motor_current_bus = ((int16_t *)&QSPI_tx_buffer[21]); -static volatile int16_t *M4_Motor_currentPhA = (((int16_t *)&QSPI_tx_buffer[21])+1); -static volatile int16_t *M4_Motor_currentPhB = ((int16_t *)&QSPI_tx_buffer[22]); -static volatile int16_t *M4_Motor_currentPhC = (((int16_t *)&QSPI_tx_buffer[22])+1); -static volatile int16_t *M4_Motor__hallState = ((int16_t *)&QSPI_tx_buffer[23]); -static volatile int16_t *M4_Motor_dutyCycle = (((int16_t *)&QSPI_tx_buffer[23])+1); -/* IMU */ -static volatile int16_t *q_x0 = (((int16_t *)&QSPI_tx_buffer[24])); -static volatile int16_t *q_y0 = (((int16_t *)&QSPI_tx_buffer[24])+1); -static volatile int16_t *q_z0 = (((int16_t *)&QSPI_tx_buffer[25])); -static volatile int16_t *q_w0 = (((int16_t *)&QSPI_tx_buffer[25])+1); -/* EMG */ -static volatile int16_t *FSR_CH1 = (((int16_t *)&QSPI_tx_buffer[26])); -static volatile int16_t *FSR_CH2 = (((int16_t *)&QSPI_tx_buffer[26])+1); -static volatile int16_t *FSR_CH3 = (((int16_t *)&QSPI_tx_buffer[27])); -static volatile int16_t *FSR_CH4 = (((int16_t *)&QSPI_tx_buffer[27])+1); -static volatile int16_t *FSR_CH5 = (((int16_t *)&QSPI_tx_buffer[28])); -static volatile int16_t *Pressure_CH1 = (((int16_t *)&QSPI_tx_buffer[28])+1); -static volatile int16_t *Pressure_CH2 = (((int16_t *)&QSPI_tx_buffer[29])); -static volatile int16_t *Pressure_CH3 = (((int16_t *)&QSPI_tx_buffer[29])+1); - -//Read From Ecat Total (XX Bytes) -//QSPI_rx_buffer -/* Motor 1*/ -static volatile uint8_t *M1_Control_mode = ((uint8_t *)&QSPI_rx_buffer[0]); -static volatile uint8_t *M1_Control_set = (((uint8_t *)&QSPI_rx_buffer[0])+1); -static volatile int16_t *M1_Desired_pos = ((int16_t *)&QSPI_rx_buffer[0]+1); -static volatile int16_t *M1_Desired_speed = ((int16_t *)&QSPI_rx_buffer[1]); -static volatile int16_t *M1_Desired_current = ((int16_t *)&QSPI_rx_buffer[1]+1); -static volatile int16_t *M1_Max_pos = ((int16_t *)&QSPI_rx_buffer[2]); -static volatile int16_t *M1_Max_velocity = ((int16_t *)&QSPI_rx_buffer[2]+1); -static volatile int16_t *M1_Max_current = ((int16_t *)&QSPI_rx_buffer[3]); -static volatile int16_t *M1_Desired_dc = ((int16_t *)&QSPI_rx_buffer[3]+1); //Spare -///* Motor 2*/ -static volatile uint8_t *M2_Control_mode = ((uint8_t *)&QSPI_rx_buffer[4]); -static volatile uint8_t *M2_Control_set = (((uint8_t *)&QSPI_rx_buffer[4])+1); -static volatile int16_t *M2_Desired_pos = ((int16_t *)&QSPI_rx_buffer[4]+1); -static volatile int16_t *M2_Desired_speed = ((int16_t *)&QSPI_rx_buffer[5]); -static volatile int16_t *M2_Desired_current = ((int16_t *)&QSPI_rx_buffer[5]+1); -static volatile int16_t *M2_Max_pos = ((int16_t *)&QSPI_rx_buffer[6]); -static volatile int16_t *M2_Max_velocity = ((int16_t *)&QSPI_rx_buffer[6]+1); -static volatile int16_t *M2_Max_current = ((int16_t *)&QSPI_rx_buffer[7]); -static volatile int16_t *M2_Desired_dc = ((int16_t *)&QSPI_rx_buffer[7]+1); //Spare -///* Motor 3*/ -static volatile uint8_t *M3_Control_mode = ((uint8_t *)&QSPI_rx_buffer[8]); -static volatile uint8_t *M3_Control_set = (((uint8_t *)&QSPI_rx_buffer[8])+1); -static volatile int16_t *M3_Desired_pos = ((int16_t *)&QSPI_rx_buffer[8]+1); -static volatile int16_t *M3_Desired_speed = ((int16_t *)&QSPI_rx_buffer[9]); -static volatile int16_t *M3_Desired_current = ((int16_t *)&QSPI_rx_buffer[9]+1); -static volatile int16_t *M3_Max_pos = ((int16_t *)&QSPI_rx_buffer[10]); -static volatile int16_t *M3_Max_velocity = ((int16_t *)&QSPI_rx_buffer[10]+1); -static volatile int16_t *M3_Max_current = ((int16_t *)&QSPI_rx_buffer[11]); -static volatile int16_t *M3_Spare = ((int16_t *)&QSPI_rx_buffer[11]+1); //Spare -///* Motor 4*/ -static volatile uint8_t *M4_Control_mode = ((uint8_t *)&QSPI_rx_buffer[12]); -static volatile uint8_t *M4_Control_set = (((uint8_t *)&QSPI_rx_buffer[12])+1); -static volatile int16_t *M4_Desired_pos = ((int16_t *)&QSPI_rx_buffer[12]+1); -static volatile int16_t *M4_Desired_speed = ((int16_t *)&QSPI_rx_buffer[13]); -static volatile int16_t *M4_Desired_current = ((int16_t *)&QSPI_rx_buffer[13]+1); -static volatile int16_t *M4_Max_pos = ((int16_t *)&QSPI_rx_buffer[14]); -static volatile int16_t *M4_Max_velocity = ((int16_t *)&QSPI_rx_buffer[14]+1); -static volatile int16_t *M4_Max_current = ((int16_t *)&QSPI_rx_buffer[15]); -static volatile int16_t *M4_Spare = ((int16_t *)&QSPI_rx_buffer[15]+1);//Spare - -static void update_telemetry(void) -{ - inline int16_t convert_to_mA(volatile float32_t current_PU) - { - return (int16_t)(current_PU*1000.0f); - } - // - //*M1_Status = 0; - //*M1_Mode = 0; - - /* Motor 1 */ - *M1_Status = Motor1.motor_state.currentstate; - *M1_Joint_rel_position = Motor1.motor_status.Num_Steps; - //*M1_Joint_abs_position = ((int16_t *)&QSPI_tx_buffer[1]); - //*M1_Motor_speed = (((int16_t *)&QSPI_tx_buffer[1])+1); - *M1_Motor_current_bus = convert_to_mA(Motor1.Iphase_pu.Bus); - *M1_Motor_currentPhA = convert_to_mA(Motor1.Iphase_pu.A); - *M1_Motor_currentPhB = convert_to_mA(Motor1.Iphase_pu.B); - *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; - //*M1_Joint_abs_position = ((int16_t *)&QSPI_tx_buffer[1]); - //*M1_Motor_speed = (((int16_t *)&QSPI_tx_buffer[1])+1); - *M2_Motor_current_bus = convert_to_mA( Motor2.Iphase_pu.Bus); - *M2_Motor_currentPhA = convert_to_mA( Motor2.Iphase_pu.A); - *M2_Motor_currentPhB = convert_to_mA( Motor2.Iphase_pu.B); - *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) -{ - 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; - //volatile int16_t d = *M1_Desired_speed; - //volatile int16_t e = *M1_Desired_current; - //volatile int16_t f = *M1_Max_pos; - //volatile int16_t g = *M1_Max_velocity; - //volatile int16_t h = *M1_Max_current; - //volatile int16_t i = *M1_Spare; - //inline float32_t convert_int_to_PU(volatile int16_t input) - //{ - //return ((float32_t)(input/1000.0f)); - //} - ////Motor1.des_mode = 0; - ////Motor1.set = 0; - //Motor1.motor_setpoints.desired_position = *desired_position; - //Motor1.motor_setpoints.desired_speed = *desired_speed; - ////Motor1.desired_speed = 1500; - //Motor1.motor_setpoints.desired_torque = convert_int_to_PU(*desired_torque); - ////Motor1.controllerParam.I_kp = 0; - ////Motor1.controllerParam.I_ki = 0; - ////Motor1.controllerParam.V_kp = 0; - ////Motor1.controllerParam.V_kd = 0; - ////Motor1.controllerParam.V_kd = 0; - ////Motor1.controllerParam.P_kp = 0; - ////Motor1.controllerParam.P_ki = 0; - ////Motor1.reductionRatio = 0; - //Motor1.motor_setpoints.max_velocity = *max_velocity; - //Motor1.motor_setpoints.max_current = convert_int_to_PU(*max_current); - //Motor1.motor_setpoints.max_torque = convert_int_to_PU(*max_torque); - ////Motor1.Spare1 = 0; - ////Motor1.Spare2 = 0; - ////Motor1.Spare3 = 0; - ////Motor1.Spare4 = 0; -} - - -static inline void comms_check(void) -{ - /* Motor 1*/ - *M1_Status = 1; - *M1_Mode = 2; - *M1_Joint_rel_position = -3; - *M1_Joint_abs_position = 4; - *M1_Motor_speed = -5; - *M1_Motor_current_bus = 6; - *M1_Motor_currentPhA = -7; - *M1_Motor_currentPhB = 8; - *M1_Motor_currentPhC = -9; - *M1_Motor__hallState = 10; - *M1_Motor_dutyCycle = -11; - /* Motor 2*/ - *M2_Status = 12; - *M2_Mode = 13; - *M2_Joint_rel_position = 14; - *M2_Joint_abs_position = -15; - *M2_Motor_speed = 16; - *M2_Motor_current_bus = -17; - *M2_Motor_currentPhA = 18; - *M2_Motor_currentPhB = -19; - *M2_Motor_currentPhC = 20; - *M2_Motor__hallState = -21; - *M3_Motor_dutyCycle = 22; - /* EMG */ - *EMG_CH1 = -23; - *EMG_CH2 = 24; - *EMG_CH3 = -25; - *EMG_CH4 = 26; - *EMG_CH5 = -27; - *EMG_CH6 = 28; - *EMG_CH7 = -29; - *EMG_CH8 = 30; - - /* Motor 3*/ - *M3_Status = 1; - *M3_Mode = 2; - *M3_Joint_rel_position = -3; - *M3_Joint_abs_position = 4; - *M3_Motor_speed = -5; - *M3_Motor_current_bus = 6; - *M3_Motor_currentPhA = -7; - *M3_Motor_currentPhB = 8; - *M3_Motor_currentPhC = -9; - *M3_Motor__hallState = 10; - *M3_Motor_dutyCycle = -11; - /* Motor 4*/ - *M4_Status = 12; - *M4_Mode = 13; - *M4_Joint_rel_position = 14; - *M4_Joint_abs_position = -15; - *M4_Motor_speed = 16; - *M4_Motor_current_bus = -17; - *M4_Motor_currentPhA = 18; - *M4_Motor_currentPhB = -19; - *M4_Motor_currentPhC = 20; - *M4_Motor__hallState = -21; - *M4_Motor_dutyCycle = 22; - /* IMU */ - *q_x0 = 23; - *q_y0 = -24; - *q_z0 = 25; - *q_w0 = -26; - /* EMG */ - *FSR_CH1 = 27; - *FSR_CH2 = -28; - *FSR_CH3 = 29; - *FSR_CH4 = -30; - *FSR_CH5 = 31; - *Pressure_CH1 = -32; - *Pressure_CH2 = 33; - *Pressure_CH3 = -34; -} - -#endif /* ETHERCAT_SLAVEDEF_H_ */ \ No newline at end of file diff --git a/2_Motor_Slave/Motor_Slave/Motor_Slave/MSIF_slave.h b/2_Motor_Slave/Motor_Slave/Motor_Slave/MSIF_slave.h index 28d6f85..ae0be59 100644 --- a/2_Motor_Slave/Motor_Slave/Motor_Slave/MSIF_slave.h +++ b/2_Motor_Slave/Motor_Slave/Motor_Slave/MSIF_slave.h @@ -9,75 +9,141 @@ #ifndef MASTER_SLAVE_IF_H_ #define MASTER_SLAVE_IF_H_ -#define SLAVE_BUFFER_SIZE 64 +#define SLAVE_BUFFER_SIZE_BYTES 64 +#define SLAVE_BUFFER_SIZE_LONG SLAVE_BUFFER_SIZE_BYTES/4 -static uint8_t SPI_rx_buffer[SLAVE_BUFFER_SIZE] = {0}; -static uint8_t SPI_tx_buffer[SLAVE_BUFFER_SIZE] = {0}; +static uint32_t SPI_rx_buffer[SLAVE_BUFFER_SIZE_LONG] = {0}; +static uint32_t SPI_tx_buffer[SLAVE_BUFFER_SIZE_LONG] = {0}; //static uint8_t SPI_tx_buffer[SLAVE_BUFFER_SIZE] = {0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17, //18,19,20,21,22,23,24,25,26,27,28,29,30,31}; //tx_buffer /* Motor 3*/ -static volatile uint8_t *M3_Status = (uint8_t *)&SPI_tx_buffer[0]; //1 byte - 0 of 64 -static volatile uint8_t *M3_Mode = (uint8_t *)&SPI_tx_buffer[1]; //1 byte - 1 of 64 -static volatile int16_t *M3_Joint_rel_position = (int16_t *)&SPI_tx_buffer[2]; //2 byte - 2 of 64 -static volatile int16_t *M3_Joint_abs_position = (int16_t *)&SPI_tx_buffer[4]; //2 byte - 4 of 64 -static volatile int16_t *M3_Motor_speed = (int16_t *)&SPI_tx_buffer[6]; //2 byte - 6 of 64 -static volatile int16_t *M3_Motor_current_bus = (int16_t *)&SPI_tx_buffer[8]; //2 byte - 8 of 64 -static volatile int16_t *M3_Motor_currentPhA = (int16_t *)&SPI_tx_buffer[10]; //2 byte - 10 of 64 -static volatile int16_t *M3_Motor_currentPhB = (int16_t *)&SPI_tx_buffer[12]; //2 byte - 12 of 64 -static volatile int16_t *M3_Motor_currentPhC = (int16_t *)&SPI_tx_buffer[14]; //2 byte - 14 of 64 -static volatile int16_t *M3_Motor__hallState = (int16_t *)&SPI_tx_buffer[16]; //2 byte - 16 of 64 -static volatile int16_t *M3_Motor_dutyCycle = (int16_t *)&SPI_tx_buffer[18]; //2 byte - 18 of 64 +static volatile uint8_t *M3_Status = (uint8_t *)&SPI_tx_buffer[0]; +static volatile uint8_t *M3_Mode = (((uint8_t *)&SPI_tx_buffer[0])+1); +static volatile int16_t *M3_Joint_rel_position = (((int16_t *)&SPI_tx_buffer[0])+1); +static volatile int16_t *M3_Joint_abs_position = ((int16_t *)&SPI_tx_buffer[1]); +static volatile int16_t *M3_Motor_speed = (((int16_t *)&SPI_tx_buffer[1])+1); +static volatile int16_t *M3_Motor_current_bus = ((int16_t *)&SPI_tx_buffer[2]); +static volatile int16_t *M3_Motor_currentPhA = (((int16_t *)&SPI_tx_buffer[2])+1); +static volatile int16_t *M3_Motor_currentPhB = ((int16_t *)&SPI_tx_buffer[3]); +static volatile int16_t *M3_Motor_currentPhC = (((int16_t *)&SPI_tx_buffer[3])+1); +static volatile int16_t *M3_Motor__hallState = ((int16_t *)&SPI_tx_buffer[4]); +static volatile int16_t *M3_Motor_dutyCycle = (((int16_t *)&SPI_tx_buffer[4])+1); /* Motor 4*/ -static volatile uint8_t *M4_Status = (uint8_t *)&SPI_tx_buffer[20]; //1 byte - 20 of 64 -static volatile uint8_t *M4_Mode = (uint8_t *)&SPI_tx_buffer[21]; //1 byte - 21 of 64 -static volatile int16_t *M4_Joint_rel_position = (int16_t *)&SPI_tx_buffer[22]; //2 byte - 22 of 64 -static volatile int16_t *M4_Joint_abs_position = (int16_t *)&SPI_tx_buffer[24]; //2 byte - 24 of 64 -static volatile int16_t *M4_Motor_speed = (int16_t *)&SPI_tx_buffer[26]; //2 byte - 26 of 64 -static volatile int16_t *M4_Motor_current_bus = (int16_t *)&SPI_tx_buffer[28]; //2 byte - 28 of 64 -static volatile int16_t *M4_Motor_currentPhA = (int16_t *)&SPI_tx_buffer[30]; //2 byte - 30 of 64 -static volatile int16_t *M4_Motor_currentPhB = (int16_t *)&SPI_tx_buffer[32]; //2 byte - 32 of 64 -static volatile int16_t *M4_Motor_currentPhC = (int16_t *)&SPI_tx_buffer[34]; //2 byte - 34 of 64 -static volatile int16_t *M4_Motor__hallState = (int16_t *)&SPI_tx_buffer[36]; //2 byte - 36 of 64 -static volatile int16_t *M4_Motor_dutyCycle = (int16_t *)&SPI_tx_buffer[38]; //2 byte - 38 of 64 +static volatile uint8_t *M4_Status = (uint8_t *)&SPI_tx_buffer[5]; +static volatile uint8_t *M4_Mode = (((uint8_t *)&SPI_tx_buffer[5])+1); +static volatile int16_t *M4_Joint_rel_position = (((int16_t *)&SPI_tx_buffer[5])+1); +static volatile int16_t *M4_Joint_abs_position = ((int16_t *)&SPI_tx_buffer[6]); +static volatile int16_t *M4_Motor_speed = (((int16_t *)&SPI_tx_buffer[6])+1); +static volatile int16_t *M4_Motor_current_bus = ((int16_t *)&SPI_tx_buffer[7]); +static volatile int16_t *M4_Motor_currentPhA = (((int16_t *)&SPI_tx_buffer[7])+1); +static volatile int16_t *M4_Motor_currentPhB = ((int16_t *)&SPI_tx_buffer[8]); +static volatile int16_t *M4_Motor_currentPhC = (((int16_t *)&SPI_tx_buffer[8])+1); +static volatile int16_t *M4_Motor__hallState = ((int16_t *)&SPI_tx_buffer[9]); +static volatile int16_t *M4_Motor_dutyCycle = (((int16_t *)&SPI_tx_buffer[9])+1); /* IMU */ -static volatile int16_t *q_x0 = (int16_t *)&SPI_tx_buffer[40]; //2 byte - 40 of 64 -static volatile int16_t *q_y0 = (int16_t *)&SPI_tx_buffer[42]; //2 byte - 42 of 64 -static volatile int16_t *q_z0 = (int16_t *)&SPI_tx_buffer[44]; //2 byte - 44 of 64 -static volatile int16_t *q_w0 = (int16_t *)&SPI_tx_buffer[46]; //2 byte - 46 of 64 +static volatile int16_t *q_x0 = (int16_t *)&SPI_tx_buffer[10]; +static volatile int16_t *q_y0 = (((int16_t *)&SPI_tx_buffer[10])+1); +static volatile int16_t *q_z0 = (int16_t *)&SPI_tx_buffer[11]; +static volatile int16_t *q_w0 = (((int16_t *)&SPI_tx_buffer[11])+1); /* EMG */ -static volatile int16_t *FSR_CH1 = (int16_t *)&SPI_tx_buffer[48]; //2 byte - 48 of 64 -static volatile int16_t *FSR_CH2 = (int16_t *)&SPI_tx_buffer[50]; //2 byte - 50 of 64 -static volatile int16_t *FSR_CH3 = (int16_t *)&SPI_tx_buffer[52]; //2 byte - 52 of 64 -static volatile int16_t *FSR_CH4 = (int16_t *)&SPI_tx_buffer[54]; //2 byte - 54 of 64 -static volatile int16_t *FSR_CH5 = (int16_t *)&SPI_tx_buffer[56]; //2 byte - 56 of 64 -static volatile int16_t *Pressure_CH1 = (int16_t *)&SPI_tx_buffer[58]; //2 byte - 58 of 64 -static volatile int16_t *Pressure_CH2 = (int16_t *)&SPI_tx_buffer[60]; //2 byte - 60 of 64 -static volatile int16_t *Pressure_CH3 = (int16_t *)&SPI_tx_buffer[62]; //2 byte - 62 of 64 +static volatile int16_t *FSR_CH1 = (int16_t *)&SPI_tx_buffer[12]; //2 byte - 48 of 64 +static volatile int16_t *FSR_CH2 = (((int16_t *)&SPI_tx_buffer[12])+1); +static volatile int16_t *FSR_CH3 = (int16_t *)&SPI_tx_buffer[13]; //2 byte - 52 of 64 +static volatile int16_t *FSR_CH4 = (((int16_t *)&SPI_tx_buffer[13])+1); +static volatile int16_t *FSR_CH5 = (int16_t *)&SPI_tx_buffer[14]; //2 byte - 56 of 64 +static volatile int16_t *Pressure_CH1 = (((int16_t *)&SPI_tx_buffer[14])+1); +static volatile int16_t *Pressure_CH2 = (int16_t *)&SPI_tx_buffer[15]; //2 byte - 60 of 64 +static volatile int16_t *Pressure_CH3 = (((int16_t *)&SPI_tx_buffer[15])+1); //rx_buffer ///* Motor 3*/ -static volatile uint8_t *M3_Control_mode = (uint8_t *)&SPI_rx_buffer[0]; //1 byte - 0 of 32 -static volatile uint8_t *M3_Control_set = (uint8_t *)&SPI_rx_buffer[1]; //1 byte - 1 of 32 -static volatile int16_t *M3_Desired_pos = (int16_t *)&SPI_rx_buffer[2]; //2 byte - 2 of 32 -static volatile int16_t *M3_Desired_speed = (int16_t *)&SPI_rx_buffer[4]; //2 byte - 4 of 32 -static volatile int16_t *M3_Desired_current = (int16_t *)&SPI_rx_buffer[6]; //2 byte - 6 of 32 -static volatile int16_t *M3_Max_pos = (int16_t *)&SPI_rx_buffer[8]; //2 byte - 8 of 32 -static volatile int16_t *M3_Max_velocity = (int16_t *)&SPI_rx_buffer[10]; //2 byte - 10 of 32 -static volatile int16_t *M3_Max_current = (int16_t *)&SPI_rx_buffer[12]; //2 byte - 12 of 32 -static volatile int16_t *M3_Spare = (int16_t *)&SPI_rx_buffer[14]; //2 byte - 14 of 32 +static volatile uint8_t *M3_Control_mode = (uint8_t *)&SPI_rx_buffer[0]; //1 byte - 0 of 32 +static volatile uint8_t *M3_Control_set = (((uint8_t *)&SPI_rx_buffer[0])+1); //1 byte - 1 of 32 +static volatile int16_t *M3_Desired_pos = ((int16_t *)&SPI_rx_buffer[0]+1); //2 byte - 2 of 32 +static volatile int16_t *M3_Desired_speed = (int16_t *)&SPI_rx_buffer[1]; //2 byte - 4 of 32 +static volatile int16_t *M3_Desired_current = ((int16_t *)&SPI_rx_buffer[1]+1); //2 byte - 6 of 32 +static volatile int16_t *M3_Max_pos = (int16_t *)&SPI_rx_buffer[2]; //2 byte - 8 of 32 +static volatile int16_t *M3_Max_velocity = ((int16_t *)&SPI_rx_buffer[2]+1); //2 byte - 10 of 32 +static volatile int16_t *M3_Max_current = (int16_t *)&SPI_rx_buffer[3]; //2 byte - 12 of 32 +static volatile int16_t *M3_Spare = ((int16_t *)&SPI_rx_buffer[3]+1); //2 byte - 14 of 32 ///* Motor 4*/ -static volatile uint8_t *M4_Control_mode = (int16_t *)&SPI_rx_buffer[16]; //1 byte - 16 of 32 -static volatile uint8_t *M4_Control_set = (int16_t *)&SPI_rx_buffer[17]; //1 byte - 17 of 32 -static volatile int16_t *M4_Desired_pos = (int16_t *)&SPI_rx_buffer[18]; //2 byte - 18 of 32 -static volatile int16_t *M4_Desired_speed = (int16_t *)&SPI_rx_buffer[20]; //2 byte - 20 of 32 -static volatile int16_t *M4_Desired_current = (int16_t *)&SPI_rx_buffer[22]; //2 byte - 22 of 32 -static volatile int16_t *M4_Max_pos = (int16_t *)&SPI_rx_buffer[24]; //2 byte - 24 of 32 -static volatile int16_t *M4_Max_velocity = (int16_t *)&SPI_rx_buffer[26]; //2 byte - 26 of 32 -static volatile int16_t *M4_Max_current = (int16_t *)&SPI_rx_buffer[28]; //2 byte - 28 of 32 -static volatile int16_t *M4_Spare = (int16_t *)&SPI_rx_buffer[30]; //2 byte - 30 of 32 +static volatile uint8_t *M4_Control_mode = (uint8_t *)&SPI_rx_buffer[4]; //1 byte - 16 of 32 +static volatile uint8_t *M4_Control_set = (((uint8_t *)&SPI_rx_buffer[4])+1); //1 byte - 17 of 32 +static volatile int16_t *M4_Desired_pos = ((int16_t *)&SPI_rx_buffer[4]+1); //2 byte - 18 of 32 +static volatile int16_t *M4_Desired_speed = (int16_t *)&SPI_rx_buffer[5]; //2 byte - 20 of 32 +static volatile int16_t *M4_Desired_current = ((int16_t *)&SPI_rx_buffer[5]+1); //2 byte - 22 of 32 +static volatile int16_t *M4_Max_pos = (int16_t *)&SPI_rx_buffer[6]; //2 byte - 24 of 32 +static volatile int16_t *M4_Max_velocity = ((int16_t *)&SPI_rx_buffer[6]+1); //2 byte - 26 of 32 +static volatile int16_t *M4_Max_current = (int16_t *)&SPI_rx_buffer[7]; //2 byte - 28 of 32 +static volatile int16_t *M4_Spare = ((int16_t *)&SPI_rx_buffer[7]+1); //2 byte - 30 of 32 + + +////tx_buffer +///* Motor 3*/ +//static volatile uint8_t *M3_Status = (uint8_t *)&SPI_tx_buffer[0]; //1 byte - 0 of 64 +//static volatile uint8_t *M3_Mode = (uint8_t *)&SPI_tx_buffer[1]; //1 byte - 1 of 64 +//static volatile int16_t *M3_Joint_rel_position = (int16_t *)&SPI_tx_buffer[2]; //2 byte - 2 of 64 +//static volatile int16_t *M3_Joint_abs_position = (int16_t *)&SPI_tx_buffer[4]; //2 byte - 4 of 64 +//static volatile int16_t *M3_Motor_speed = (int16_t *)&SPI_tx_buffer[6]; //2 byte - 6 of 64 +//static volatile int16_t *M3_Motor_current_bus = (int16_t *)&SPI_tx_buffer[8]; //2 byte - 8 of 64 +//static volatile int16_t *M3_Motor_currentPhA = (int16_t *)&SPI_tx_buffer[10]; //2 byte - 10 of 64 +//static volatile int16_t *M3_Motor_currentPhB = (int16_t *)&SPI_tx_buffer[12]; //2 byte - 12 of 64 +//static volatile int16_t *M3_Motor_currentPhC = (int16_t *)&SPI_tx_buffer[14]; //2 byte - 14 of 64 +//static volatile int16_t *M3_Motor__hallState = (int16_t *)&SPI_tx_buffer[16]; //2 byte - 16 of 64 +//static volatile int16_t *M3_Motor_dutyCycle = (int16_t *)&SPI_tx_buffer[18]; //2 byte - 18 of 64 +///* Motor 4*/ +//static volatile uint8_t *M4_Status = (uint8_t *)&SPI_tx_buffer[20]; //1 byte - 20 of 64 +//static volatile uint8_t *M4_Mode = (uint8_t *)&SPI_tx_buffer[21]; //1 byte - 21 of 64 +//static volatile int16_t *M4_Joint_rel_position = (int16_t *)&SPI_tx_buffer[22]; //2 byte - 22 of 64 +//static volatile int16_t *M4_Joint_abs_position = (int16_t *)&SPI_tx_buffer[24]; //2 byte - 24 of 64 +//static volatile int16_t *M4_Motor_speed = (int16_t *)&SPI_tx_buffer[26]; //2 byte - 26 of 64 +//static volatile int16_t *M4_Motor_current_bus = (int16_t *)&SPI_tx_buffer[28]; //2 byte - 28 of 64 +//static volatile int16_t *M4_Motor_currentPhA = (int16_t *)&SPI_tx_buffer[30]; //2 byte - 30 of 64 +//static volatile int16_t *M4_Motor_currentPhB = (int16_t *)&SPI_tx_buffer[32]; //2 byte - 32 of 64 +//static volatile int16_t *M4_Motor_currentPhC = (int16_t *)&SPI_tx_buffer[34]; //2 byte - 34 of 64 +//static volatile int16_t *M4_Motor__hallState = (int16_t *)&SPI_tx_buffer[36]; //2 byte - 36 of 64 +//static volatile int16_t *M4_Motor_dutyCycle = (int16_t *)&SPI_tx_buffer[38]; //2 byte - 38 of 64 +///* IMU */ +//static volatile int16_t *q_x0 = (int16_t *)&SPI_tx_buffer[40]; //2 byte - 40 of 64 +//static volatile int16_t *q_y0 = (int16_t *)&SPI_tx_buffer[42]; //2 byte - 42 of 64 +//static volatile int16_t *q_z0 = (int16_t *)&SPI_tx_buffer[44]; //2 byte - 44 of 64 +//static volatile int16_t *q_w0 = (int16_t *)&SPI_tx_buffer[46]; //2 byte - 46 of 64 +///* EMG */ +//static volatile int16_t *FSR_CH1 = (int16_t *)&SPI_tx_buffer[48]; //2 byte - 48 of 64 +//static volatile int16_t *FSR_CH2 = (int16_t *)&SPI_tx_buffer[50]; //2 byte - 50 of 64 +//static volatile int16_t *FSR_CH3 = (int16_t *)&SPI_tx_buffer[52]; //2 byte - 52 of 64 +//static volatile int16_t *FSR_CH4 = (int16_t *)&SPI_tx_buffer[54]; //2 byte - 54 of 64 +//static volatile int16_t *FSR_CH5 = (int16_t *)&SPI_tx_buffer[56]; //2 byte - 56 of 64 +//static volatile int16_t *Pressure_CH1 = (int16_t *)&SPI_tx_buffer[58]; //2 byte - 58 of 64 +//static volatile int16_t *Pressure_CH2 = (int16_t *)&SPI_tx_buffer[60]; //2 byte - 60 of 64 +//static volatile int16_t *Pressure_CH3 = (int16_t *)&SPI_tx_buffer[62]; //2 byte - 62 of 64 +// +////rx_buffer +/////* Motor 3*/ +//static volatile uint8_t *M3_Control_mode = (uint8_t *)&SPI_rx_buffer[0]; //1 byte - 0 of 32 +//static volatile uint8_t *M3_Control_set = (uint8_t *)&SPI_rx_buffer[1]; //1 byte - 1 of 32 +//static volatile int16_t *M3_Desired_pos = (int16_t *)&SPI_rx_buffer[2]; //2 byte - 2 of 32 +//static volatile int16_t *M3_Desired_speed = (int16_t *)&SPI_rx_buffer[4]; //2 byte - 4 of 32 +//static volatile int16_t *M3_Desired_current = (int16_t *)&SPI_rx_buffer[6]; //2 byte - 6 of 32 +//static volatile int16_t *M3_Max_pos = (int16_t *)&SPI_rx_buffer[8]; //2 byte - 8 of 32 +//static volatile int16_t *M3_Max_velocity = (int16_t *)&SPI_rx_buffer[10]; //2 byte - 10 of 32 +//static volatile int16_t *M3_Max_current = (int16_t *)&SPI_rx_buffer[12]; //2 byte - 12 of 32 +//static volatile int16_t *M3_Spare = (int16_t *)&SPI_rx_buffer[14]; //2 byte - 14 of 32 +/////* Motor 4*/ +//static volatile uint8_t *M4_Control_mode = (int16_t *)&SPI_rx_buffer[16]; //1 byte - 16 of 32 +//static volatile uint8_t *M4_Control_set = (int16_t *)&SPI_rx_buffer[17]; //1 byte - 17 of 32 +//static volatile int16_t *M4_Desired_pos = (int16_t *)&SPI_rx_buffer[18]; //2 byte - 18 of 32 +//static volatile int16_t *M4_Desired_speed = (int16_t *)&SPI_rx_buffer[20]; //2 byte - 20 of 32 +//static volatile int16_t *M4_Desired_current = (int16_t *)&SPI_rx_buffer[22]; //2 byte - 22 of 32 +//static volatile int16_t *M4_Max_pos = (int16_t *)&SPI_rx_buffer[24]; //2 byte - 24 of 32 +//static volatile int16_t *M4_Max_velocity = (int16_t *)&SPI_rx_buffer[26]; //2 byte - 26 of 32 +//static volatile int16_t *M4_Max_current = (int16_t *)&SPI_rx_buffer[28]; //2 byte - 28 of 32 +//static volatile int16_t *M4_Spare = (int16_t *)&SPI_rx_buffer[30]; //2 byte - 30 of 32 + + static void update_telemetry(void) @@ -91,7 +157,8 @@ static void update_telemetry(void) //*M3_Mode = 0; /* Motor 1 */ - *M3_Status = Motor1.motor_state.currentstate; + *M3_Status = Motor1.motor_state.fault; + *M3_Mode = Motor1.motor_state.currentstate; *M3_Joint_rel_position = Motor1.motor_status.Num_Steps; //*M3_Joint_abs_position = ((int16_t *)&QSPI_tx_buffer[1]; //*M3_Motor_speed = (((int16_t *)&QSPI_tx_buffer[1]+1); @@ -104,7 +171,8 @@ static void update_telemetry(void) *M3_Motor_speed = (int16_t)Motor1.motor_status.calc_rpm; //*M3_Joint_abs_position = ; /* Motor 2 */ - *M4_Status = Motor2.motor_state.currentstate; + *M4_Status = Motor2.motor_state.fault; + *M4_Mode = Motor2.motor_state.currentstate; *M4_Joint_rel_position = Motor2.motor_status.Num_Steps; //*M3_Joint_abs_position = ((int16_t *)&QSPI_tx_buffer[1]; //*M3_Motor_speed = (((int16_t *)&QSPI_tx_buffer[1]+1); @@ -134,6 +202,7 @@ static void update_setpoints(void) Motor2.motor_setpoints.max_torque = *M4_Max_current; Motor2.motor_setpoints.max_velocity = *M4_Max_velocity; + volatile int y = 0; //volatile uint8_t a = *M3_Control_mode; //volatile uint8_t b = *M3_Control_set; //volatile int16_t c = *M3_Desired_pos; diff --git a/2_Motor_Slave/Motor_Slave/Motor_Slave/Motor_Slave.cproj b/2_Motor_Slave/Motor_Slave/Motor_Slave/Motor_Slave.cproj index ccfc355..359c057 100644 --- a/2_Motor_Slave/Motor_Slave/Motor_Slave/Motor_Slave.cproj +++ b/2_Motor_Slave/Motor_Slave/Motor_Slave/Motor_Slave.cproj @@ -212,7 +212,7 @@ - + @@ -389,6 +389,7 @@ %24(PackRepoDir)\arm\CMSIS\5.4.0\CMSIS\Core\Include\ + %24(PackRepoDir)\atmel\SAME51_DFP\1.1.139\include ../Config ../ ../examples @@ -413,7 +414,6 @@ ../hpl/tcc ../hri ../bosch_sensor - %24(PackRepoDir)\atmel\SAME51_DFP\1.1.139\include True @@ -440,6 +440,7 @@ %24(PackRepoDir)\arm\CMSIS\5.4.0\CMSIS\Core\Include\ + %24(PackRepoDir)\atmel\SAME51_DFP\1.1.139\include ../Config ../ ../examples @@ -464,13 +465,13 @@ ../hpl/tcc ../hri ../bosch_sensor - %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 @@ -495,7 +496,6 @@ ../hpl/tcc ../hri ../bosch_sensor - %24(PackRepoDir)\atmel\SAME51_DFP\1.1.139\include Default (-Wa,-g) diff --git a/2_Motor_Slave/Motor_Slave/Motor_Slave/bldc.c b/2_Motor_Slave/Motor_Slave/Motor_Slave/bldc.c index 8520781..fba907f 100644 --- a/2_Motor_Slave/Motor_Slave/Motor_Slave/bldc.c +++ b/2_Motor_Slave/Motor_Slave/Motor_Slave/bldc.c @@ -9,7 +9,7 @@ #include "statemachine.h" #include "utilities.h" -#include "Ethercat_SlaveDef.h" + void motor_StateMachine(BLDCMotor_t* const motor) { @@ -38,7 +38,7 @@ void motor_StateMachine(BLDCMotor_t* const motor) motor->motor_state.currentstate = MOTOR_PVI_CTRL_STATE; break; case MOTOR_OPEN_LOOP_STATE: - BLDC_runOpenLoop(motor, 0); + BLDC_runOpenLoop(motor, 350); calculate_motor_speed(motor); motor->motor_state.previousstate = motor->motor_state.currentstate; break; @@ -77,6 +77,9 @@ void motor_StateMachine(BLDCMotor_t* const motor) if(motor->regulation_loop_count > 23) motor->regulation_loop_count = 0; else motor->regulation_loop_count++; break; + case MOTOR_FAULT: + disable_phases(motor); + break; } //end switch (motor->motor_state.currentstate) // ---------------------------------------------------------------------- @@ -357,11 +360,25 @@ void calculate_motor_speed(BLDCMotor_t* const motor) } +void disable_phases(BLDCMotor_t* const motor) +{ + Tcc * tmp = (Tcc *)motor->motor_param->pwm_desc->device.hw; + tmp->PATTBUF.reg = DISABLE_PATTERN; +} + //------------------------------------------------------------------------------ // pi current control //------------------------------------------------------------------------------ void BLDC_runCurrentCntl(BLDCMotor_t *motor, const float32_t curfbk, const float32_t curRef) { + + if (curfbk > DEVICE_SHUNT_CURRENT_A) + { + motor->motor_state.currentstate = MOTOR_FAULT; + motor->motor_state.fault = MOTOR_CURRENT_OVERSCALE; + } + + motor->controllers.Pi_Idc.Fbk_pu = f_clamp(curfbk, -DEVICE_SHUNT_CURRENT_A, DEVICE_SHUNT_CURRENT_A); // Clamped to max current sensor readingspeedfbk; motor->controllers.Pi_Idc.Ref_pu = f_clamp(curRef, -motor->motor_param->motor_Max_Current_IDC_A, motor->motor_param->motor_Max_Current_IDC_A); // Clamp desired to Motor Max Current i_ref_clamped; @@ -460,6 +477,16 @@ volatile uint8_t readHallSensorM1(void) 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)); + + //if(motor_read == INVALID_HALL_7) { + //Motor1.motor_state.currentstate = MOTOR_FAULT; + //Motor1.motor_state.fault = MOTOR_HALLSENSORINVALID; + ////applicationStatus.currentstate = APP_FAULT; + //} + + + + return motor_read; //volatile uint8_t a = gpio_get_pin_level(M1_HALL_A_PIN); @@ -479,13 +506,14 @@ 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; + + //if(motor_read == INVALID_HALL_7) { //Motor2.motor_state.currentstate = MOTOR_FAULT; + //Motor2.motor_state.fault = MOTOR_HALLSENSORINVALID; ////applicationStatus.currentstate = APP_FAULT; //} + return motor_read; //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); @@ -503,15 +531,17 @@ volatile uint8_t readHallSensorM2(void) // ---------------------------------------------------------------------- void read_zero_current_offset_value(BLDCMotor_t *motor1, BLDCMotor_t *motor2) { - uint32_t phase_A_zero_current_offset_temp = 0; - uint32_t phase_B_zero_current_offset_temp = 0; - volatile uint16_t zero_current_offset_temp[2] = {0,0}; - uint8_t samples = 32; + volatile int32_t phase_A_zero_current_offset_temp = 0; + volatile int32_t phase_B_zero_current_offset_temp = 0; + volatile int16_t zero_current_offset_temp[2] = {0,0}; + const uint8_t samples = 16; uint8_t i; - // ------------------------- Motor 1 --------------------------------- + // ------------------------------------------------------------------ + // Motor 1 + // ------------------------------------------------------------------- + adc_sync_enable_channel(&ADC_1, 9); - //adc_sync_enable_channel(&ADC_1, 0); /* Single ended */ //ADC1->INPUTCTRL.reg = 0x1809; @@ -521,35 +551,33 @@ void read_zero_current_offset_value(BLDCMotor_t *motor1, BLDCMotor_t *motor2) for (i=0; iSTATUS.bit.ADCBUSY) {}; /* Wait for bus synchronization. */ ADC1->SWTRIG.bit.START = true; /* Start the ADC using a software trigger. */ - while (ADC1->INTFLAG.bit.RESRDY == 0); /* Wait for the result ready flag to be set. */ + while (ADC1->INTFLAG.bit.RESRDY == 0){}; /* Wait for the result ready flag to be set. */ + zero_current_offset_temp[0] = (int16_t)ADC1->RESULT.reg; /* Read the value. */ ADC1->INTFLAG.reg = ADC_INTFLAG_RESRDY; /* Clear the flag. */ - zero_current_offset_temp[0] = ADC1->RESULT.reg; /* Read the value. */ - - phase_A_zero_current_offset_temp += zero_current_offset_temp[0]; + phase_A_zero_current_offset_temp += (int32_t)zero_current_offset_temp[0]; } /* Set Motor Variables */ motor1->Voffset_lsb.A = phase_A_zero_current_offset_temp/samples; - + adc_sync_disable_channel(&ADC_1, 9); + + adc_sync_enable_channel(&ADC_1, 8); /* Single ended */ //ADC1->INPUTCTRL.reg = 0x1808; /* Differential */ ADC1->INPUTCTRL.reg = 0x0088; while (ADC1->STATUS.bit.ADCBUSY) {}; /* Wait for bus synchronization. */ - + for (i=0; iSTATUS.bit.ADCBUSY) {}; /* Wait for bus synchronization. */ ADC1->SWTRIG.bit.START = true; /* Start the ADC using a software trigger. */ - while (ADC1->INTFLAG.bit.RESRDY == 0); /* Wait for the result ready flag to be set. */ - ADC1->INTFLAG.reg = ADC_INTFLAG_RESRDY; /* Clear the flag. */ - zero_current_offset_temp[1] = ADC1->RESULT.reg; /* Read the value. */ - - phase_B_zero_current_offset_temp += zero_current_offset_temp[1]; + while (ADC1->INTFLAG.bit.RESRDY == 0){}; /* Wait for the result ready flag to be set. */ + zero_current_offset_temp[1] = (int16_t)ADC1->RESULT.reg; /* Read the value. */ + ADC1->INTFLAG.reg = ADC_INTFLAG_RESRDY; /* Clear the flag. */ + phase_B_zero_current_offset_temp += (int32_t)zero_current_offset_temp[1]; } @@ -557,13 +585,23 @@ void read_zero_current_offset_value(BLDCMotor_t *motor1, BLDCMotor_t *motor2) motor1->Voffset_lsb.B = phase_B_zero_current_offset_temp/samples; adc_sync_disable_channel(&ADC_1, 8); - adc_sync_enable_channel(&ADC_1, 7); + //adc_sync_enable_channel(&ADC_1, 0); + if ((abs(motor1->Voffset_lsb.A) > MAX_CUR_SENSE_OFFSET) || (abs(motor1->Voffset_lsb.B) > MAX_CUR_SENSE_OFFSET)) + { + motor1->motor_state.currentstate = MOTOR_FAULT; + motor1->motor_state.fault = MOTOR_CURRENT_SENSOR; + + } + + // ------------------------------------------------------------------ + // Motor 2 + // ------------------------------------------------------------------- phase_A_zero_current_offset_temp = 0; - phase_B_zero_current_offset_temp = 0; - + phase_B_zero_current_offset_temp = 0; + adc_sync_enable_channel(&ADC_1, 7); /* Single ended */ //ADC1->INPUTCTRL.reg = 0x1807; /* Differential */ @@ -572,39 +610,47 @@ void read_zero_current_offset_value(BLDCMotor_t *motor1, BLDCMotor_t *motor2) for (i=0; iSTATUS.bit.ADCBUSY) {}; /* Wait for bus synchronization. */ ADC1->SWTRIG.bit.START = true; /* Start the ADC using a software trigger. */ while (ADC1->INTFLAG.bit.RESRDY == 0); /* Wait for the result ready flag to be set. */ ADC1->INTFLAG.reg = ADC_INTFLAG_RESRDY; /* Clear the flag. */ - zero_current_offset_temp[0] = ADC1->RESULT.reg; /* Read the value. */ + zero_current_offset_temp[0] = (int16_t)ADC1->RESULT.reg; /* Read the value. */ - phase_A_zero_current_offset_temp += zero_current_offset_temp[0]; + phase_A_zero_current_offset_temp += (int32_t)zero_current_offset_temp[0]; } /* Set Motor Variables */ motor2->Voffset_lsb.A = phase_A_zero_current_offset_temp/samples; + adc_sync_disable_channel(&ADC_1, 7); + adc_sync_enable_channel(&ADC_1, 6); /* Single ended */ //ADC1->INPUTCTRL.reg = 0x1806; /* Differential */ ADC1->INPUTCTRL.reg = 0x0086; while (ADC1->STATUS.bit.ADCBUSY) {}; /* Wait for bus synchronization. */ - + for (i=0; iSTATUS.bit.ADCBUSY) {}; /* Wait for bus synchronization. */ ADC1->SWTRIG.bit.START = true; /* Start the ADC using a software trigger. */ while (ADC1->INTFLAG.bit.RESRDY == 0); /* Wait for the result ready flag to be set. */ ADC1->INTFLAG.reg = ADC_INTFLAG_RESRDY; /* Clear the flag. */ - zero_current_offset_temp[1] = ADC1->RESULT.reg; /* Read the value. */ + zero_current_offset_temp[1] = (int16_t)ADC1->RESULT.reg; /* Read the value. */ - phase_B_zero_current_offset_temp += zero_current_offset_temp[1]; + phase_B_zero_current_offset_temp += (int32_t)zero_current_offset_temp[1]; } /* Set Motor Variables */ motor2->Voffset_lsb.B = phase_B_zero_current_offset_temp/samples; adc_sync_disable_channel(&ADC_1, 6); //adc_sync_disable_channel(&ADC_1, 0); + + if ((abs(motor2->Voffset_lsb.A) > MAX_CUR_SENSE_OFFSET) || (abs(motor2->Voffset_lsb.B) > MAX_CUR_SENSE_OFFSET)) + { + motor2->motor_state.currentstate = MOTOR_FAULT; + motor2->motor_state.fault = MOTOR_CURRENT_SENSOR; + } + + } \ No newline at end of file diff --git a/2_Motor_Slave/Motor_Slave/Motor_Slave/bldc.h b/2_Motor_Slave/Motor_Slave/Motor_Slave/bldc.h index 55b82cc..e416a1c 100644 --- a/2_Motor_Slave/Motor_Slave/Motor_Slave/bldc.h +++ b/2_Motor_Slave/Motor_Slave/Motor_Slave/bldc.h @@ -35,7 +35,7 @@ // ---------------------------------------------------------------------- // ADC Parameters // ---------------------------------------------------------------------- -#define ADC_VOLTAGE_REFERENCE (3.3f) +#define ADC_VOLTAGE_REFERENCE (3.0f) #define ADC_RESOLUTION (12) #define ADC_MAX_COUNTS (1<SPI.CTRLC.bit.ICSPACE = 5; + SERCOM1->SPI.CTRLC.bit.DATA32B= true; spi_s_sync_enable(&SPI_1_MSIF); } @@ -181,24 +184,24 @@ void init_spi_slave_dma_descriptors() _dma_set_source_address(CONF_SERCOM_1_RECEIVE_DMA_CHANNEL, (uint32_t *)&(((SercomSpi *)(SPI_1_MSIF.dev.prvt))->DATA.reg)); _dma_set_destination_address(CONF_SERCOM_1_RECEIVE_DMA_CHANNEL, &SPI_rx_buffer[0]); - _dma_set_data_amount(CONF_SERCOM_1_RECEIVE_DMA_CHANNEL, SLAVE_BUFFER_SIZE); + _dma_set_data_amount(CONF_SERCOM_1_RECEIVE_DMA_CHANNEL, SLAVE_BUFFER_SIZE_LONG); _dma_set_source_address(CONF_SERCOM_1_TRANSMIT_DMA_CHANNEL, &SPI_tx_buffer[0]); _dma_set_destination_address(CONF_SERCOM_1_TRANSMIT_DMA_CHANNEL, (uint32_t *)&(((SercomSpi *)(SPI_1_MSIF.dev.prvt))->DATA.reg)); - _dma_set_data_amount(CONF_SERCOM_1_TRANSMIT_DMA_CHANNEL, SLAVE_BUFFER_SIZE); + _dma_set_data_amount(CONF_SERCOM_1_TRANSMIT_DMA_CHANNEL, SLAVE_BUFFER_SIZE_LONG); hri_dmacdescriptor_set_BTCTRL_VALID_bit(&_descriptor_section[CONF_SERCOM_1_RECEIVE_DMA_CHANNEL]); hri_dmacdescriptor_set_BTCTRL_VALID_bit(&_descriptor_section[CONF_SERCOM_1_TRANSMIT_DMA_CHANNEL]); /* callback */ - //struct _dma_resource *resource_rx, *resource_tx; - //_dma_get_channel_resource(&resource_rx, CONF_SERCOM_1_RECEIVE_DMA_CHANNEL); - //_dma_get_channel_resource(&resource_tx, CONF_SERCOM_1_TRANSMIT_DMA_CHANNEL); - //resource_rx->dma_cb.transfer_done = spi_slave_rx_complete_cb; + struct _dma_resource *resource_rx, *resource_tx; + _dma_get_channel_resource(&resource_rx, CONF_SERCOM_1_RECEIVE_DMA_CHANNEL); + _dma_get_channel_resource(&resource_tx, CONF_SERCOM_1_TRANSMIT_DMA_CHANNEL); + resource_rx->dma_cb.transfer_done = b2bTransferComplete_cb; //resource_tx->dma_cb.transfer_done = spi_slave_tx_complete_cb; - - /* Enable DMA transfer complete interrupt */ - //_dma_set_irq_state(CONF_SERCOM_1_RECEIVE_DMA_CHANNEL, DMA_TRANSFER_COMPLETE_CB, true); +// + ///* Enable DMA transfer complete interrupt */ + _dma_set_irq_state(CONF_SERCOM_1_RECEIVE_DMA_CHANNEL, DMA_TRANSFER_COMPLETE_CB, true); } @@ -209,12 +212,12 @@ void spi_s_sync_enable_ss_detect(void *hw, bool state) NVIC_ClearPendingIRQ((IRQn_Type)SERCOM1_1_IRQn); NVIC_EnableIRQ((IRQn_Type)SERCOM1_1_IRQn); if (state) { - hri_sercomspi_set_INTEN_TXC_bit(hw); + //hri_sercomspi_set_INTEN_TXC_bit(hw); //hri_sercomspi_set_INTEN_SSL_bit(hw); //hri_sercomspi_set_INTEN_SSL_bit(hw); //SERCOM_SPI_INTENSET_SSL } else { - hri_sercomspi_clear_INTEN_TXC_bit(hw); + //hri_sercomspi_clear_INTEN_TXC_bit(hw); //hri_sercomspi_clear_INTEN_SSL_bit(hw); } } diff --git a/2_Motor_Slave/Motor_Slave/Motor_Slave/hpl/dmac/hpl_dmac.c b/2_Motor_Slave/Motor_Slave/Motor_Slave/hpl/dmac/hpl_dmac.c index c7b03b0..b08448b 100644 --- a/2_Motor_Slave/Motor_Slave/Motor_Slave/hpl/dmac/hpl_dmac.c +++ b/2_Motor_Slave/Motor_Slave/Motor_Slave/hpl/dmac/hpl_dmac.c @@ -216,10 +216,10 @@ static void _dmac_handler(void) uint8_t channel = hri_dmac_get_INTPEND_reg(DMAC, DMAC_INTPEND_ID_Msk); struct _dma_resource *tmp_resource = &_resources[channel]; - if (hri_dmac_get_INTPEND_TERR_bit(DMAC)) { + if (hri_dmac_get_CHINTFLAG_TERR_bit(DMAC, channel)) { hri_dmac_clear_CHINTFLAG_TERR_bit(DMAC, channel); tmp_resource->dma_cb.error(tmp_resource); - } else if (hri_dmac_get_INTPEND_TCMPL_bit(DMAC)) { + } else if (hri_dmac_get_CHINTFLAG_TCMPL_bit(DMAC, channel)) { hri_dmac_clear_CHINTFLAG_TCMPL_bit(DMAC, channel); tmp_resource->dma_cb.transfer_done(tmp_resource); } diff --git a/2_Motor_Slave/Motor_Slave/Motor_Slave/interrupts.h b/2_Motor_Slave/Motor_Slave/Motor_Slave/interrupts.h index a9f7d66..7fa7e33 100644 --- a/2_Motor_Slave/Motor_Slave/Motor_Slave/interrupts.h +++ b/2_Motor_Slave/Motor_Slave/Motor_Slave/interrupts.h @@ -59,7 +59,18 @@ static void M2_RESET_BAR(void) volatile int x = 0; } - +// ---------------------------------------------------------------------- +// Master/Slave IF Callback +// ---------------------------------------------------------------------- +static void b2bTransferComplete_cb(struct _dma_resource *resource) +{ + + DMAC->Channel[0].CHCTRLA.reg |= DMAC_CHCTRLA_ENABLE; + DMAC->Channel[3].CHCTRLA.reg |= DMAC_CHCTRLA_ENABLE; + volatile int x = 0; + //PORT->Group[GPIO_PORTB].OUTCLR.reg = (1<SS_pin); + //gpio_set_pin_level(SPI1_CS, true); +} diff --git a/2_Motor_Slave/Motor_Slave/Motor_Slave/main.c b/2_Motor_Slave/Motor_Slave/Motor_Slave/main.c index d884679..abb5ab7 100644 --- a/2_Motor_Slave/Motor_Slave/Motor_Slave/main.c +++ b/2_Motor_Slave/Motor_Slave/Motor_Slave/main.c @@ -86,9 +86,7 @@ void SERCOM1_1_Handler() //SERCOM1->SPI.INTFLAG.bit.TXC = 0x01; //SPI_tx_buffer[0] += 1; //tx_buffer[31] += 1; - - DMAC->Channel[CONF_SERCOM_1_RECEIVE_DMA_CHANNEL].CHCTRLA.reg |= DMAC_CHCTRLA_ENABLE; - DMAC->Channel[CONF_SERCOM_1_TRANSMIT_DMA_CHANNEL].CHCTRLA.reg |= DMAC_CHCTRLA_ENABLE; + //_dma_enable_transaction(CONF_SERCOM_1_RECEIVE_DMA_CHANNEL, false); //_dma_enable_transaction(CONF_SERCOM_1_TRANSMIT_DMA_CHANNEL, false); @@ -108,7 +106,7 @@ void SERCOM1_3_Handler() //tx_buffer[0] += 1; //tx_buffer[31] += 1; - + // //DMAC->Channel[0].CHCTRLA.reg |= DMAC_CHCTRLA_ENABLE; //DMAC->Channel[1].CHCTRLA.reg |= DMAC_CHCTRLA_ENABLE; //_dma_enable_transaction(CONF_SERCOM_1_RECEIVE_DMA_CHANNEL, false); @@ -138,10 +136,10 @@ void enable_NVIC_IRQ(void) NVIC_SetPriority(ADC1_0_IRQn, 3); NVIC_EnableIRQ(TCC0_0_IRQn); NVIC_EnableIRQ(TCC1_0_IRQn); - //NVIC_EnableIRQ(SERCOM1_3_IRQn); + NVIC_EnableIRQ(SERCOM1_3_IRQn); //NVIC_SetPriority(SERCOM1_3_IRQn, 0); - NVIC_EnableIRQ(SERCOM1_1_IRQn); - NVIC_SetPriority(SERCOM1_1_IRQn, 1); + //NVIC_EnableIRQ(SERCOM1_1_IRQn); + //NVIC_SetPriority(SERCOM1_1_IRQn, 1); //NVIC_EnableIRQ(SERCOM1_3_IRQn); //NVIC_EnableIRQ(EIC_5_IRQn); } @@ -177,7 +175,7 @@ void APPLICATION_StateMachine(void) //applicationStatus.currentstate ; //comms_check(); motor_StateMachine(&Motor1); - //motor_StateMachine(&Motor2); + motor_StateMachine(&Motor2); break; case APP_FAULT: //DisableGateDrivers(&Motor1); @@ -216,16 +214,19 @@ int main(void) { /* Initializes MCU, drivers and middleware */ atmel_start_init(); + + BldcInitStruct(&Motor1, &FH_22mm24BXTR); - BldcInitStruct(&Motor2, &FH_32mm24BXTR); + BldcInitStruct(&Motor2, &FH_22mm24BXTR_temp); Motor1.readHall = &readHallSensorM1; Motor2.readHall = &readHallSensorM2; read_zero_current_offset_value(&Motor1, &Motor2); + __disable_irq(); //config_qspi(); configure_tcc_pwm(); adc_sync_enable_channel(&ADC_1, 6); //ECAT_STATE_MACHINE(); - adc_init_dma(); + //adc_init_dma(); boardToBoardTransferInit(); init_spi_slave_dma_descriptors(); @@ -233,6 +234,7 @@ int main(void) One_ms_timer_init(); custom_logic_enable(); enable_NVIC_IRQ(); + __enable_irq(); DMAC->Channel[CONF_SERCOM_1_RECEIVE_DMA_CHANNEL].CHCTRLA.reg |= DMAC_CHCTRLA_ENABLE; DMAC->Channel[CONF_SERCOM_1_TRANSMIT_DMA_CHANNEL].CHCTRLA.reg |= DMAC_CHCTRLA_ENABLE; @@ -250,10 +252,11 @@ int main(void) /* Replace with your application code */ while (1) { if (Motor1.timerflags.adc_readings_ready_tic) {process_currents();} + if (Motor1.timerflags.current_loop_tic) { APPLICATION_StateMachine(); exec_commutation(&Motor1); - ////exec_commutation(&Motor2); + exec_commutation(&Motor2); } if (Motor1.timerflags.motor_telemetry_flag) { diff --git a/2_Motor_Slave/Motor_Slave/Motor_Slave/motorparameters.h b/2_Motor_Slave/Motor_Slave/Motor_Slave/motorparameters.h index 8491f2d..38df29d 100644 --- a/2_Motor_Slave/Motor_Slave/Motor_Slave/motorparameters.h +++ b/2_Motor_Slave/Motor_Slave/Motor_Slave/motorparameters.h @@ -163,6 +163,30 @@ const static BLDCMotor_param_t FH_22mm24BXTR = { .motor_MaxPWM = 800.0, }; +/* Small Motor - 2214S024BXTR*/ +const static BLDCMotor_param_t FH_22mm24BXTR_temp = { + .pwm_desc = &PWM_1, + .speedtimer_hw = TC4, + .motor_Poles = 14, + .motor_polePairs = 7, + .motor_commutationStates = 42, //polePairs * 6 + .motor_RS_Ohm = 25.9, + .motor_LD_H = 0.003150, + .motor_LQ_H = 0.003150, + .motor_Flux_WB = 0.001575, + .motor_Max_Spd_RPM = 3000, + .motor_MeasureRange_RPM = 3000 * 1.2, //(1.2f * MOTOR_MAX_SPD_RPM)f // give 20% headroom + .motor_Max_Spd_ELEC = (3000/60)*7.0, //(MOTOR_MAX_SPD_RPM/60)*MOTOR_POLEPAIRS + //.motor_Max_Current_IDC_A = 0.368, + .motor_Max_Current_IDC_A = 0.180, + .controller_param.Pid_Speed.Kp = 0.00008f, + .controller_param.Pid_Speed.Ki = 0.0000001f, + //.controller_param.Pid_Speed.Ki = 0.0000001f, + .controller_param.Pi_Pos.Kp = 50.0f, + .controller_param.Pi_Pos.Ki = 0.0f, + .motor_MaxPWM = 800.0, +}; + /* Big Motor - 3216W012BXTR */ const static BLDCMotor_param_t FH_32mm12BXTR = { diff --git a/2_Motor_Slave/Motor_Slave/Motor_Slave/statemachine.h b/2_Motor_Slave/Motor_Slave/Motor_Slave/statemachine.h index 3bbb186..cb2aeff 100644 --- a/2_Motor_Slave/Motor_Slave/Motor_Slave/statemachine.h +++ b/2_Motor_Slave/Motor_Slave/Motor_Slave/statemachine.h @@ -59,9 +59,11 @@ typedef enum typedef enum { - MOTOR_NOFAULT = 0xE1, - MOTOR_HALLSENSORINVALID = 0xE2, - MOTOR_DRIVER_OVER_CURRENT = 0xE3, + MOTOR_NOFAULT = 0x0E, + MOTOR_HALLSENSORINVALID = 0xE1, + MOTOR_DRIVER_OVER_CURRENT = 0xE2, + MOTOR_CURRENT_SENSOR = 0xE3, + MOTOR_CURRENT_OVERSCALE = 0xE4, } MOTOR_FAULTS_t; typedef struct MOTOR_STATE diff --git a/Twincat/MotorData/.vs/MotorData/v15/.suo b/Twincat/MotorData/.vs/MotorData/v15/.suo index fd8227e..4b47388 100644 Binary files a/Twincat/MotorData/.vs/MotorData/v15/.suo and b/Twincat/MotorData/.vs/MotorData/v15/.suo differ diff --git a/Twincat/MotorData/MotorData/Motordata_PLC/GVLs/GVL_motor_data.TcGVL b/Twincat/MotorData/MotorData/Motordata_PLC/GVLs/GVL_motor_data.TcGVL index 97d429a..88cf66f 100644 --- a/Twincat/MotorData/MotorData/Motordata_PLC/GVLs/GVL_motor_data.TcGVL +++ b/Twincat/MotorData/MotorData/Motordata_PLC/GVLs/GVL_motor_data.TcGVL @@ -80,47 +80,47 @@ Pressure_CH3 AT %I* : INT; //Read From Ecat Total (XX Bytes) //QSPI_rx_buffer // Motor 1 // -M1_Control_mode AT %Q* : BYTE := 0; -M1_Control_set AT %Q* : BYTE := 0; -M1_Desired_pos AT %Q* : INT := 0; -M1_Desired_speed AT %Q* : INT := 0; -M1_Desired_current AT %Q* : INT := 0; -M1_Max_pos AT %Q* : INT := 0; -M1_Max_velocity AT %Q* : INT := 0; -M1_Max_current AT %Q* : INT := 0; -M1_Desired_dc AT %Q* : INT := 0; +M1_Control_mode AT %Q* : BYTE ; +M1_Control_set AT %Q* : BYTE ; +M1_Desired_pos AT %Q* : INT ; +M1_Desired_speed AT %Q* : INT ; +M1_Desired_current AT %Q* : INT ; +M1_Max_pos AT %Q* : INT ; +M1_Max_velocity AT %Q* : INT ; +M1_Max_current AT %Q* : INT ; +M1_Desired_dc AT %Q* : INT ; ///* Motor 2*/ -M2_Control_mode AT %Q* : BYTE := 0; -M2_Control_set AT %Q* : BYTE := 0; -M2_Desired_pos AT %Q* : INT := 0; -M2_Desired_speed AT %Q* : INT := 0; -M2_Desired_current AT %Q* : INT := 0; -M2_Max_pos AT %Q* : INT := 0; -M2_Max_velocity AT %Q* : INT := 0; -M2_Max_current AT %Q* : INT := 0; -M2_Desired_dc AT %Q* : INT := 0; +M2_Control_mode AT %Q* : BYTE ; +M2_Control_set AT %Q* : BYTE ; +M2_Desired_pos AT %Q* : INT ; +M2_Desired_speed AT %Q* : INT ; +M2_Desired_current AT %Q* : INT ; +M2_Max_pos AT %Q* : INT ; +M2_Max_velocity AT %Q* : INT ; +M2_Max_current AT %Q* : INT ; +M2_Desired_dc AT %Q* : INT ; ///* Motor 3*/ -M3_Control_mode AT %Q* : BYTE := 0; -M3_Control_set AT %Q* : BYTE := 0; -M3_Desired_pos AT %Q* : INT := 0; -M3_Desired_speed AT %Q* : INT := 0; -M3_Desired_current AT %Q* : INT := 0; -M3_Max_pos AT %Q* : INT := 0; -M3_Max_velocity AT %Q* : INT := 0; -M3_Max_current AT %Q* : INT := 0; -M3_Desired_dc AT %Q* : INT := 0; +M3_Control_mode AT %Q* : BYTE ; +M3_Control_set AT %Q* : BYTE ; +M3_Desired_pos AT %Q* : INT ; +M3_Desired_speed AT %Q* : INT ; +M3_Desired_current AT %Q* : INT ; +M3_Max_pos AT %Q* : INT ; +M3_Max_velocity AT %Q* : INT ; +M3_Max_current AT %Q* : INT ; +M3_Desired_dc AT %Q* : INT ; ///* Motor 4*/ -M4_Control_mode AT %Q* : BYTE := 0; -M4_Control_set AT %Q* : BYTE := 0; -M4_Desired_pos AT %Q* : INT := 0; -M4_Desired_speed AT %Q* : INT := 0; -M4_Desired_current AT %Q* : INT := 0; -M4_Max_pos AT %Q* : INT := 0; -M4_Max_velocity AT %Q* : INT := 0; -M4_Max_current AT %Q* : INT := 0; -M4_Desired_dc AT %Q* : INT := 0; +M4_Control_mode AT %Q* : BYTE ; +M4_Control_set AT %Q* : BYTE ; +M4_Desired_pos AT %Q* : INT ; +M4_Desired_speed AT %Q* : INT ; +M4_Desired_current AT %Q* : INT ; +M4_Max_pos AT %Q* : INT ; +M4_Max_velocity AT %Q* : INT ; +M4_Max_current AT %Q* : INT ; +M4_Desired_dc AT %Q* : INT ; END_VAR]]> diff --git a/Twincat/MotorData/MotorData/Motordata_PLC/POUs/POU_Position_Seq.TcPOU b/Twincat/MotorData/MotorData/Motordata_PLC/POUs/POU_Position_Seq.TcPOU index 7661377..eae8394 100644 --- a/Twincat/MotorData/MotorData/Motordata_PLC/POUs/POU_Position_Seq.TcPOU +++ b/Twincat/MotorData/MotorData/Motordata_PLC/POUs/POU_Position_Seq.TcPOU @@ -1103,7 +1103,8 @@ END_VAR +GVL_motor_data.M3_Desired_pos := 1000; +GVL_motor_data.M4_Desired_pos := 1000;]]> @@ -1123,7 +1124,8 @@ GVL_motor_data.M3_Desired_pos := 1000;]]> +GVL_motor_data.M3_Desired_pos := -500; +GVL_motor_data.M4_Desired_pos := -500;]]> @@ -1143,7 +1145,8 @@ GVL_motor_data.M3_Desired_pos := -500;]]> +GVL_motor_data.M3_Desired_pos := 0; +GVL_motor_data.M4_Desired_pos := 0;]]> @@ -1257,17 +1260,17 @@ GVL_motor_data.M3_Desired_pos := 0;]]> - + - + - + \ No newline at end of file diff --git a/Twincat/MotorData/Scope Project1.svdx b/Twincat/MotorData/Scope Project1.svdx index 821ca84..166e55a 100644 Binary files a/Twincat/MotorData/Scope Project1.svdx and b/Twincat/MotorData/Scope Project1.svdx differ diff --git a/Twincat/MotorData/TwinCAT Measurement Project1/Scope Project1.tcscopex b/Twincat/MotorData/TwinCAT Measurement Project1/Scope Project1.tcscopex index 560edf3..f4c53b4 100644 --- a/Twincat/MotorData/TwinCAT Measurement Project1/Scope Project1.tcscopex +++ b/Twincat/MotorData/TwinCAT Measurement Project1/Scope Project1.tcscopex @@ -15,8 +15,8 @@ false <?xml version="1.0" encoding="utf-16"?> <Layout> - <Window Guid="8766837b-106b-4ca8-84ce-2fbbc3ef10f3" LastFocused="132743765367691913" DockedSize="200" PopupSize="0" FloatingLocation="-1, -1" FloatingSize="550, 400" LastOpenDockSituation="Document" LastFixedDockSituation="Document" LastFixedDockLocation="Right" LastFloatingWindowGuid="00000000-0000-0000-0000-000000000000" LastDockContainerCount="0" LastDockContainerIndex="0" DockedWorkingSize="250, 400" DockedWindowGroupGuid="00000000-0000-0000-0000-000000000000" DockedIndexInWindowGroup="0" DockedSplitPath="0" DocumentWorkingSize="250, 400" DocumentWindowGroupGuid="a5d32a52-1886-4ce8-9970-731db69737a6" DocumentIndexInWindowGroup="0" DocumentSplitPath="0" FloatingWorkingSize="250, 400" FloatingWindowGroupGuid="00000000-0000-0000-0000-000000000000" FloatingIndexInWindowGroup="0" FloatingSplitPath="0" /> - <Window Guid="17812b7c-7d18-4668-ae12-d2633798b279" LastFocused="132743765596671663" DockedSize="200" PopupSize="0" FloatingLocation="-1, -1" FloatingSize="550, 400" LastOpenDockSituation="Document" LastFixedDockSituation="Document" LastFixedDockLocation="Right" LastFloatingWindowGuid="00000000-0000-0000-0000-000000000000" LastDockContainerCount="0" LastDockContainerIndex="0" DockedWorkingSize="250, 400" DockedWindowGroupGuid="00000000-0000-0000-0000-000000000000" DockedIndexInWindowGroup="0" DockedSplitPath="0" DocumentWorkingSize="250, 400" DocumentWindowGroupGuid="a5d32a52-1886-4ce8-9970-731db69737a6" DocumentIndexInWindowGroup="1" DocumentSplitPath="0" FloatingWorkingSize="250, 400" FloatingWindowGroupGuid="00000000-0000-0000-0000-000000000000" FloatingIndexInWindowGroup="0" FloatingSplitPath="0" /> + <Window Guid="8766837b-106b-4ca8-84ce-2fbbc3ef10f3" LastFocused="132744640175318045" DockedSize="200" PopupSize="0" FloatingLocation="-1, -1" FloatingSize="550, 400" LastOpenDockSituation="Document" LastFixedDockSituation="Document" LastFixedDockLocation="Right" LastFloatingWindowGuid="00000000-0000-0000-0000-000000000000" LastDockContainerCount="0" LastDockContainerIndex="0" DockedWorkingSize="250, 400" DockedWindowGroupGuid="00000000-0000-0000-0000-000000000000" DockedIndexInWindowGroup="0" DockedSplitPath="0" DocumentWorkingSize="250, 400" DocumentWindowGroupGuid="a5d32a52-1886-4ce8-9970-731db69737a6" DocumentIndexInWindowGroup="0" DocumentSplitPath="0" FloatingWorkingSize="250, 400" FloatingWindowGroupGuid="00000000-0000-0000-0000-000000000000" FloatingIndexInWindowGroup="0" FloatingSplitPath="0" /> + <Window Guid="17812b7c-7d18-4668-ae12-d2633798b279" LastFocused="132745396376071386" DockedSize="200" PopupSize="0" FloatingLocation="-1, -1" FloatingSize="550, 400" LastOpenDockSituation="Document" LastFixedDockSituation="Document" LastFixedDockLocation="Right" LastFloatingWindowGuid="00000000-0000-0000-0000-000000000000" LastDockContainerCount="0" LastDockContainerIndex="0" DockedWorkingSize="250, 400" DockedWindowGroupGuid="00000000-0000-0000-0000-000000000000" DockedIndexInWindowGroup="0" DockedSplitPath="0" DocumentWorkingSize="250, 400" DocumentWindowGroupGuid="a5d32a52-1886-4ce8-9970-731db69737a6" DocumentIndexInWindowGroup="1" DocumentSplitPath="0" FloatingWorkingSize="250, 400" FloatingWindowGroupGuid="00000000-0000-0000-0000-000000000000" FloatingIndexInWindowGroup="0" FloatingSplitPath="0" /> <DocumentContainer Dock="5"> <SplitLayoutSystem WorkingSize="250, 400" SplitMode="0"> <ControlLayoutSystem WorkingSize="250, 400" Guid="a5d32a52-1886-4ce8-9970-731db69737a6" Collapsed="0" SelectedControl="17812b7c-7d18-4668-ae12-d2633798b279">