diff --git a/applications/app_rccar.c b/applications/app_rccar.c index 36b99f1b..ce4793d0 100644 --- a/applications/app_rccar.c +++ b/applications/app_rccar.c @@ -44,11 +44,6 @@ static void trig_func(void *p); void app_rccar_init(void) { chThdCreateStatic(rccar_thread_wa, sizeof(rccar_thread_wa), NORMALPRIO, rccar_thread, NULL); - servodec_init(servodec_func); - - chSysLock(); - chVTSetI(&vt, MS2ST(10), trig_func, NULL); - chSysUnlock(); } static void trig_func(void *p) { @@ -73,6 +68,12 @@ static msg_t rccar_thread(void *arg) { chRegSetThreadName("APP_RCCAR"); rccar_tp = chThdSelf(); + servodec_init(servodec_func); + + chSysLock(); + chVTSetI(&vt, MS2ST(10), trig_func, NULL); + chSysUnlock(); + for(;;) { chEvtWaitAny((eventmask_t) 1); @@ -90,12 +91,7 @@ static msg_t rccar_thread(void *arg) { servo_val = 0.0; } - // Use duty cycle control when running slowly, otherwise use current control. - if (fabsf(mcpwm_get_rpm()) < 5000 && fabsf(servo_val) > MCPWM_MIN_DUTY_CYCLE) { - mcpwm_set_duty(servo_val); - } else { - mcpwm_set_current(servo_val * MCPWM_CURRENT_MAX); - } + mcpwm_set_current(servo_val * MCPWM_CURRENT_MAX); } else { mcpwm_set_current(0.0); } diff --git a/applications/app_sten.c b/applications/app_sten.c index 984e74b0..bcad1812 100644 --- a/applications/app_sten.c +++ b/applications/app_sten.c @@ -102,12 +102,7 @@ static UARTConfig uart_cfg = { void app_sten_init(void) { chThdCreateStatic(sten_thread_wa, sizeof(sten_thread_wa), NORMALPRIO, sten_thread, NULL); - chThdCreateStatic(log_thread_wa, sizeof(log_thread_wa), NORMALPRIO - 1, log_thread, NULL); - servodec_init(servodec_func); - - chSysLock(); - chVTSetI(&vt, MS2ST(10), trig_func, NULL); - chSysUnlock(); +// chThdCreateStatic(log_thread_wa, sizeof(log_thread_wa), NORMALPRIO - 1, log_thread, NULL); } static void trig_func(void *p) { @@ -158,6 +153,12 @@ static msg_t sten_thread(void *arg) { chRegSetThreadName("APP_STEN"); sten_tp = chThdSelf(); + servodec_init(servodec_func); + + chSysLock(); + chVTSetI(&vt, MS2ST(10), trig_func, NULL); + chSysUnlock(); + for(;;) { chEvtWaitAny((eventmask_t) 1); diff --git a/mcconf/mcconf_rccar1.h b/mcconf/mcconf_rccar1.h index cdbc46cc..99feb920 100644 --- a/mcconf/mcconf_rccar1.h +++ b/mcconf/mcconf_rccar1.h @@ -32,6 +32,7 @@ #define MCPWM_CURRENT_MIN -80.0 // Current limit in Amperes (Lower) #define MCPWM_IN_CURRENT_MAX 80.0 // Input current limit in Amperes (Upper) #define MCPWM_IN_CURRENT_MIN -25.0 // Input current limit in Amperes (Lower) +#define MCPWM_CURRENT_STARTUP_BOOST 0.08 // The lowest duty cycle to use in current control mode @ 20V. // Sensorless settings #define MCPWM_IS_SENSORLESS 1 // Use sensorless commutation diff --git a/mcpwm.c b/mcpwm.c index 78481df2..86a32c18 100644 --- a/mcpwm.c +++ b/mcpwm.c @@ -1419,8 +1419,9 @@ void mcpwm_adc_int_handler(void *p, uint32_t flags) { if (control_mode == CONTROL_MODE_CURRENT) { // Compute error - float error = current_set - (direction ? current : -current); + const float error = current_set - (direction ? current : -current); float step = error * MCPWM_CURRENT_CONTROL_GAIN * voltage_scale; + const float start_boost = MCPWM_CURRENT_STARTUP_BOOST / voltage_scale; // Do not ramp too much utils_truncate_number(&step, -MCPWM_RAMP_STEP_CURRENT_MAX, @@ -1433,12 +1434,13 @@ void mcpwm_adc_int_handler(void *p, uint32_t flags) { utils_truncate_number(&step, -ramp_step, ramp_step); } + // Optionally apply startup boost. if (!MCPWM_CURRENT_CONTROL_NO_REV || dutycycle_now_tmp > 0.0) { - if (fabsf(dutycycle_now_tmp) < MCPWM_CURRENT_STARTUP_BOOST) { + if (fabsf(dutycycle_now_tmp) < start_boost) { step_towards(&dutycycle_now_tmp, current_set > 0.0 ? - MCPWM_CURRENT_STARTUP_BOOST : - -MCPWM_CURRENT_STARTUP_BOOST, ramp_step); + start_boost : + -start_boost, ramp_step); } else { dutycycle_now_tmp += step; } @@ -1449,17 +1451,28 @@ void mcpwm_adc_int_handler(void *p, uint32_t flags) { // Lower truncation if (MCPWM_CURRENT_CONTROL_NO_REV) { + // This means that the motor shouldn't go in reverse, only brake. + if (dutycycle_now > -(MCPWM_MIN_DUTY_CYCLE + 0.01)) { - if (dutycycle_now_tmp < 0.0) { + // Going forwards... + + if (dutycycle_now_tmp < (MCPWM_MIN_DUTY_CYCLE + 0.01) && current_set < 0.0) { + // Apply full brake dutycycle_now_tmp = 0.0; dutycycle_set = 0.0; } else { if (dutycycle_now_tmp < MCPWM_MIN_DUTY_CYCLE && current_set > 0.0) { + // Too low duty cycle, use the minimum one. dutycycle_now_tmp = MCPWM_MIN_DUTY_CYCLE + 0.001; + dutycycle_set = dutycycle_now_tmp; } } } else { + // Going reverse, ramp down... step_towards(&dutycycle_now_tmp, 0.0, ramp_step); + + // If the set current is > 0, brake when stopping, otherwise change direction + dutycycle_set = current_set > 0.0 ? MCPWM_MIN_DUTY_CYCLE + 0.001 : 0.0; } } else { if (fabsf(dutycycle_now_tmp) < MCPWM_MIN_DUTY_CYCLE) { diff --git a/mcpwm.h b/mcpwm.h index 620b6c5a..9932390f 100644 --- a/mcpwm.h +++ b/mcpwm.h @@ -112,7 +112,7 @@ extern volatile int mcpwm_vzero; #define MCPWM_SWITCH_FREQUENCY_MAX 30000 // The highest switching frequency in Hz #define MCPWM_DEAD_TIME_CYCLES 80 // Dead time #define MCPWM_PWM_MODE PWM_MODE_SYNCHRONOUS // Default PWM mode -#define MCPWM_MIN_DUTY_CYCLE 0.02 // Minimum duty cycle +#define MCPWM_MIN_DUTY_CYCLE 0.01 // Minimum duty cycle #define MCPWM_MAX_DUTY_CYCLE 0.95 // Maximum duty cycle #define MCPWM_AVG_COM_RPM 6 // Number of commutations to average RPM over #define MCPWM_RAMP_STEP 0.02 // Ramping step (1000 times/sec) at maximum duty cycle @@ -145,7 +145,7 @@ extern volatile int mcpwm_vzero; #define MCPWM_CURRENT_CONTROL_NO_REV 0 // Do not reverse the direction in current control mode, brake only #endif #ifndef MCPWM_CURRENT_STARTUP_BOOST -#define MCPWM_CURRENT_STARTUP_BOOST 0.05 // The lowest duty cycle to use in current control mode (has to be > MCPWM_MIN_DUTY_CYCLE) +#define MCPWM_CURRENT_STARTUP_BOOST 0.05 // The lowest duty cycle to use in current control mode @ 20V. #endif #endif /* MC_PWM_H_ */