File content as of revision 5:b6d81c106fd1:

/*
 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) & 0x0001) + 1;
    wheelState1.state = (wheelState1.hallA << 1) 
                        + (((~wheelState1.hallA) && wheelState1.hallB) || (wheelState1.hallA && (~wheelState1.hallB))) 
                        + 1;
    
    
    if(wheelState1.state == 1) {
        if(wheelState1.prestate == 4) {
            wheelState1.direction = true;
        } else if(wheelState1.prestate != 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 > wheelState1.prestate) {
            wheelState1.direction = true;
        } else if(wheelState1.state < wheelState1.prestate) {
            wheelState1.direction = false;
        }   
    }        
    
    /* do nothing if state ain't change */
    if(wheelState1.state != wheelState1.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)&0x0001) + 1;
    wheelState2.state = (wheelState2.hallA << 1) 
                        + (((~wheelState2.hallA) && wheelState2.hallB) || (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      //  _HALLSENSOR_SOFTWARE_DECODER_H