/*

* 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("_");

}