Current control improvements

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);
-			}
 		} else {
 			mcpwm_set_current(0.0);
 		}

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);
 

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

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) {

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_ */