Motor control on mbed

Dependencies:   mbed

main.cpp

Committer:
roger5641
Date:
2016-07-23
Revision:
1:f47edcd44466
Parent:
0:f76be6916639

File content as of revision 1:f47edcd44466:

#include "mbed.h"

//Motor sensor
InterruptIn HallA(D3);
InterruptIn HallB(D4);
InterruptIn HallC(D5);

void init_CN(void);
void CN_interrupt(void);

int8_t stateA=0, stateB=0, stateC=0;
int8_t state_1 = 0, state_1_old = 0;
int vCount;

int main() {
    init_CN();
    
    while(1)
    {
        ;  
    }     
}

void CN_interrupt(void)
{
    //Motor 1
    stateA = HallA.read();
    stateB = HallB.read();
    stateC = HallC.read();
    
    ///code for state determination///
    if (stateA == 1)
    {
        if (stateB == 1)
            if(stateC == 0)
            {
                state_1 = 2;               
            }
            else
            {
                ;
            }
        else
            if(stateC == 1)
            {
                state_1 = 6;              
            }
            else
            {
                state_1 = 1;
            }
    }
    else
    {
        if (stateB == 1)
            if(stateC == 1)
            {
               state_1 = 4;                
            }
            else
            {
                state_1 = 3;
            }
        else
            if(stateC == 1)
            {
                state_1 = 5;               
            }
            else
            {
                ;
            }
    }
    
    //Forward: vCount +1
    //Inverse: vCount -1
    if ( (state_1 == (state_1_old + 1)) || (state_1 == 1 && state_1_old == 6) )
        vCount++;
    else if ( (state_1 == (state_1_old - 1)) || (state_1 == 6 && state_1_old == 1))
        vCount--;
            
    state_1_old = state_1;
}

void init_CN(void)
{
    HallA.rise(&CN_interrupt);
    HallA.fall(&CN_interrupt);
    HallB.rise(&CN_interrupt);
    HallB.fall(&CN_interrupt);    
    HallC.rise(&CN_interrupt);
    HallC.fall(&CN_interrupt);
       
    stateA = HallA.read();
    stateB = HallB.read();
    stateC = HallC.read();
    
}