diff --git a/CHANGELOG.md b/CHANGELOG.md new file mode 100644 index 00000000..62e96abb --- /dev/null +++ b/CHANGELOG.md @@ -0,0 +1,13 @@ + +## [0.1] - UNRELEASED +### Added +* Step/Dir interface +* this Changelog +* motor control interrupt timing diagram +* uint16 exposed variable type +* null termination to USB string parsing + +### Changed +* Fixed Resistance measurement bug +* Simplified motor control adc triggers +* Increased AUX bridge deadtime diff --git a/MotorControl/low_level.c b/MotorControl/low_level.c index 510a5179..aa553f9d 100755 --- a/MotorControl/low_level.c +++ b/MotorControl/low_level.c @@ -45,7 +45,7 @@ static float elec_rad_per_enc = POLE_PAIRS * 2 * M_PI * (1.0f / (float)ENCODER_C // TODO: For nice encapsulation, consider not having the motor objects public Motor_t motors[] = { { // M0 - .control_mode = CTRL_MODE_CURRENT_CONTROL, + .control_mode = CTRL_MODE_POSITION_CONTROL, //see: Motor_control_mode_t .enable_step_dir = false, //auto enabled after calibration .counts_per_step = 2.0f, .error = ERROR_NO_ERROR, @@ -62,8 +62,8 @@ Motor_t motors[] = { .phase_resistance = 0.0f, // to be set by measure_phase_resistance .motor_thread = 0, .thread_ready = false, - .enable_control = false, - .do_calibration = false, + .enable_control = true, + .do_calibration = true, .calibration_ok = false, .motor_timer = &htim1, .next_timings = {TIM_1_8_PERIOD_CLOCKS/2, TIM_1_8_PERIOD_CLOCKS/2, TIM_1_8_PERIOD_CLOCKS/2}, @@ -108,7 +108,7 @@ Motor_t motors[] = { .timing_log = {0} }, { // M1 - .control_mode = CTRL_MODE_CURRENT_CONTROL, + .control_mode = CTRL_MODE_POSITION_CONTROL, //see: Motor_control_mode_t .enable_step_dir = false, //auto enabled after calibration .counts_per_step = 2.0f, .error = ERROR_NO_ERROR, @@ -125,8 +125,8 @@ Motor_t motors[] = { .phase_resistance = 0.0f, // to be set by measure_phase_resistance .motor_thread = 0, .thread_ready = false, - .enable_control = false, - .do_calibration = false, + .enable_control = true, + .do_calibration = true, .calibration_ok = false, .motor_timer = &htim8, .next_timings = {TIM_1_8_PERIOD_CLOCKS/2, TIM_1_8_PERIOD_CLOCKS/2, TIM_1_8_PERIOD_CLOCKS/2}, @@ -590,10 +590,6 @@ static void start_adc_pwm(){ __HAL_DBGMCU_FREEZE_TIM1(); __HAL_DBGMCU_FREEZE_TIM8(); - // Turn off the regular conversion trigger for the inital phase - hadc2.Instance->CR2 &= ~ADC_CR2_EXTEN; - hadc3.Instance->CR2 &= ~ADC_CR2_EXTEN; - start_pwm(&htim1); start_pwm(&htim8); // TODO: explain why this offset @@ -708,7 +704,7 @@ void step_cb(uint16_t GPIO_Pin) { } } -void vbus_sense_adc_cb(ADC_HandleTypeDef* hadc) { +void vbus_sense_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { static const float voltage_scale = 3.3f * 11.0f / (float)(1<<12); // Only one conversion in sequence, so only rank1 uint32_t ADCValue = HAL_ADCEx_InjectedGetValue(hadc, ADC_INJECTED_RANK_1); @@ -717,7 +713,7 @@ void vbus_sense_adc_cb(ADC_HandleTypeDef* hadc) { // This is the callback from the ADC that we expect after the PWM has triggered an ADC conversion. // TODO: Document how the phasing is done, link to timing diagram -void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc) { +void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { #define calib_tau 0.2f //@TOTO make more easily configurable static const float calib_filter_k = CURRENT_MEAS_PERIOD / calib_tau; @@ -727,27 +723,17 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc) { return; }; + // Motor 0 is on Timer 1, which triggers ADC 2 and 3 on an injected conversion + // Motor 1 is on Timer 8, which triggers ADC 2 and 3 on a regular conversion + // If the corresponding timer is counting up, we just sampled in SVM vector 0, i.e. real current + // If we are counting down, we just sampled in SVM vector 7, with zero current + Motor_t* motor = injected ? &motors[0] : &motors[1]; + bool counting_down = motor->motor_timer->Instance->CR1 & TIM_CR1_DIR; + bool current_meas_not_DC_CAL; - Motor_t* motor; - - // Check if this trigger was the CC4 channel, used for actual current measurement at SVM vector 0 - // or the update trigger, which is used for DC_CAL measurement at SVM vector 7 - // M1 DC_CAL is a special case since due to hardware limitations, it uses the "regular" conversions - // rather than the injected ones. - uint32_t inj_src = hadc->Instance->CR2 & ADC_CR2_JEXTSEL; - uint32_t reg_edge = hadc->Instance->CR2 & ADC_CR2_EXTEN; - if (reg_edge != ADC_EXTERNALTRIGCONVEDGE_NONE) { + if (motor == &motors[1] && counting_down) { // We are measuring M1 DC_CAL here current_meas_not_DC_CAL = false; - motor = &motors[1]; - // Next measurement on this motor will be M1 current measurement - HAL_GPIO_WritePin(M1_DC_CAL_GPIO_Port, M1_DC_CAL_Pin, GPIO_PIN_RESET); - // Next measurement on this ADC will be M0 current - hadc->Instance->CR2 &= ~(ADC_CR2_JEXTEN | ADC_CR2_EXTEN | ADC_CR2_JEXTSEL); - hadc->Instance->CR2 |= (ADC_EXTERNALTRIGINJECCONVEDGE_RISING | ADC_EXTERNALTRIGINJECCONV_T1_CC4); - // Set ADC channels for next measurement - hadc->Instance->JSQR &= ~ADC_JSQR(ADC_JSQR_JSQ1, 1, 1); - hadc->Instance->JSQR |= ADC_JSQR((hadc == &hadc2) ? ADC_CHANNEL_10 : ADC_CHANNEL_11, 1, 1); // Load next timings for M0 (only once is sufficient) if (hadc == &hadc2) { motors[0].motor_timer->Instance->CCR1 = motors[0].next_timings[0]; @@ -757,18 +743,9 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc) { // Check the timing of the sequencing check_timing(motor); - } else if (inj_src == ADC_EXTERNALTRIGINJECCONV_T1_CC4) { + } else if (motor == &motors[0] && !counting_down) { // We are measuring M0 current here current_meas_not_DC_CAL = true; - motor = &motors[0]; - // Next measurement on this motor will be M0 DC_CAL measurement - HAL_GPIO_WritePin(M0_DC_CAL_GPIO_Port, M0_DC_CAL_Pin, GPIO_PIN_SET); - // Next measurement on this ADC will be M1 current - hadc->Instance->CR2 &= ~(ADC_CR2_JEXTEN | ADC_CR2_EXTEN | ADC_CR2_JEXTSEL); - hadc->Instance->CR2 |= (ADC_EXTERNALTRIGINJECCONVEDGE_RISING | ADC_EXTERNALTRIGINJECCONV_T8_CC4); - // Set ADC channels for next measurement - hadc->Instance->JSQR &= ~ADC_JSQR(ADC_JSQR_JSQ1, 1, 1); - hadc->Instance->JSQR |= ADC_JSQR((hadc == &hadc2) ? ADC_CHANNEL_13 : ADC_CHANNEL_12, 1, 1); // Load next timings for M1 (only once is sufficient) if (hadc == &hadc2) { motors[1].motor_timer->Instance->CCR1 = motors[1].next_timings[0]; @@ -778,33 +755,15 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc) { // Check the timing of the sequencing check_timing(motor); - } else if (inj_src == ADC_EXTERNALTRIGINJECCONV_T8_CC4) { + } else if (motor == &motors[1] && !counting_down) { // We are measuring M1 current here current_meas_not_DC_CAL = true; - motor = &motors[1]; - // Next measurement on this motor will be M1 DC_CAL measurement - HAL_GPIO_WritePin(M1_DC_CAL_GPIO_Port, M1_DC_CAL_Pin, GPIO_PIN_SET); - // Next measurement on this ADC will be M0 DC_CAL - hadc->Instance->CR2 &= ~(ADC_CR2_JEXTEN | ADC_CR2_EXTEN | ADC_CR2_JEXTSEL); - hadc->Instance->CR2 |= (ADC_EXTERNALTRIGINJECCONVEDGE_RISING | ADC_EXTERNALTRIGINJECCONV_T1_TRGO); - // Set ADC channels for next measurement - hadc->Instance->JSQR &= ~ADC_JSQR(ADC_JSQR_JSQ1, 1, 1); - hadc->Instance->JSQR |= ADC_JSQR((hadc == &hadc2) ? ADC_CHANNEL_10 : ADC_CHANNEL_11, 1, 1); // Check the timing of the sequencing check_timing(motor); - } else if (inj_src == ADC_EXTERNALTRIGINJECCONV_T1_TRGO) { + } else if (motor == &motors[0] && counting_down) { // We are measuring M0 DC_CAL here current_meas_not_DC_CAL = false; - motor = &motors[0]; - // Next measurement on this motor will be M0 current measurement - HAL_GPIO_WritePin(M0_DC_CAL_GPIO_Port, M0_DC_CAL_Pin, GPIO_PIN_RESET); - // Next measurement on this ADC will be M1 DC_CAL - hadc->Instance->CR2 &= ~(ADC_CR2_JEXTEN | ADC_CR2_EXTEN | ADC_CR2_JEXTSEL); - hadc->Instance->CR2 |= ADC_EXTERNALTRIGCONVEDGE_RISING; - // Set ADC channels for next measurement - hadc->Instance->JSQR &= ~ADC_JSQR(ADC_JSQR_JSQ1, 1, 1); - hadc->Instance->JSQR |= ADC_JSQR((hadc == &hadc2) ? ADC_CHANNEL_13 : ADC_CHANNEL_12, 1, 1); // Check the timing of the sequencing check_timing(motor); @@ -814,10 +773,10 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc) { } uint32_t ADCValue; - if (reg_edge != ADC_EXTERNALTRIGCONVEDGE_NONE) { - ADCValue = HAL_ADC_GetValue(hadc); - } else { + if (injected) { ADCValue = HAL_ADCEx_InjectedGetValue(hadc, ADC_INJECTED_RANK_1); + } else { + ADCValue = HAL_ADC_GetValue(hadc); } float current = phase_current_from_adcval(motor, ADCValue); @@ -873,7 +832,8 @@ static bool measure_phase_resistance(Motor_t* motor, float test_current, float m queue_voltage_timings(motor, test_voltage, 0.0f); // Check we meet deadlines after queueing - if (!(check_timing(motor) < motor->control_deadline)){ + motor->last_cpu_time = check_timing(motor); + if (!(motor->last_cpu_time < motor->control_deadline)){ motor->error = ERROR_PHASE_RESISTANCE_TIMING; return false; } @@ -1293,25 +1253,8 @@ void motor_thread(void const * argument) { motor->motor_thread = osThreadGetId(); motor->thread_ready = true; -#ifdef STANDALONE_MODE - //Only run tests on M0 for now - // if (motor == &motors[1]) { - // // TODO: figure out why M1 MOE must be enabled to run M0 correctly - // __HAL_TIM_MOE_ENABLE(motor->motor_timer); - // FOC_voltage_loop(motor, 0.0f, 0.0f); - // } - - motor->do_calibration = true; - motor->enable_control = true; - - //Turn on position control by default. - //NOTE: This may not be the preffered behaviour in your application. - set_pos_setpoint(motor, 0.0f, 0.0f, 0.0f); -#endif - for (;;) { if (motor->do_calibration) { - osDelay(10); __HAL_TIM_MOE_ENABLE(motor->motor_timer);// enable pwm outputs motor_calibration(motor); if(!motor->calibration_ok){ @@ -1321,7 +1264,6 @@ void motor_thread(void const * argument) { } if (motor->calibration_ok && motor->enable_control) { - osDelay(10); motor->enable_step_dir = true; __HAL_TIM_MOE_ENABLE(motor->motor_timer); control_motor_loop(motor); diff --git a/MotorControl/low_level.h b/MotorControl/low_level.h index c724bb7d..edfd2403 100644 --- a/MotorControl/low_level.h +++ b/MotorControl/low_level.h @@ -130,8 +130,8 @@ void set_current_setpoint(Motor_t* motor, float current_setpoint); void safe_assert(int arg); void init_motor_control(); void step_cb(uint16_t GPIO_Pin); -void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc); -void vbus_sense_adc_cb(ADC_HandleTypeDef* hadc); +void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected); +void vbus_sense_adc_cb(ADC_HandleTypeDef* hadc, bool injected); //@TODO move motor thread to high level file void motor_thread(void const * argument); diff --git a/Odrive.ioc b/Odrive.ioc index 183fe2a9..b7bd70f6 100755 --- a/Odrive.ioc +++ b/Odrive.ioc @@ -10,7 +10,7 @@ ADC1.EOCSelection=ADC_EOC_SINGLE_CONV ADC1.EnableAnalogWatchDog=false ADC1.ExternalTrigConv=ADC_SOFTWARE_START ADC1.ExternalTrigConvEdge=ADC_EXTERNALTRIGCONVEDGE_NONE -ADC1.ExternalTrigInjecConv=ADC_EXTERNALTRIGINJECCONV_T1_CC4 +ADC1.ExternalTrigInjecConv=ADC_EXTERNALTRIGINJECCONV_T1_TRGO ADC1.ExternalTrigInjecConvEdge=ADC_EXTERNALTRIGINJECCONVEDGE_RISING ADC1.IPParameters=Rank-0\#ChannelRegularConversion,Channel-0\#ChannelRegularConversion,SamplingTime-0\#ChannelRegularConversion,NbrOfConversionFlag,master,ClockPrescaler,Resolution,DataAlign,ScanConvMode,ContinuousConvMode,DiscontinuousConvMode,DMAContinuousRequests,EOCSelection,NbrOfConversion,ExternalTrigConvEdge,InjNumberOfConversion,EnableAnalogWatchDog,Rank-1\#ChannelInjectedConversion,Channel-1\#ChannelInjectedConversion,SamplingTime-1\#ChannelInjectedConversion,InjectedOffset-1\#ChannelInjectedConversion,InjectedConvMode,ExternalTrigInjecConvEdge,ExternalTrigInjecConv,ExternalTrigConv ADC1.InjNumberOfConversion=1 @@ -36,7 +36,7 @@ ADC2.EOCSelection=ADC_EOC_SINGLE_CONV ADC2.EnableAnalogWatchDog=false ADC2.ExternalTrigConv=ADC_EXTERNALTRIGCONV_T8_TRGO ADC2.ExternalTrigConvEdge=ADC_EXTERNALTRIGCONVEDGE_RISING -ADC2.ExternalTrigInjecConv=ADC_EXTERNALTRIGINJECCONV_T1_CC4 +ADC2.ExternalTrigInjecConv=ADC_EXTERNALTRIGINJECCONV_T1_TRGO ADC2.ExternalTrigInjecConvEdge=ADC_EXTERNALTRIGINJECCONVEDGE_RISING ADC2.IPParameters=Rank-0\#ChannelRegularConversion,Channel-0\#ChannelRegularConversion,SamplingTime-0\#ChannelRegularConversion,NbrOfConversionFlag,ClockPrescaler,Resolution,DataAlign,ScanConvMode,ContinuousConvMode,DiscontinuousConvMode,DMAContinuousRequests,EOCSelection,NbrOfConversion,InjNumberOfConversion,EnableAnalogWatchDog,Rank-1\#ChannelInjectedConversion,Channel-1\#ChannelInjectedConversion,SamplingTime-1\#ChannelInjectedConversion,InjectedOffset-1\#ChannelInjectedConversion,ExternalTrigInjecConvEdge,ExternalTrigConvEdge,InjectedConvMode,ExternalTrigInjecConv,ExternalTrigConv ADC2.InjNumberOfConversion=1 @@ -61,7 +61,7 @@ ADC3.EOCSelection=ADC_EOC_SINGLE_CONV ADC3.EnableAnalogWatchDog=false ADC3.ExternalTrigConv=ADC_EXTERNALTRIGCONV_T8_TRGO ADC3.ExternalTrigConvEdge=ADC_EXTERNALTRIGCONVEDGE_RISING -ADC3.ExternalTrigInjecConv=ADC_EXTERNALTRIGINJECCONV_T1_CC4 +ADC3.ExternalTrigInjecConv=ADC_EXTERNALTRIGINJECCONV_T1_TRGO ADC3.ExternalTrigInjecConvEdge=ADC_EXTERNALTRIGINJECCONVEDGE_RISING ADC3.IPParameters=Rank-7\#ChannelRegularConversion,Channel-7\#ChannelRegularConversion,SamplingTime-7\#ChannelRegularConversion,NbrOfConversionFlag,ClockPrescaler,Resolution,DataAlign,ScanConvMode,ContinuousConvMode,DiscontinuousConvMode,DMAContinuousRequests,EOCSelection,NbrOfConversion,ExternalTrigConvEdge,InjNumberOfConversion,EnableAnalogWatchDog,Rank-8\#ChannelInjectedConversion,Channel-8\#ChannelInjectedConversion,SamplingTime-8\#ChannelInjectedConversion,InjectedOffset-8\#ChannelInjectedConversion,ExternalTrigInjecConvEdge,InjectedConvMode,ExternalTrigInjecConv,ExternalTrigConv ADC3.InjNumberOfConversion=1 diff --git a/README.md b/README.md index 5bcdc98e..e37e9292 100644 --- a/README.md +++ b/README.md @@ -54,6 +54,10 @@ An upcoming feature will enable automatic tuning. Until then, here is a rough tu * Back down `pos_gain` until you do not have overshoot anymore. * The integrator is not easily tuned, nor is it strictly required. Tune at your own discression. +### Optional parameters +By default both motors are enabled, and the default control mode is position control. +If you want a different mode, you can change `.control_mode`. To disable a motor, set `.enable_control` and `.do_calibration` to false. + ## Compiling and downloading firmware ### Getting a programmer diff --git a/Src/adc.c b/Src/adc.c index 9f10db2a..96549988 100644 --- a/Src/adc.c +++ b/Src/adc.c @@ -102,7 +102,7 @@ void MX_ADC1_Init(void) sConfigInjected.InjectedNbrOfConversion = 1; sConfigInjected.InjectedSamplingTime = ADC_SAMPLETIME_3CYCLES; sConfigInjected.ExternalTrigInjecConvEdge = ADC_EXTERNALTRIGINJECCONVEDGE_RISING; - sConfigInjected.ExternalTrigInjecConv = ADC_EXTERNALTRIGINJECCONV_T1_CC4; + sConfigInjected.ExternalTrigInjecConv = ADC_EXTERNALTRIGINJECCONV_T1_TRGO; sConfigInjected.AutoInjectedConv = DISABLE; sConfigInjected.InjectedDiscontinuousConvMode = DISABLE; sConfigInjected.InjectedOffset = 0; @@ -154,7 +154,7 @@ void MX_ADC2_Init(void) sConfigInjected.InjectedNbrOfConversion = 1; sConfigInjected.InjectedSamplingTime = ADC_SAMPLETIME_3CYCLES; sConfigInjected.ExternalTrigInjecConvEdge = ADC_EXTERNALTRIGINJECCONVEDGE_RISING; - sConfigInjected.ExternalTrigInjecConv = ADC_EXTERNALTRIGINJECCONV_T1_CC4; + sConfigInjected.ExternalTrigInjecConv = ADC_EXTERNALTRIGINJECCONV_T1_TRGO; sConfigInjected.AutoInjectedConv = DISABLE; sConfigInjected.InjectedDiscontinuousConvMode = DISABLE; sConfigInjected.InjectedOffset = 0; @@ -206,7 +206,7 @@ void MX_ADC3_Init(void) sConfigInjected.InjectedNbrOfConversion = 1; sConfigInjected.InjectedSamplingTime = ADC_SAMPLETIME_3CYCLES; sConfigInjected.ExternalTrigInjecConvEdge = ADC_EXTERNALTRIGINJECCONVEDGE_RISING; - sConfigInjected.ExternalTrigInjecConv = ADC_EXTERNALTRIGINJECCONV_T1_CC4; + sConfigInjected.ExternalTrigInjecConv = ADC_EXTERNALTRIGINJECCONV_T1_TRGO; sConfigInjected.AutoInjectedConv = DISABLE; sConfigInjected.InjectedDiscontinuousConvMode = DISABLE; sConfigInjected.InjectedOffset = 0; diff --git a/Src/stm32f4xx_it.c b/Src/stm32f4xx_it.c index 1bb34b77..7cec13a9 100644 --- a/Src/stm32f4xx_it.c +++ b/Src/stm32f4xx_it.c @@ -40,7 +40,7 @@ #include "freertos_vars.h" #include "low_level.h" -typedef void (*ADC_handler_t)(ADC_HandleTypeDef* hadc); +typedef void (*ADC_handler_t)(ADC_HandleTypeDef* hadc, bool injected); void ADC_IRQ_Dispatch(ADC_HandleTypeDef* hadc, ADC_handler_t callback); /* USER CODE END 0 */ @@ -250,14 +250,14 @@ void ADC_IRQ_Dispatch(ADC_HandleTypeDef* hadc, ADC_handler_t callback) { uint32_t JEOC = __HAL_ADC_GET_FLAG(hadc, ADC_FLAG_JEOC); uint32_t JEOC_IT_EN = __HAL_ADC_GET_IT_SOURCE(hadc, ADC_IT_JEOC); if (JEOC && JEOC_IT_EN) { - callback(hadc); + callback(hadc, true); __HAL_ADC_CLEAR_FLAG(hadc, (ADC_FLAG_JSTRT | ADC_FLAG_JEOC)); } // Regular measurements uint32_t EOC = __HAL_ADC_GET_FLAG(hadc, ADC_FLAG_EOC); uint32_t EOC_IT_EN = __HAL_ADC_GET_IT_SOURCE(hadc, ADC_IT_EOC); if (EOC && EOC_IT_EN) { - callback(hadc); + callback(hadc, false); __HAL_ADC_CLEAR_FLAG(hadc, (ADC_FLAG_STRT | ADC_FLAG_EOC)); } } diff --git a/motor_timing.jpg b/motor_timing.jpg new file mode 100644 index 00000000..16f3f3bb Binary files /dev/null and b/motor_timing.jpg differ