File content as of revision 24:1de0291bc5eb:

#include "mbed.h"
#include "Steering.h"

#define myAbs(x)    ((x>0)?(x):(-(x)))

#define M_PI    3.1415f
#define ratioLPF    0.0609f //cut off:10Hz(at 1ms sampling)

extern SPI spi;
extern DigitalOut ioExpCs;
extern DigitalOut LED[];

Ticker ticker;

int gSteerAngle = 0;

void initIoExp(void)
{
    //SPIバスに複数のデバイスをぶら下げる場合，modeをそろえる必要がある
    spi.format(8, 0);
    spi.frequency(1000000);
    ioExpCs = 1;
    wait(0.1);    //Omajinai

    ioExpCs = 0;
    spi.write(IOEXP_WRITEADD);
    spi.write(IOEXP_IODIR);    //IO方向レジスタ
    spi.write(0xFF);    //All input
    ioExpCs = 1;
    wait_us(10);    //Omajinai

    ioExpCs = 0;
    spi.write(IOEXP_WRITEADD);
    spi.write(IOEXP_IPOL);    //入力極性
    spi.write(0xFF);    //All inverted
    ioExpCs = 1;
    wait_us(10);

    ioExpCs = 0;
    spi.write(IOEXP_WRITEADD);
    spi.write(IOEXP_GPPU);    //PULL-UP
    spi.write(0xFF);    //ALL PULL-UP
    ioExpCs = 1;
    wait_us(10);
}

int readIoExp(void)
{
    int gpio = 0;

    ioExpCs = 0;
    spi.write(IOEXP_READADD);
    spi.write(IOEXP_GPIO);
    gpio = spi.write(0x00);
    ioExpCs = 1;
    //wait_us(10);
    return gpio;
}

volatile bool steerIntFlag = false;

void loadSteerAngleISR(void)
{
    steerIntFlag = true;
}

void loadSteerAngle(void)
{
    static int preAngle=0;

    if(steerIntFlag == true) {
        steerIntFlag = false;

        int readData = GTB[readIoExp()];

        if(readData == 127) {
            LED[2] = LED[3] = 1;
        } else if(readData < 127) {
            LED[2] = 1;
            LED[3] = 0;
        } else {
            LED[2] = 0;
            LED[3] = 1;
        }

        //gSteerAngle = (int)(readData*ratioLPF + preAngle*(1.0f-ratioLPF));
        gSteerAngle = readData;

        preAngle = gSteerAngle;
    }
}

float getSteerAngle(void)
{
    //gSteerAngle:0~255
    //rad:0~2PI
    //return:-PI~PI
    float rad = 2.0f*M_PI*(gSteerAngle/255.0f);
    
    if(rad > 2*M_PI)
        rad = 2*M_PI;
    else if(rad < 0)
        rad = 0;

    return rad - M_PI;  //steer left->[+]
}

int getSteerDirection(void)
{
    return ((gSteerAngle > 127) ? 1 : 0); //steer left->1
}

void initSteering(void)
{
    initIoExp();
    ticker.attach(loadSteerAngleISR, 0.001);
}