You are here

Improved low speed and full stop control with Hall

5 posts / 0 new
Last post
pf26
Offline
Last seen: 2 months 2 weeks ago
VESC Free
Joined: 2018-12-04 08:44
Posts: 52
Improved low speed and full stop control with Hall

Just wanted to shared my app code to improve control at low speed and provide full stop  with hall sensors only  (encoder would allow to use position_pid instead).

I use "mcpwm_foc_set_openloop_phase" with a current proportional to the difference between the actual position (from hall sensors) and the last angle when motor was rotating.

otherwise "mc_interface_set_duty" with current compensation :  duty is increased when current increases, so as to keep speed more constant whatever the torque required. Compensation must be limitted to avoid unstable operation.

I use it in a little vehicle used on steep slopes with frequent halts for agricultural work - no more need for mechanical brakes. Relatively energy efficient - at least no so bad as "stepper motor like" control

Hope this helps.

 

                const volatile mc_configuration *mcconf = mc_interface_get_configuration();

                float my_duty = 0.6 * pwr * mcconf->l_max_duty;

                curr_now = mcpwm_foc_get_tot_current_filtered();   // VESC6 version need #include "mcpwm_foc.h"

                if (fabsf(my_duty) < 0.002) {

                    if (is_set_duty)    {

                        is_set_duty = 0;

                        my_angle = mcpwm_foc_get_phase_observer() - 90.0;  //  setting Iq in set_openloop_phase yields a 90degree shift vs phase_observer.

                    }

                    utils_norm_angle(&my_angle);

                    my_diff = utils_angle_difference(my_angle+90.0,get_phase_angle);

                    if (my_diff>55.0)   my_angle-=0.05;   // allows slight position shift when pushing really hard and reaching too high difference

                    if (my_diff>65.0)   my_angle-=0.1;

                    if (my_diff<-55.0)   my_angle+=0.05;

                    if (my_diff<-65.0)   my_angle+=0.1;

                    avg_diff += (fabsf(my_diff)-avg_diff)*0.002;                // smooth in time  (loop in done fast at 4kHz)

                    float avg_diff2 = 0.008 * avg_diff * avg_diff;       // max avg_diff = 60 degree -> max current

                    utils_truncate_number(&avg_diff2,0.0,40.0);    // 40.0 is max current - hard coded.. better take it from config

                    mcpwm_foc_set_openloop_phase(2.0 + avg_diff2, my_angle);    //float current, float angle)

                 }

                else    {

                    my_duty += (my_duty>0?1:-1) * curr_now * 0.001;     // current compensation: increase duty when more current required

                    utils_truncate_number(&my_duty,-1.0,1.0);     //  0.001 increase should be calculated from the motor resistance  (something like 70%)

                    mc_interface_set_duty(my_duty);      //

                    is_set_duty = 1;

                }

Aharrison
Offline
Last seen: 3 years 4 months ago
Joined: 2020-05-12 05:06
Posts: 1

pardon me sir I am very new to the electronics of flipsky 6.6. I have a motor that say it can not read the hall sensor and so the motor wll not spin, is there a feature in the vesc project platform where i can shut the hall sensor off and run a test?

Alan Harrison

pf26
Offline
Last seen: 2 months 2 weeks ago
VESC Free
Joined: 2018-12-04 08:44
Posts: 52

@Alan, using Vesc-tool, in the motor settings, you can select FOC mode / sensorless. Alternately, you may try to run an Hall Sensor detection to see how they perform (or not) I think FESC6.6 has a switch to select 3V or 5V supply to the hall sensors.

Poornesh
Offline
Last seen: 3 years 6 months ago
Joined: 2020-09-22 17:05
Posts: 1

Sir
May i know in which file and in which part of the code should i make changes? And in which part of the code should i pase it ?

Thankyou

Poornesh

pf26
Offline
Last seen: 2 months 2 weeks ago
VESC Free
Joined: 2018-12-04 08:44
Posts: 52

Please find the code of my custom app.  Quite direty coding, and obvisouly it has to be adapted to your application... But it may give you ideas..

/*
	app_adc modified
 */

#include "app.h"

#include "ch.h"
#include "hal.h"
#include "stm32f4xx_conf.h"
#include "mc_interface.h"
#include "timeout.h"
#include "utils.h"
#include "comm_can.h"
#include "hw.h"
#include <math.h>
#include "mcpwm_foc.h"
#include "commands.h"

// Settings
#define MAX_CAN_AGE						0.1
#define MIN_MS_WITHOUT_POWER			500
#define FILTER_SAMPLES					5
#define RPM_FILTER_SAMPLES				8

// Threads
static THD_FUNCTION(agri_thread, arg);
static THD_WORKING_AREA(agri_thread_wa, 1024);

// Private variables
static volatile adc_config config;
static volatile float ms_without_power = 0.0;
static volatile float decoded_level2 = 0.0;
static volatile float read_voltage2 = 0.0;
static float my_angle = 0.0;
static float avg_diff = 0.0;
static int k=0;
static volatile bool use_rx_tx_as_buttons = false;
static volatile bool stop_now = true;
static volatile bool is_running = false;
static bool mod_button = false;
static int is_set_duty = 0;

void app_agri_configure(adc_config *conf) {
	config = *conf;
	ms_without_power = 0.0;
}

void app_agri_start(bool use_rx_tx) {
	use_rx_tx_as_buttons = use_rx_tx;
	stop_now = false;
	chThdCreateStatic(agri_thread_wa, sizeof(agri_thread_wa), NORMALPRIO, agri_thread, NULL);
}

void app_agri_stop(void) {
	stop_now = true;
	while (is_running) {
		chThdSleepMilliseconds(1);
	}
}

static THD_FUNCTION(agri_thread, arg) {
	(void)arg;
	chRegSetThreadName("APP_AGRI");

	// Set servo pin as an input with pullup
	palSetPadMode(HW_UART_TX_PORT, HW_UART_TX_PIN, PAL_MODE_INPUT_PULLUP);  // Reverse
	palSetPadMode(HW_UART_RX_PORT, HW_UART_RX_PIN, PAL_MODE_INPUT_PULLUP);  // Mode
	is_running = true;

	for(;;) {
		// Sleep for a time according to the specified rate  CH_CFG_ST_FREQUENCY=10000
		systime_t sleep_time = CH_CFG_ST_FREQUENCY / 4000.0;        //config.update_rate_hz;
		// At least one tick should be slept to not block the other threads
		chThdSleep(sleep_time);
		if (stop_now) {
			is_running = false;
			return;
		}

		// For safe start when fault codes occur
		if (mc_interface_get_fault() != FAULT_CODE_NONE) {
			ms_without_power = 0;
		}

		// Read the external ADC pin and convert the value to a voltage.
		float pwr = (float)ADC_Value[ADC_IND_EXT];
		pwr /= 4095;
		pwr *= V_REG;
        // Linear mapping between the start and end voltage
		pwr = utils_map(pwr, config.voltage_start, config.voltage_end, 0.0, 1.0);
        if (pwr<0) pwr=0;

		// Optionally apply a mean value filter
		if (config.use_filter) {
			static float filter_buffer[FILTER_SAMPLES];
			static int filter_ptr = 0;

			filter_buffer[filter_ptr++] = pwr;
			if (filter_ptr >= FILTER_SAMPLES) {
				filter_ptr = 0;
			}
			pwr = 0.0;
			for (int i = 0;i < FILTER_SAMPLES;i++) {
				pwr += filter_buffer[i];
			}
			pwr /= FILTER_SAMPLES;
		}

		// Read the button pins
		bool rev_button = false;
		rev_button = !palReadPad(HW_UART_TX_PORT, HW_UART_TX_PIN);
		if (palReadPad(HW_UART_RX_PORT, HW_UART_RX_PIN) != mod_button)
        {
            mod_button = palReadPad(HW_UART_RX_PORT, HW_UART_RX_PIN);
			ms_without_power = 0;
        }
        // Invert the voltage if the button is pressed
        if (rev_button) {
            pwr = -pwr;
        }

		// Apply ramping
		static systime_t last_time = 0;
		static float pwr_ramp = 0.0;
		float ramp_time = fabsf(pwr) > fabsf(pwr_ramp) ? 2.5 : 1.5;

		if (ramp_time > 0.01) {
			const float ramp_step = 1 / ramp_time / 4000.0;       // 4000Hz 5s
			utils_step_towards(&pwr_ramp, pwr, ramp_step);
            pwr = pwr_ramp;
		}

		// Use the filtered and mapped voltage for control according to the configuration.
        if (fabsf(pwr_ramp) < 0.002) {
            pwr = 0.0;
            ms_without_power += (1000.0 * (float)sleep_time) / (float)CH_CFG_ST_FREQUENCY;
        }
        float get_phase_angle = -1;
        float my_diff;
        float curr_now = -1;

        if (!(ms_without_power < MIN_MS_WITHOUT_POWER && config.safe_start)) {
            get_phase_angle = mcpwm_foc_get_phase_observer();
            if (!mod_button) {  // ----------------- Button Rx = 0 ------- no longer used
                if (fabsf(pwr) > 0.001) {
                    my_angle+=pwr * 1.0;           // à 4kHz
                    utils_norm_angle(&my_angle);
                }
                my_diff = utils_angle_difference(my_angle+90.0,get_phase_angle);
                avg_diff += (fabsf(my_diff)-avg_diff) * 0.0003;        // smooth
                float avg_diff2 = 0.3 * avg_diff;       // max avg_diff = 60 degree -> max current
                utils_truncate_number(&avg_diff2,0.0,25.0);
                mcpwm_foc_set_openloop_phase(4.0 + avg_diff2, my_angle);    //float current, float angle)
                }
            else    {       //---------------------- Normal use ------------
                float my_duty = 0.75 * pwr;			// scaling dependings upon application
                curr_now = mcpwm_foc_get_tot_current_filtered();   			// VESC6 version
                if (fabsf(my_duty) < 0.002) {		
                    if (is_set_duty)    {
                        is_set_duty = 0;
                        my_angle = mcpwm_foc_get_phase_observer() - 90.0;  // remember angle to block when no longer rotating
                    }
                    utils_norm_angle(&my_angle);
                    my_diff = utils_angle_difference(my_angle+90.0,get_phase_angle);
                    if (my_diff>45.0)   my_angle-=0.05;
                    if (my_diff>55.0)   my_angle-=0.1;
                    if (my_diff<-45.0)   my_angle+=0.05;
                    if (my_diff<-55.0)   my_angle+=0.1;
                    avg_diff += (fabsf(my_diff)-avg_diff)*0.0003;                // smooth
                    float avg_diff2 = 0.008 * avg_diff * avg_diff;       // max avg_diff = 60 degree -> max current
                    utils_truncate_number(&avg_diff2,0.0,45.0);
                    mcpwm_foc_set_openloop_phase(2.0 + avg_diff2, my_angle);    //float current, float angle)
                 }
                else    {
                    my_duty += (my_duty>0?1:-1) * curr_now * 0.001;     // current compensation
                    utils_truncate_number(&my_duty,-1.0,1.0);
                    mc_interface_set_duty(my_duty);      //
                    is_set_duty = 1;
                }
            }
        }
        if (!(++k % 1000)) {
 		// Reset timeout
		timeout_reset();
//            commands_printf("diff %3.1f | %3.1f | %3.1f R %1.3f",(double)avg_diff,(double)my_angle,(double)get_phase_angle,(double)curr_now);
        }

		// If safe start is enabled and the output has not been zero for long enough
		if (ms_without_power < MIN_MS_WITHOUT_POWER && config.safe_start) {
			static int pulses_without_power_before = 0;
			if (ms_without_power == pulses_without_power_before) {
				ms_without_power = 0;
			}
			pulses_without_power_before = ms_without_power;
			mc_interface_set_brake_current(timeout_get_brake_current());
			continue;
		}
	}
}