File content as of revision 7:09d682fdd50e:

#include "mbed.h"                                       //header files 
#include "math.h"
#include "Defines.h"
#include "PWM.h"
#include "ADC.h"
#include <string> 
//code to operate a 3 phase BLDC motor using velocity control. The system works by looking at the hall effect sensors and determining the 
//switch configuation that drives the motor to the next poisiton.
//

// things to do...
//1 implement a velocity ramp
//2 implement a hall offset algorithm 
//3  
//------------------------------------------
//    |    |        |    |          |    |
//   1x   3x       5x   7x         9x  11x
//    |-   |        |-   |          |-   |
//    |    |        |    |          |    | 
//   2x   4x       6x    8x       10x  12x 
//    |    |        |    |          |    |
//------------------------------------------
//
//


void  Initilisation (void);
void  DisableAll(void);
void  ADC_IRQHandler(void);
void  PWM1_IRQHandler(void);
void  HallRead(void);
int  CheckHalls(void);
void DisplayMenu(void);

DigitalOut      U      (LED1);
DigitalOut      V      (LED2);
DigitalOut      W      (LED3);
Serial          pc     (USBTX, USBRX);               //IO decleration

//PWMout        Q1     (p21);                           //all high side mosfets of the HHH are PWM - initlised in function
DigitalOut      Q2     (p10);                           //all low side mosfets of the HHH are digital 
//PWMout        Q3     (p22);
DigitalOut      Q4     (p9);     
//PWMout        Q5     (p23);
DigitalOut      Q6     (p8);                         
//PWMout        Q7     (p24);
DigitalOut      Q8     (p7);                         
//PWMout        Q9     (p25);
DigitalOut      Q10    (p6);
//PWMout        Q11    (p26); 
DigitalOut      Q12    (p5); 

DigitalIn     Ha     (p11);                             //buffered hall signals   
DigitalIn     Hb     (p12);
DigitalIn     Hc     (p13);

Timer t;                                                //uS timer used to RPM calculation

struct  InverterData    Info;                           //structure containing operating data for the inverter 
//int     Sequence            [8] = {5,1,3,2,6,4,5,1};   //State tracker sequnce - forward is clockwise rotation , backwards is anticlockwise 
int     Sequence            [8] = {3,1,5,4,6,2,3,1};    //State tracker sequnce reveresed  
 
int     ClockReset = 0;                                 //used to translate a hall transition event to the fast loop 
int     TimePerClick = 0;                               //used as buffer for timer value 
int     State = 1;                                       //varible containing the current State - used to change the MOSFET activness 
int     nextState;                                      //vaiible holding expected next State, used by State tracker 
int     StatePointer = 0;                               //holds index used to point at sequnce array, which holds the expected next States.  
int     direction = Clockwise;                      //flag used to determin the desired direciton
int     SwitchOverDelay = 0;                            //delay between switching the low side OffDuty and the switching the high side on - 
int     DisplayCounter = 0;                             //used to count number of loops, lets the display update at a slower rate than the main loop executes 
int     HallReset = 0;
char    SerialIn = 0;                                   //character inout from the serial. used for controlling the machine
float   OffDuty = 0;                                    //values used by the PWM to indicate duty. OffDuty is always 0,  
float   OnDuty = 50;                                    //OnDuty is changed by the velocity control loop.
float   RPM = 100;                                        //calculated RPM value 
float   Error = 1;                                          //Error value used in control loop 
float   SetPoint = 48;                                  //Velocity Setpoint 
int     PWMPRESCALE  = (12);
int     ControlLoopActive = 0;


int main()
{       
    int RevCounter = 0;
    float X = ((float)240 / (float) 60); //used in speed calc, but put here so math is only done once
    
    pc.baud(460800);
    NVIC_SetVector(PWM1_IRQn,   (uint32_t)&PWM1_IRQHandler);    // set the interrupt vector for PWM 
    
    Initilisation ();                                           //initilise the HW and interrupts 
    wait(0.2);
    
    StatePointer = CheckHalls();                                                         
    t.start(); 
    
    EnablePWM   (1);
    EnablePWM   (2);                                            //Q9 enabled swapped legs 56 with legs 34
    EnablePWM   (3);
    EnablePWM   (4);                                            //Q5 not used 
    EnablePWM   (5);
    EnablePWM   (6);                                            //Q1 enabled  
    ClockReset = 1;
    
    DisplayMenu();
 
    while(1)
    {
        if(State == 7)
        {
            State = 1;
        }    

        if(direction == Clockwise)
        {       
           /* if(StatePointer >5)                        //keep the Statepointer within indexs 0 and 5
            StatePointer = 0; 
      
            if(StatePointer < 0)                
            StatePointer = 5; 
        
            StatePointer++;   
            State = Sequence[StatePointer]; 
            wait(1);
            */
   
            HallRead();                                             //read halls first to initilise where the rotor is.
            
            switch(State)
            { 
                case 1: 
                Q2 = 0; 
                Q4 = 1;                                     //A sink, B supply, C float
                Q6 = 1;
                Q8 = 1;
                Q10 = 1;      
                Q12 = 1;
                wait_us(SwitchOverDelay);
                Info.Q1= OffDuty;
                Info.Q3= OffDuty;
                Info.Q5= OnDuty;
                Info.Q7= OffDuty;
                Info.Q9= OffDuty;
                Info.Q11= OffDuty;
                break;
               
                case 2:                                             //A float, B sink, C supply
                Q2 = 1; 
                Q4 = 1;                                     
                Q6 = 0;
                Q8 = 1;
                Q10 = 1;      
                Q12 = 1;
                wait_us(SwitchOverDelay);
                Info.Q1= OffDuty;
                Info.Q3= OffDuty;
                Info.Q5= OffDuty;
                Info.Q7= OffDuty;
                Info.Q9= OnDuty;
                Info.Q11= OffDuty;
                break;
                
                case 3:
                Q2 = 0; 
                Q4 = 1;                                     //A sink, B float, C supply
                Q6 = 1;
                Q8 = 1;
                Q10 = 1;      
                Q12 = 1;
                wait_us(SwitchOverDelay);
                Info.Q1= OffDuty;
                Info.Q3= OffDuty;
                Info.Q5= OffDuty;
                Info.Q7= OffDuty;
                Info.Q9= OnDuty;
                Info.Q11= OffDuty;
                break; 
        
                case 4:
                Q2 = 1; 
                Q4 = 1;                                     //A supply, B float, C sink
                Q6 = 1;
                Q8 = 1;
                Q10 = 0;      
                Q12 = 1;
                wait_us(SwitchOverDelay);
                Info.Q1= OnDuty;
                Info.Q3= OffDuty;
                Info.Q5= OffDuty;
                Info.Q7= OffDuty;
                Info.Q9= OffDuty;
                Info.Q11= OffDuty;
                break;
                        
                case 6:
                Q2 = 1; 
                Q4 = 1;                                     //A supply, B sink, C float
                Q6 = 0;
                Q8 = 1;
                Q10 = 1;      
                Q12 = 1;
                wait_us(SwitchOverDelay);
                Info.Q1= OnDuty;
                Info.Q3= OffDuty;
                Info.Q5= OffDuty;
                Info.Q7= OffDuty;
                Info.Q9= OffDuty;
                Info.Q11= OffDuty;
                break;
                
                case 5: 
                Q2 = 1; 
                Q4 = 1;                                     //A float, B supply, C sink
                Q6 = 1;
                Q8 = 1;
                Q10 = 0;      
                Q12 = 1;
                wait_us(SwitchOverDelay);
                Info.Q1= OffDuty;
                Info.Q3= OffDuty;
                Info.Q5= OnDuty;
                Info.Q7= OffDuty;
                Info.Q9= OffDuty;
                Info.Q11= OffDuty;
                break;
                
            }       
       
             SetPWM (Info.Q11,Info.Q9,Info.Q7,Info.Q5,Info.Q3,Info.Q1);              //set the PWM values
             
            if (OnDuty > 97)
            OnDuty = 97;
            
            if(OnDuty < 3)
            OnDuty = 3;   

            if (SetPoint > 402)
            SetPoint = 402;
            
            if(SetPoint < 4)
            SetPoint = 4;   
            
            if(ControlLoopActive == 1)
            {
                Error = (SetPoint - RPM);
                OnDuty = OnDuty + (0.000006 * Error);
            }
    
    
            if(HallReset == 1)
            {
                StatePointer = CheckHalls();                           //reset State to current hall State. this stops the motor getting confused about where it is. essentially we reset the State machines  
                HallReset = 0;    
            }
    


            if(ClockReset == 1)
            {
                TimePerClick =  t.read_us();                    //get us timing for best resolution
                t.reset();  
                RPM = (   ( (float)1000000 / (float)TimePerClick ) / X);  //1000000us / time per click = clicks per secons. 12 click = 1 rev 
                
                ClockReset = 0;    
                RevCounter ++;
                    
                if(RevCounter > 100)
                {
                    //ControlLoopActive = 1;
                }

            }                 
    
        
        
        
         
            
            if(pc.readable())                               //check if a keypress is observed 
            {
                SerialIn = pc.getc();                       //get the char, check if it is a/s. then alter the PWM value 
                switch(SerialIn)
                {
                    case 'a':
                        SetPoint = SetPoint + 6;
                        pc.printf("Setpoint = %f, \n\r",SetPoint) ;
                        break;
                    case 's':
                        SetPoint = SetPoint - 6;
                        pc.printf("Setpoint = %f, \n\r",SetPoint) ;
                        break;
                    case 'z': 
                        SwitchOverDelay = SwitchOverDelay + 1 ; 
                        pc.printf("%d\n\r",SwitchOverDelay);
                        break;
                    case 'x':
                        SwitchOverDelay = SwitchOverDelay - 1 ; 
                        pc.printf("%d\n\r",SwitchOverDelay);
                        if(SwitchOverDelay <2)
                        {
                            SwitchOverDelay = 2;
                            }
                        break;
                    case 'm': ControlLoopActive = 1; break;
                    case 'n': ControlLoopActive = 0; break;
                    case 'e':  StatePointer++;
                                if(StatePointer >5)                        //keep the Statepointer within indexs 0 and 5
                                StatePointer= 0; 
                                State = Sequence[StatePointer];
                                break;
             
             
                    case 'f': pc.printf("State = %d",State); State = State+1;  break;
                    case 'g': 
                        pc.printf("Setpoint = %f, Duty = %f, RPM = %f, Tclick = %d, Error = %f, PWM prescale = %d, switch over = %d, \n\r",SetPoint, OnDuty,RPM,TimePerClick,Error, PWMPRESCALE) ;
                        //pc.printf("%f \n\r", OnDuty) ;
                        break;
                    case 'h': 
                        PWMPRESCALE = PWMPRESCALE + 1;
                        pc.printf("%d\n\r",PWMPRESCALE);  
                        LPC_PWM1->PR =  PWMPRESCALE;    
                        break;
                    
                    case 'i': 
                        PWMPRESCALE = PWMPRESCALE - 1;
                        pc.printf("%d\n\r",PWMPRESCALE);   
                        if(PWMPRESCALE < 2)
                        {
                            PWMPRESCALE = 2;
                        }
                        LPC_PWM1->PR =  PWMPRESCALE;
                        break;
                    
                    case 'j':
                        OnDuty = OnDuty + 1;
                        if(OnDuty>97)
                        OnDuty = 97;
                        break;
                                     
                    case 'k':
                        OnDuty = OnDuty - 1;
                        if(OnDuty<3)
                        OnDuty = 3;
                        break;

                    case 'r': 
                        pc.printf("%f \n\r",RPM) ;
                        break;    
                    
                    case 'o':
                        DisplayMenu();
                    break;
    
                }    
                SerialIn = 0;
            }
    





        }//direction
    
    }//while(1)
}//main









int CheckHalls(void)                   //function to check the hall State and set the State tracker to point at the correct place. used at start up and when State tracker and motor are unsynchronised
{
    int localState = 0;
    int StatePointerBuff = 0;
    
    if(Ha){localState = localState + 2;}          //check the halls with bells of hollie, get the State. 
    if(Hb){localState = localState + 4;}
    if(Hc){localState = localState + 1;}   
    
        switch(localState)
        {
            /*case 1:StatePointerBuff = 1;break;  //read the halls to get current State - now point pointer at coorect place in sequence buffer
            case 2:StatePointerBuff = 3;break;
            case 3:StatePointerBuff = 2;break;
            case 4:StatePointerBuff = 5;break;
            case 5:StatePointerBuff = 6;break;
            case 6:StatePointerBuff = 4;break;*/
            case 1:StatePointerBuff = 1;break;  //read the halls to get current State - now point pointer at coorect place in sequence buffer
            case 2:StatePointerBuff = 5;break;
            case 3:StatePointerBuff = 6;break;
            case 4:StatePointerBuff = 3;break;
            case 5:StatePointerBuff = 2;break;
            case 6:StatePointerBuff = 4;break;
            
            
        }
        
    State = Sequence[StatePointerBuff];          //inilisie the State to the pointed to sequence State
    //pc.printf("checked halls, read State as %d, StatePointer reset to %d\n\r", State, StatePointerBuff);
    return StatePointerBuff;
}







void HallRead(void)                         //function reads State, filters spurous reads, checks valid reads against the tracker, and if correct it sets the next State. 
{
    int State_filter1 = 0;
    
    State = 0;                              //red the halls once
    if(Ha){State = State + 2;}
    if(Hb){State = State + 4;}
    if(Hc){State = State + 1;}
    
    State_filter1 = State;                   //save the value
    State = 0;                              //read halls again and compare with first read. Helps weed out accidental reads 
    if(Ha){State = State + 2;}
    if(Hb){State = State + 4;}
    if(Hc){State = State + 1;}
 
    
    if(State_filter1 == State )                   //debounce filter, two reads are identical, so unlikly spurious trigger 
    { 
       // State = State;                           //natural operation. it will move to next State as it reaches the current State. velocity it a function of voltage. 
        //pc.printf("State = %d \n\r ",State);

      if(direction == Clockwise)
      {
          //  pc.printf("1: State %d, sequence %d, StatePointer = %d\n\r",State,Sequence[StatePointer],StatePointer );  
  
            if(State == Sequence[StatePointer+1])           //when going clockwise the next expected State is pointed to by the sequence. this is correct operation 
            {   
                 if(StatePointer >5)                        //keep the Statepointer within indexs 0 and 5
                     StatePointer = 0; 
          
                 if(StatePointer < 0)                
                    StatePointer = 5; 
                
                    StatePointer++;    
                    //pc.printf("State %d, sequence %d, StatePointer = %d\n\r",State,Sequence[StatePointer],StatePointer );       
                    State = Sequence[StatePointer];         //make current State = to the next State 
                    ClockReset = 1; 
            }
            else if(State == Sequence[StatePointer])   
            {
             //halls havent changed, do nothing 

            }
            else
            {
                //a real edge has been detected, it isnt noise and it isnt the expected output. it looks like the rotor has gone backwards 
                HallReset = 1;      //reset the State tracker by reevealuating the hall position 
                ClockReset = 1;     //
         //   pc.printf("gone backwards\n\r");
            }
        
        
            if(State == 7)                                  //illegal State  - stop, drop and roll
            {
                    pc.printf("S7\n\r");    
            }
            
            if(State == 0)                                  //illegal State  - stop, drop and roll
            {
                    pc.printf("S0\n\r");   
            }
        }
    }
}




void PWM1_IRQHandler(void)
{ 
    LPC_PWM1->IR    |= (1<<0)|(1<<1)|(1<<2)|(1<<3)|(1<<8)|(1<<9)|(1<<10);              /* clear interrupt flag on match 0 */                                                     //Start new Conversion
}    




        
 





void DisableAll(void)
{
    Q2 = 1;
    Q4 = 1;
    Q6 = 1;
    Q8 = 1;
    Q10 = 1;
    
    DisablePWM(1);
    DisablePWM(2);
    DisablePWM(3);
    DisablePWM(4);
    DisablePWM(5);
    DisablePWM(6);
}


   
    
    

    
    
void Initilisation (void)
{

    Q2  = 1;                                //lowside MOSFETS. digital outs, set to 1 because active low
    Q4  = 1;
    Q6  = 1;
    Q8  = 1;
    Q10 = 1;
    Q12 = 1;
    
    Info.Q1        = 0;                     //high side MOSFETS. PWM, set to 0 because active high 
    Info.Q3        = 0;
    Info.Q5        = 0;
    Info.Q7        = 0;
    Info.Q9        = 0;
    Info.Q11       = 0;
    

    InitPWM(PWMPRESCALE);  
    ADC_Init();     
}
       
       
       
       

void DisplayMenu(void)
{        
pc.printf("---The Triple H SyxStep 5000---\n\r");
pc.printf("Press a to increase speed setpoint by 10\n\r");
pc.printf("Press s to decrease speed setpoint by 10\n\r");
pc.printf("Press z to increase switch over delay by 10\n\r");
pc.printf("Press x to decrease switch over delay by 10\n\r");
pc.printf("Press m to turn on control loop\n\r");
pc.printf("Press n to turn off control loop\n\r");   
pc.printf("Press g to print settings to screen \n\r");
pc.printf("Press h to increase PWM prescale\n\r");
pc.printf("Press i to decrease PWM prescale\n\r");
pc.printf("Press j to increase PWM Duty\n\r");
pc.printf("Press k to decrease PWM Duty\n\r");
pc.printf("Press r to display current RPM \n\r");
pc.printf("Press o to display this menu \n\r");
pc.printf("------\n\r");
}