File content as of revision 0:5af71b978fb7:

// Reply to a SPI master as slave
float GAIN_P = 4.5f; // 比例ｹﾞｲﾝ
float GAIN_I = 3.7f; // 積分ｹﾞｲﾝ
float GAIN_D = 1.0f;
 
#include "mbed.h"
#include "Motor.h"
#include "QEI.h"
#include "PID.h"
#include "Defines.h"

char Registar[0x80]={123};
char mode;
Timer timer;
double PIctrl(double dCommand, double dVal)
 {
 static double s_dErrIntg = 0 ,dErr_prev=0;
 double dErr;
 double dRet;

 // 誤差
dErr = dCommand - dVal;

 // 誤差積分
s_dErrIntg += (dErr+dErr_prev )* timer.read() /2.0;

 // 制御入力
dRet = GAIN_P * dErr + GAIN_I * s_dErrIntg + GAIN_D*(dErr-dErr_prev)/timer.read();
timer.reset();
dErr_prev = dErr;
 return (dRet);
}
PinName rotatepin[3];
Ticker rotateT;
Ticker rotateN;
QEI wheel(Rotal_A,Rotal_B,NC,100,QEI::X4_ENCODING);

    
#include "Registar.h"
Motor motor(Motor_H1,Motor_L2,Motor_L1,Motor_H2,Motor_PWM,1);

void ManiacMotor_Mode(short duty,char mode)
{
    motor.run(mode,duty);
    //motor=1;
}
//PinName rotatepin[3];
int rotatemode;
void RotateSet()
{
    //delete wheel;
    short pulse = short(Registar[PulsePerRev]<<8|Registar[PulsePerRev2]);
    /*if(Registar[RotateMode]&0x01)rotatepin[0]=Rotal_A;
    //else rotatepin[0]=dp13;
    if(Registar[RotateMode]&0x02)rotatepin[1]=Rotal_B;
    //else rotatepin[0]=dp13;
    if(Registar[RotateMode]&0x04)rotatepin[2]=Rotal_Z;
    //else rotatepin[0]=dp13;*/
    if(Registar[RotateMode]&0x08)rotatemode=1;
    else if(!Registar[RotateMode]&0x08)rotatemode=0;
    if(Registar[RotateMode]&0x80)
    { 
        timer.start();
        //wheel.SetUp(rotatepin[0],rotatepin[1],rotatepin[2],pulse,rotatemode);
        //printf("make QEI\n");
    }
}
void Rotate(/*void const *argument*/)
{
    static bool flag=0;
    if(Registar[RotateMode]&0x80)
    {
        if(!flag)
        {
            RotateSet();
            flag=1;
        }
        short pulse = short(wheel.getPulses());
        short Rev = short(wheel.getRevolutions());
        short PR = short(wheel.getRPUS()*1000);
        Registar[MoterRevolutionH] = (Rev&0xff00)>>8;
        Registar[MoterRevolutionH-1] = (Rev&0xff);
        Registar[MoterPulseH] = (pulse&0xff00)>>8;
        Registar[MoterPulseH-1] = (pulse&0xff);
        Registar[MoterSpeedH] = (PR&0xff00)>>8;
        Registar[MoterSpeedH-1] = (PR&0xff);
        
        GAIN_P = (float)Registar[MotorP]/10;
        GAIN_I = (float)Registar[MotorI]/10;
        GAIN_D = (float)Registar[MotorD]/10;
        //printf("Rotate::: %d ,%d ,%d\n",pulse, Registar[MoterPulseH-1], Registar[MoterPulseH]);
    }
    /*if(flag==1)
    {
        static char Reg;
        static short pul;
        if(Registar[RotateMode]!=Reg||short(Registar[PulsePerRev]<<8|Registar[PulsePerRev2])!=pul)
        flag=0;
    }*/
}
void AngleStay(float point , float mypoint ,bool mode)
{
    float sa;
    if(mode==1)    sa = (int)point%360-(int)mypoint%360;
    else        sa = point-mypoint;
    if(sa>10)
        motor.run(Front,1);
    else if(sa<-10)
        motor.run(Back,1);
    else motor.run(Stop,1);
 
    printf("%f %f %f \n",mypoint,point,sa);    
}
void AngleStay_PID(float point , float mypoint)
{
    float x = PIctrl(point , mypoint);
    //pid.dPoint = mypoint;
    //pid.point = pid.PIDval;//wheel1.getSumangle();
    //float x = pid.PIDval;
    motor = x/5000;
}
void SpeedStay();
void SpeedStay_PID();

void Motor_mode()
{
    char IF = Registar[MotorMode]&(~0x80);
    switch(IF)
    {
        case 1:ManiacMotor_Mode(short(Registar[MotorPWM]<<8|Registar[MotorPWM2]),Registar[MotorState]);break;
        case 2:AngleStay(short(Registar[TargetAngle]<<8|Registar[TargetAngle2]),wheel.getSumangle(),Registar[MotorMode]>>7);break;
        case 3:AngleStay_PID(short(Registar[TargetAngle]<<8|Registar[TargetAngle2]),wheel.getSumangle());break;
        //case 4:SpeedStay();break;
        //case 5:SpeedStay_PID();break;
        default:motor.run(0,Stop);
    }
    //motor.run(Registar[MotorState],float(Registar[MotorPWM])/256.0);
    /*motor=float(Registar[MotorPWM]<<8|Registar[MotorPWM2]-32768)/32768.0;
    printf("%f\n",float(Registar[MotorPWM]<<8|Registar[MotorPWM2]-32768)/32768.0);*/
}

void encode(/*void const *argument*/)
{
        //wheel.encode();
     Motor_mode();
     Rotate();
}
//void timer(){
extern "C"  void execute_spi_slave_hw( void )
{
    //ledDbg = 1;
    wheel.state(1);
    if(i2c->receive()==I2CSlave::WriteAddressed&&mode==I2C_MODE)
        {
            LED=Red;
            //encoder.stop();
            //wheel->state(1);
            char DATA[2] = {}; 
            i2c->read(DATA,2);
            char reg=DATA[0];
            char num =DATA[1]; 
            char X[num];
            char f=0;
            //wait_us(1000);
            printf("R registar %d ",reg);
            switch(i2c->receive())
            {
                case 0 :break;
                case I2CSlave::ReadAddressed:
                {
                    char *po = Registar+reg;
                    for(int i=0;i<num;i++)
                        X[i]=*po+i;
                    i2c->write(X,num);
                    f=1;
                    do
                    {
                        f = i2c->write(Registar[reg]);
                    //    printf(" %d ",Registar[reg]);
                        reg++;
                    }while(f==1);
                    break;
                }
                case I2CSlave::WriteGeneral:{break;}
                case I2CSlave::WriteAddressed:
                {
                    char num = DATA[1];
                    for(int i=1; i<num; i++,reg++)
                        char X[num];
                        i2c->read(X,num);
                        //Registar[reg]=D;
                        for (int i=0;i<num;i++)
                        {
                            Registar[reg]=X[i];
                         //   printf("%d ",Registar[reg]);
                            reg++;
                        }
                        //printf(" Registar : %d  ,%d\n",Registar[reg],reg);
                    
                    break;
                }
            }
            //printf("OK\n");
            wheel.state(0);
        }
        if(spi->receive()&&mode==SPI_MODE) {
            LED=Blue|Red;
            //wheel.state(1);
            //rotateT.detach();
            //encoder.stop();
            char flag=1;
            char reg = spi->read();
            wait_us(50);
            char num = spi->read();
            //printf("SIZE %d\n",num);
            if(reg&0x80){
                reg=reg&(~0x80);
                flag=0;
                spi->reply(Registar[reg]);
            }
            else spi->reply(0x00);
            //wait_us(10);
            
            if(flag)
                for(int i=0;i<num;i++)
                {
                    while(!spi->receive());
                    wait_us(50);
                    Registar[reg+i] = spi->read();
                    //printf("%d,%d\n",reg+i,Registar[reg+i]);
                }
            else 
            {
                for(int i=0;i<num;i++)
                {
                    //
                    while(!spi->receive());
                    //wait_us(50);
                    spi->reply(Registar[reg++]);
                    char dummy = spi->read();
                    //printf("%d,%d\n",reg+i,Registar[reg+i]);
                }
                
            }
            //printf("%d  , %d\n",reg,Registar[reg]);
            flag=1;
            //spi->reply(00);
            
            //wheel.state(0);
            //encoder.start(10);
        
    }
    wheel.state(0);


}
int main() {
    //pc.baud(230400);
    Mode = new DigitalIn(MODE);
    Registar[0x60]=10;
    Registar[0x61]=32;
    //rotateN.attach(&enc,0.001);
    //Motor=0x08|0x01;
    if(*Mode==1)
    {
        spi = new SPISlave(MOSI, MISO, SCK,SSEL);
        spi->format(8,1);
        spi->frequency(4000000);
        spi->reply(0x00);              // Prime SPI with first reply
        mode=SPI_MODE;
        LED=Blue;
    }
    else if(*Mode==0)
    {
        i2c = new I2CSlave(SDA,SCL);
        i2c->frequency(200000);
        //char address[4]={0x20,0xb2,0xee,0xf4};
        Address = new BusIn(I2C_addr_L,I2C_addr_H);
        i2c->address(0xa0/*address[Address->read()]*/);
        mode=I2C_MODE;
        LED=Green;
        delete Address ;
    }
    NVIC_SetVector( I2C_IRQn , ( uint32_t ) execute_spi_slave_hw ) ;
    NVIC_SetPriority( I2C_IRQn , 20 ) ;
    NVIC_EnableIRQ( I2C_IRQn ) ;
    
    NVIC_SetPriority(TIMER_16_0_IRQn,9);
    NVIC_SetPriority(TIMER_16_1_IRQn,8);
    NVIC_SetPriority(TIMER_32_0_IRQn,7);
    NVIC_SetPriority(TIMER_32_1_IRQn,6);
    rotateT.attach(&encode,0.01);
    
    NVIC_SetPriority( EINT0_IRQn   , 5 ) ;
    NVIC_SetPriority( EINT1_IRQn   , 4 ) ;
    NVIC_SetPriority( EINT2_IRQn   , 3 ) ;
    NVIC_SetPriority( EINT3_IRQn   , 2 ) ;
    while(1) {
    
        /*Motor_mode();   
        Rotate();
        encode();*/
    }
}