示例#1
0
void motor_rtctl_start(float duty_cycle, bool reverse)
{
    motor_rtctl_stop();                    // Just in case

    if (!_initialization_confirmed) {
        return; // Go home you're drunk
    }

    if (duty_cycle <= 0) {
        assert(0);
        return;
    }

    /*
     * Initialize the structs
     */
    memset(&_diag, 0, sizeof(_diag));
    memset(&_state, 0, sizeof(_state));    // Mighty reset

    motor_forced_rotation_detector_reset();

    _diag.started_at = motor_timer_hnsec();

    _state.comm_table = reverse ? COMMUTATION_TABLE_REVERSE : COMMUTATION_TABLE_FORWARD;

    _state.pwm_val_after_spinup = motor_pwm_compute_pwm_val(duty_cycle);

    init_adc_filters();

    /*
     * Initial spinup.
     */
    const tprio_t orig_priority = chThdSetPriority(HIGHPRIO);

    motor_pwm_prepare_to_start();

    const bool started = do_bemf_spinup(duty_cycle);

    /*
     * Engage the normal mode if started
     * At this point, if the spinup was OK, we're sutiated RIGHT AFTER THE DETECTED ZERO CROSS, engaged.
     */
    if (started) {
        _state.blank_time_deadline = motor_timer_hnsec() + _params.comm_blank_hnsec;
        _state.zc_detection_result = ZC_DETECTED;
        _state.flags = FLAG_ACTIVE | FLAG_SPINUP;
        motor_timer_set_relative(_state.comm_period / 3);
        lowsyslog("Motor: Spinup OK, comm period: %u usec\n", (unsigned)(_state.comm_period / HNSEC_PER_USEC));
    } else {
        lowsyslog("Motor: Spinup failed\n");
        motor_rtctl_stop();
    }

    chThdSetPriority(orig_priority);
}
示例#2
0
文件: motor.c 项目: JarryChou/sapog
int motor_init(void)
{
	_watchdog_id = watchdog_create(WATCHDOG_TIMEOUT_MSEC);
	if (_watchdog_id < 0) {
		return _watchdog_id;
	}

	int ret = motor_rtctl_init();
	if (ret) {
		return ret;
	}

	chMtxInit(&_mutex);
	chEvtInit(&_setpoint_update_event);

	configure();

	init_filters();
	if (_state.input_voltage < MIN_VALID_INPUT_VOLTAGE || _state.input_voltage > MAX_VALID_INPUT_VOLTAGE) {
		lowsyslog("Motor: Invalid input voltage: %f\n", _state.input_voltage);
		return -1;
	}

	ret = rpmctl_init();
	if (ret) {
		return ret;
	}

	motor_rtctl_stop();

	assert_always(chThdCreateStatic(_wa_control_thread, sizeof(_wa_control_thread),
	                                HIGHPRIO, control_thread, NULL));
	return 0;
}
示例#3
0
文件: motor.c 项目: branux/sapog
int motor_init(void)
{
	_watchdog_id = watchdogCreate(WATCHDOG_TIMEOUT_MSEC);
	if (_watchdog_id < 0) {
		return _watchdog_id;
	}

	int ret = motor_rtctl_init();
	if (ret) {
		return ret;
	}

	configure();

	init_filters();
	if (_state.input_voltage < MIN_VALID_INPUT_VOLTAGE || _state.input_voltage > MAX_VALID_INPUT_VOLTAGE) {
		printf("Motor: Invalid input voltage: %f\n", _state.input_voltage);
		return -1;
	}

	ret = rpmctl_init();
	if (ret) {
		return ret;
	}

	motor_rtctl_stop();

	if (!chThdCreateStatic(_wa_control_thread, sizeof(_wa_control_thread), HIGHPRIO, control_thread, NULL)) {
		abort();
	}
	return 0;
}
示例#4
0
文件: motor.c 项目: JarryChou/sapog
static void stop(bool expected)
{
	motor_rtctl_stop();
	_state.limit_mask = 0;
	_state.dc_actual = 0.0;
	_state.dc_openloop_setpoint = 0.0;
	_state.rpm_setpoint = 0;
	_state.setpoint_ttl_ms = 0;
	_state.rtctl_state = motor_rtctl_get_state();
	if (expected) {
		_state.num_unexpected_stops = 0;
	} else {
		_state.num_unexpected_stops++;
	}
	rpmctl_reset();
}
示例#5
0
int motor_rtctl_init(void)
{
    int ret = motor_pwm_init();
    if (ret) {
        return ret;
    }

    motor_timer_init();

    ret = motor_adc_init();
    if (ret) {
        return ret;
    }

    motor_forced_rotation_detector_init();

    configure();
    motor_rtctl_stop();
    return 0;
}