/*
Program, urceny k bezsenzorovemu rizeni BLDC motoru platformou IHM001 pomoci snimani BEMF
autor: Krystof Kuzma
CVUT FEL
*/

#include "mbed.h"

//piny pro zero-crossing detekci BEMF
AnalogIn BEMF1(PC_3);   //C7_37
AnalogIn BEMF2(PB_0);   //C7_34
AnalogIn BEMF3(PA_7);   //C10_26 PA_7

//piny pro povoleni kanalu driveru L6230
DigitalOut phaseAEN(PC_10); //CH1
DigitalOut phaseBEN(PC_11); //CH2
DigitalOut phaseCEN(PC_12); //CH3

//vystupy pro napajeni civek PWM modulaci
PwmOut phaseA(PA_8);    //UH_PWM
PwmOut phaseB(PA_9);    //VH_PWM
PwmOut phaseC(PA_10);   //WH_PWM

AnalogIn pot(PB_1);   //potenciometr
InterruptIn button(USER_BUTTON);    //preruseni od modreho tlacitka
DigitalOut led(LED1);   //led dioda pro signalizaci sepnuti tlacitka

//-----------------------------definice promennych--------------------------------
int state = 0;  //promenna pro vypocet stavu prepnuti
double pwmDuty = 0.6; //sirka PWM modulace
double potRead; //promenna pro cteni hodnoty potenciometru
int buttonState = 0;    //promenna pro ulozeni stavu tlacitka
double pwmSet;  //pomocna promenna pro nastaveni sirky PWM pulsu
char doOnce = 0;    //promenna pro jednorazove spusteni rozbehove rampy

//promenna pro cteni soucasneho BEMF signalu
float sense1;
float sense2;
float sense3;

//promenna pro zastaveni funkce while pro dane kroky
char stopA = 0;
char stopB = 0;
char stopC = 0;

//ukladani predchoziho BEMF signalu
float sense1Prev = 0;
float sense2Prev = 0;
float sense3Prev = 0;

//----------------------------rozbehova rampa motoru-------------------------------
void ramp(){
  if(doOnce == 0){
    for(short rampDelay = 7000; rampDelay >= 1000; rampDelay -= 200){
        phaseAEN = 0;
        phaseA.write(0);
        phaseB.write(pwmDuty);
        phaseBEN = 1;
        wait_us(rampDelay);  
        
        phaseCEN = 0;
        phaseAEN = 1;
        wait_us(rampDelay); 
        
        phaseBEN = 0;
        phaseB.write(0);
        phaseC.write(pwmDuty);
        phaseCEN = 1;            
        wait_us(rampDelay); 
        
        phaseAEN = 0;
        phaseBEN = 1;          
        wait_us(rampDelay);
        
        phaseCEN = 0;
        phaseC.write(0);
        phaseA.write(pwmDuty);
        phaseAEN = 1;       
        wait_us(rampDelay); 
        
        phaseBEN = 0;
        phaseCEN = 1;         
        wait_us(rampDelay);
        
        doOnce = 1;
       }//for end 
     }//if doOnce end
  }//ramp end

//-----------------------------------jednotlive kroky komutace------------------------------------------------    
    void step0(){   //B+, C-
        phaseAEN = 0;
        phaseBEN = 1;
        phaseCEN = 1;
        phaseB.write(pwmDuty);
        phaseC.write(0);
    }//step0 end
    void step1(){   //B+, A-
        phaseAEN = 1;
        phaseBEN = 1;
        phaseCEN = 0;
        phaseA.write(0);
        phaseB.write(pwmDuty);    
    }//step1 end
    void step2(){   //C+, A-
        phaseAEN = 1;
        phaseBEN = 0;
        phaseCEN = 1;
        phaseA.write(0);
        phaseC.write(pwmDuty);    
    }//step2 end
    void step3(){   //C+, B-
        phaseAEN = 0;
        phaseBEN = 1;
        phaseCEN = 1;
        phaseB.write(0);
        phaseC.write(pwmDuty);    
    }//step3 end
    void step4(){   //A+, B-
        phaseAEN = 1;
        phaseBEN = 1;
        phaseCEN = 0;
        phaseA.write(pwmDuty);
        phaseB.write(0);    
    }//step4 end
    void step5(){   //A+, C-
        phaseAEN = 1;
        phaseAEN = 0;
        phaseCEN = 1;
        phaseA.write(pwmDuty);
        phaseC.write(0);    
    }//step5 end

//----------------------------detekce indukovanzch napeti a prepinani do dalsiho kroku----------------------------------------    
void run(){ 
    
 if(state == 0){   //B+ C-, sense A down
    while (stopA == 0){
        step0();
        sense1 = BEMF1.read();
        if((sense1 <= 0.0f)&&(sense1Prev >= 0.0f)){
            stopA = 1;
            state = 1;
            }//if sense1 end
            sense1Prev = sense1;
        }//while stopA end
        stopA = 0;
    }//if count end  

 if(state == 1){   //B+ A-, sense C up
    while (stopC == 0){
        step1();
        sense3 = BEMF3.read();
        if((sense3 >= 0.0f)&&(sense3Prev <= 0.0f)){
            stopC = 1;
            state = 2;
            }//if sense1 end
            sense3Prev = sense3;        
        }//while stopC end
        stopC = 0;
    }//if count end 
    
 if(state == 2){   //C+ A-, sense B down
    while (stopB == 0){
        step2();
        sense2 = BEMF2.read();
        if((sense2 <= 0.0f)&&(sense2Prev >= 0.0f)){
            stopB = 1;
            state = 3;
            }//if sense1 end  
            sense2Prev = sense2;      
        }//while stopB end
        stopB = 0;
    }//if count end     
    
 if(state == 3){   //C+ B-, sense A up
    while (stopA == 0){
        step3();
        sense1 = BEMF1.read();
        if((sense1 >= 0.0f)&&(sense1Prev <= 0.0f)){
            stopA = 1;
            state = 4;
            }//if sense1 end  
            sense1Prev = sense1;      
        }//while stopA end
        stopA = 0;
    }//if count end      
    
 if(state == 4){   //A+ B-, sense C down
    while (stopC == 0){
        step4();
        sense3 = BEMF3.read();
        if((sense3 <= 0.0f)&&(sense3Prev >= 0.0f)){
            stopC = 1;
            state = 5;
            }//if sense1 end
            sense3Prev = sense3;        
        }//while stopC end
        stopC = 0;
    }//if count end  
     
 if(state == 5){   //A+ C-, sense B up
    while (stopB == 0){
        step5();
        sense2 = BEMF2.read();
        if((sense2 >= 0.0f)&&(sense2Prev <= 0.0f)){
            stopB = 1;
            state = 0;
            }//if sense1 end  
            sense2Prev = sense2;        
        }//while stopB end
        stopB = 0;
    }//if count end              
}//run end       
   
//------------------------------zapinani/vypinani pomoci preruseni od tlacitka-------------------------------------    
void pressed(){
        buttonState = !buttonState;
        led = !led; //signalizace stavu led diodou 
    }//pressed end   

//------------------------------------------zastaveni motoru----------------------------------------    
void stop(){
        phaseAEN = 0;
        phaseBEN = 0;
        phaseCEN = 0;
        doOnce = 0; //umoznuje opetovny rozbeh po stisknuti
    }//stop end 
    
//------------------------nastaveni rychlosti rotace sirkou PWM pulsu-----------------------------
void speed(){
    //nastaveni sirky duty cyklu od 0.4 do 1.0    
    pwmSet = pot.read();
    pwmSet *= 0.6;  
    pwmSet += 0.4;  
    pwmDuty = pwmSet;
    }//speed end                

int main() {
    
    //nastaveni periody PWM pro jednotliva vinuti
    phaseA.period_us(20);  
    phaseB.period_us(20);
    phaseC.period_us(20);
    
    //vychozi poloha rotoru A+ C- (krok 5)
    phaseAEN = 1;
    phaseBEN = 0;
    phaseCEN = 1;     
    phaseA.write(1);
    phaseC.write(0);      
        
    while(1) {
        
        button.fall(&pressed);  //preruseni od tlacitka
        
        if(buttonState){ //pokud je buttonState == 1
            
            ramp(); //asynchronni rozbehova rampa
            run();  //algoritmus pro chod motoru s vyuzitim BEMF
            speed();    //rizeni rychlosti pomoci duty cycle
            
        }//if buttonState end
        
        else{
            stop(); //zastaveni motoru
            }//else end
  
    }//while 1 end
}//main end