File content as of revision 1:878564121574:

/*
 software decoder is for motor2
 "speed_count_2" can save the decoded data after one run the function "init_CH()"
*/

#ifndef _HALLSENSOR_SOFTWARE_DECODER_H
#define _HALLSENSOR_SOFTWARE_DECODER_H


#include "mbed.h"

InterruptIn HallA_1(A1);
InterruptIn HallB_1(A2);
InterruptIn HallA_2(D13);
InterruptIn HallB_2(D12);

void init_CN();
void CN_ITR();

typedef volatile struct WheelState {
    volatile int hallA;
    volatile int hallB;
    volatile unsigned short state;
    volatile unsigned short prestate;
    volatile int numStateChange;
    volatile bool direction;            // true: forward, false: backward
} WheelState_t;  

WheelState_t wheelState1, wheelState2;

void init_CN(){
    HallA_1.rise(&CN_ITR);
    HallA_1.fall(&CN_ITR);
    HallB_1.rise(&CN_ITR);
    HallB_1.fall(&CN_ITR);    
    HallA_2.rise(&CN_ITR);
    HallA_2.fall(&CN_ITR);
    HallB_2.rise(&CN_ITR);
    HallB_2.fall(&CN_ITR);

}

void CN_ITR() {
    /* MOTOR1 */
    wheelState1.hallA = HallA_1.read();
    wheelState1.hallB = HallB_1.read();

    /******************************** 
     **  State Estimation
     **     1. hallA = 0, 1
     **     2. hallB = 0, 1 
     ********************************/
    wheelState1.state = (wheelState1.hallA << 1) + (wheelState1.hallA ^ wheelState1.hallB) + 1;
    
    
    if(wheelState1.state == 1){
        if(wheelState1.prestate == 4){
            wheelState1.direction = true;
        } else if(state_1_old != 1){
            wheelState1.direction = false;
        }
    }
    else if(wheelState1.state == 4){
        if(wheelState1.prestate == 1){
            wheelState1.direction = false;
        } else if(wheelState1.prestate != 4) {
            wheelState1.direction = true;
        }
    }
    else {
        if(wheelState1.state > wheelState.prestate){
            wheelState1.direction = true;
        }else if(wheelState1.state < wheelState.prestate){
            wheelState1.direction = false;
        }   
    }        
    
    /* do nothing if state ain't change */
    if(wheelState1.state != wheelState.prestate){
        if(wheelState1.direction) { 
            ++wheelState1.numStateChange; 
        } else {
            --wheelState1.numStateChange; 
        }
    }    
    /* update previous state */
    wheelState1.prestate = wheelState1.state;    
    
    /* MOTOR2 */
    wheelState2.hallA = HallA_2.read();
    wheelState2.hallB = HallB_2.read();

    /* state determination */
    wheelState2.state = (wheelState2.hallA << 1) + (wheelState2.hallA ^ wheelState2.hallB) + 1;
    
    
    if(wheelState2.state == 1){
        if(wheelState2.prestate == 4){
            wheelState2.direction = true;
        } else if(wheelState2.prestate != 1){
            wheelState2.direction = false;
        }
    }
    else if(wheelState2.state == 4){
        if(wheelState2.prestate == 1){
            wheelState2.direction = false;
        } else if(wheelState2.prestate != 4){
            wheelState2.direction = true;
        }
    }
    else{
        if(wheelState2.state > wheelState2.prestate){
            wheelState2.direction = true;
        } else if(wheelState2.state < wheelState2.prestate){
            wheelState2.direction = false;
        }   
    } 
           
    /* do nothing if state ain't change */
    if(wheelState2.state != wheelState2.prestate){
        if(wheelState2.direction) { 
            ++wheelState2.numStateChange; 
        } else {
            --wheelState2.numStateChange; 
        }
    }    
    /* update previous state */
    wheelState2.prestate = wheelState2.state;
}


#endif