bldc.c

/* 
 * Open-BLDC Firmware, Firmware for BrushLess Drive Controllers
 * Copyright (C) 2009 Piotr Esden-Tempski <piotr at esden.net>
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/wdt.h>
#include <stdint.h>

#include "settings.h"
#include "timer.h"
#include "led.h"
#include "uart.h"

#include "bldc_utils.h"
#include "bldc.h"

volatile uint8_t bldc_phase = 0;
volatile uint16_t bldc_cnt_comm = 0;
uint8_t bldc_pwm = 0;
uint8_t bldc_pwm_max = PWM_MAX;
uint8_t bldc_running = 0;
uint8_t bldc_do_start = 0;
uint8_t bldc_old_phase = 0;

void bldc_set_pwm(uint8_t);
int8_t bldc_start();
void bldc_set_comm();

void bldc_init() {
    DDRD |= 0x38;
    PORTD = 0x00;
    DDRB |= 0x0E;
    PORTB = 0x31;

    PWM_OFF();

    /* enable analog comperator */
    SFIOR = 0x08;
    BEMF_SET_B();

    /* PWM generator timer */
    TCCR1B = (1 << CS10) | //
            (0 << CS11) | //
            (0 << CS12) | // clkI/O /1 (No prescaling)
            (0 << WGM12) | //
            (0 << WGM13) | //
            (0 << ICES1) | //
            (0 << ICNC1);
}

void bldc_set_pwm(uint8_t val) {
    unsigned char pwm_tmp;
    pwm_tmp = bldc_pwm = val;
    if (pwm_tmp > bldc_pwm_max) {
        pwm_tmp = bldc_pwm_max;
        LED_RED_ON();
    }

    PWM_SET(pwm_tmp);
}

/**
 * Generates a manual field rotation to give an initial impulse to the
 * motor.
 */
int8_t bldc_start() {
    unsigned long timer = START_MAX, i;
    unsigned char maintain = START_MAINTAIN;

    bldc_phase = 0;
    DISABLE_BEMF_INT();
    bldc_set_pwm(PWM_START);

    bldc_set_comm();
    while (1) {
        for (i = 0; i < timer; i++) {
            timer_wait(100);
        }
        timer -= START_DEC(timer);
        if (timer < START_MIN) {
            if (maintain) {
                timer = START_MIN;
                maintain--;
            } else {
                return 1;
            }
        }
        bldc_set_comm();
        bldc_phase++;
        bldc_phase %= 6;
        if (BEMF) {
            LED_GREEN_TOGGLE();
        }
    }

    return 0;
}

/**
 * Monitor the motor and control it's speed.
 */
void bldc_run() {

    //wdt_enable(WDTO_2S);

    uint16_t bldc_stop_detect_timer = timer_new_sw_ms(64);
    uint16_t speed_report = timer_new_sw_ms(50);
    while (1) {
        wdt_reset();

        static uint8_t uart_cmd = 0;
        uint8_t ch = 0;
        if (getch((char*) &ch) == UART_SUCCESS) {
            if (ch <= '9' && ch >= '1') {
                ch = ch - '0';
                ch = ((uint16_t) ch) * 255 / 9;
                bldc_set_pwm(uart_cmd = ch);

                print_str("cmd:");
                print_number(ch, 10);
                print('\n');
            } else if (ch == ',') {
                print_str("=");
                bldc_phase = 0;
                bldc_old_phase = bldc_phase;
                DISABLE_BEMF_INT();
                bldc_set_pwm(5);
                bldc_pwm = 0;
                bldc_running = 0;
                bldc_set_comm();
                bldc_cnt_comm = 0;
            } else if (ch == '.') {
                do {
                    bldc_phase++;
                    bldc_phase %= 6;
                    bldc_set_comm();
                    bldc_cnt_comm++;
                    if (BEMF) {
                        LED_GREEN_TOGGLE();
                    }
                    for (int ee = 0; ee < 20 * 10; ee++)
                        timer_wait(100);
                } while (bldc_phase != 0);
                bldc_running = 0;
                bldc_old_phase = bldc_phase;
                print_str("stop.\n");
                print_number(bldc_cnt_comm, 10);
            } else {
                // echo
                while (!(UCSRA & (1 << UDRE)))
                    ;
                UDR = ch;

                print_str("stop.\n");
                bldc_set_pwm(0);
                LED_RED_ON();
                DISABLE_BEMF_INT();
                bldc_running = 0;
                SET_ALL_OFF();
                uart_cmd = 0;
            }
        }

        if (bldc_old_phase != bldc_phase) {
            bldc_stop_detect_timer = timer_new_sw_ms(125);
            bldc_running = 1;
            bldc_old_phase = bldc_phase;
        }

        if (bldc_running && timer_sw_check(bldc_stop_detect_timer)) {
            print_str("stalled\n");
            LED_RED_ON();
            DISABLE_BEMF_INT();
            bldc_running = 0;
            SET_ALL_OFF();
        }

        if (!bldc_running) {
            LED_GREEN_OFF();
            bldc_do_start = (bldc_pwm != 0);
        }

        if (bldc_running && timer_sw_check(speed_report)) {
            const uint16_t dt = 20; //ms
            speed_report = timer_new_sw_ms(dt);
            static uint16_t previous_cnt = 0;
            uint32_t rotations = (bldc_cnt_comm - previous_cnt);
            uint32_t rps = rotations * (1000L / dt) / 66;

            bldc_set_pwm(uart_cmd);

            print_number(rps, 10);
            print('\n');
            previous_cnt = bldc_cnt_comm;
        }

        if (bldc_do_start) {
            LED_RED_OFF();
            bldc_do_start = 0;
            print_str("starting...");
            if (bldc_start()) {
                print_str("done.\n");
                LED_GREEN_ON();
                bldc_running = 1;
                BEMF_TOGGLE_INT();
                ENABLE_BEMF_INT();
                bldc_set_pwm(PWM_MIN);
                bldc_stop_detect_timer = timer_new_sw_ms(75);
                while (!timer_sw_check(bldc_stop_detect_timer)) {
                    asm("nop");
                }
                bldc_old_phase = 7;
            } else
                bldc_do_start = 1;
        }
    }
}

/**
 * Enforce commutation setting.
 */
void bldc_set_comm() {
    switch (bldc_phase) {
    case 0:
        SET_A_H();
        SET_B_L();
        BEMF_SET_C();
        BEMF_RISING_INT();
        break;
    case 1:
        SET_A_H();
        SET_C_L();
        BEMF_SET_B();
        BEMF_FALLING_INT();
        break;
    case 2:
        SET_B_H();
        SET_C_L();
        BEMF_SET_A();
        BEMF_RISING_INT();
        break;
    case 3:
        SET_B_H();
        SET_A_L();
        BEMF_SET_C();
        BEMF_FALLING_INT();
        break;
    case 4:
        SET_C_H();
        SET_A_L();
        BEMF_SET_B();
        BEMF_RISING_INT();
        break;
    case 5:
        SET_C_H();
        SET_B_L();
        BEMF_SET_A();
        BEMF_FALLING_INT();
        break;
    }
}

/*
 * Handle commutation.
 *
 * The analog comparator compares the positive pin AIN0 and negative
 * pin AIN1. When the voltage on the positive pin AIN0 is higher then
 * the voltage on the negative pin AIN1, the Analog Comparator Output,
 * ACO, is set. The following interrupt gets called on ACO raise, fall
 * or toggle depending on the call of BEMF_RISING_INT(),
 * BEMF_FALLING_INT() or BEMF_TOGGLE_INT() macros.
 */
ISR(ANA_COMP_vect)
{
    unsigned char i;
    uint8_t cnt = 0;
    /* debounce the bemf signal */
    for (i = 0; i < BEMF_DEBOUNCE_COUNT && cnt != 50; i++) {
        if (bldc_phase & 1) {
            if (BEMF_L) {
                i -= BEMF_DEBOUNCE_DEC;
                //print_str("o");
                cnt++;
            }
        } else {
            if (BEMF_H) {
                i -= BEMF_DEBOUNCE_DEC;
                //print_str("O");
                cnt++;
            }
        }
    }

    bldc_set_comm();

    bldc_phase++;
    bldc_phase %= 6;

    bldc_cnt_comm++;

    //print_str("_");
}