File content as of revision 3:1194c29429bf:

// Reply to a SPI master as slave
float GAIN_P = 4.3f; // 比例ｹﾞｲﾝ
float GAIN_I = 1.3f; // 積分ｹﾞｲﾝ
float GAIN_D = 1.21f;
 
#include "mbed.h"
#include "Motor.h"
#include "QEI.h"
#include "PID.h"
#include "Defines.h"
#include "Registar.h"
BusOut LED(Red_Pin,Green_Pin,Blue_Pin);
class LEDMode
{
    Ticker Tic;
    private:
        char led,Off;
        void Inter()
        {
            static int flag;
            flag++;
            if(flag&2)
            {
                LED = led;
            }
            else
            {
                Tic.attach(this,&LEDMode::Inter,TransWait*0.3);
                LED = Off; 
            } 
        }
    public:
        char Red,Blue,Green;
        char TransWait;
        char InterVal;
        char Mode[7];// = {RED|BLUE,RED|GREEN,RED|GREEN,BLUE|GREEN,BLUE|GREEN};
        float Int[7];
        char OFF[7];
        LEDMode()
        {
            char mode[7] = {RED,RED|BLUE,RED|GREEN,RED|GREEN,BLUE|GREEN,BLUE|GREEN,RED|BLUE};
            char off[7] = {0,0,0,GREEN,0,GREEN,RED};
            float in[7] = {1,1,1.5,1.7,1.5,1.7,1.0};
            for(int i=0;i<7;i++)
            {
                Mode[i]=mode[i];
                Int[i]=in[i];
                OFF[i]=off[i];
            }
            Red=RED;
            Blue=BLUE;
            Green=GREEN;
            Off=0;
        }
        void Wait(int i)
        {
            float in = 1.0;
            TransWait = in;
            if(i==0)led = BLUE;
            else led = GREEN;
            Off=0;
            Tic.detach();
            Tic.attach(this,&LEDMode::Inter,TransWait);
        }
        void Act(int i)
        {
            if(i>=6)i=6; 
            {
                Off=OFF[i];
                led = Mode[i];
                TransWait = Int[i];
                Tic.detach();
                Tic.attach(this,&LEDMode::Inter,TransWait);
            }
        }
};
LEDMode Led;
PID pid(GAIN_P,GAIN_I,GAIN_D);
short PR;
short pulse ;
short Rev;
char Registar[0x80]={0};
char mode;
/*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();
//printf("%f    ",timer.read());
timer.reset();
//printf("%f    ",dRet);
dErr_prev = dErr;
 return (dRet);
}*/
Ticker rotateT;
//Ticker rotateN;
//QEI wheel(Rotal_A,Rotal_B,NC,100,QEI::X4_ENCODING);

    
Motor motor(Motor_H1,Motor_L2,Motor_L1,Motor_H2,Motor_PWM,short(0xffff));

void ManiacMotor_Mode(float duty,char mode)
{
    motor.run(mode,duty);
    //motor=1;
    //printf("%f %f\n " , duty,1.0/duty);
}
//PinName rotatepin[3];
int rotatemode;
void RotateSet()
{
    //delete wheel;
    //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)
    { 
        //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*/1)
    {
        if(!flag)
        {
            RotateSet();
            flag=1;
        }
        pulse = short((Registar[PulsePerRev]<<8)|Registar[PulsePerRev+1]/*wheel.getPulses()*/);
        Rev = short((Registar[PulsePerRev]<<8)|Registar[PulsePerRev]/*wheel.getRevolutions()*/);
        PR = short((Registar[PulsePerSec]<<8)|Registar[PulsePerSec+1]/*wheel.getRPMS()*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);
        char IF = Registar[MotorMode]&(~0x80);
        if(IF == 3 || IF == 5)
        {
            pid.GAIN_P = (float)Registar[MotorP]/10;
            pid.GAIN_I = (float)Registar[MotorI]/10;
            pid.GAIN_D = (float)Registar[MotorD]/10;
            //printf("PID P%f I%f D%f \n",pid.GAIN_P,pid.GAIN_I,pid.GAIN_D);
        }
        //printf("Rotate::: %d ,%d ,%d\n",pulse, Registar[PulsePerRev], Registar[PulsePerRev+1]);
    }
    /*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,bool mode)
{
    //float x = PIctrl(point , mypoint);
    float sa=0;
    if(mode==1)    sa = (int)point%360-(int)mypoint%360;
    else        sa = point-mypoint;
    //pid.dPoint = mypoint;
    //pid.point = pid.PIDval;//wheel1.getSumangle();
    pid.dPoint = mypoint;
    pid.dTarget = point;//wheel1.getSumangle();
    float x = (float)pid.data;
    //float x = pid.PIDval;
    motor = x/5000;
    //printf("%f %f %f \n",mypoint,point,x/5000);    
}

void SpeedStay(float point , float mypoint ,int mode)
{
    float sa;
    static float duty=0.1;
    int X[2] = {Front , Back};
    sa = (point-mypoint);
    float a = sa;
    if(sa<0)sa*=-1;
    if(a>0.3)
    {
        motor.run(X[mode],sa*duty/1000);
        duty+=1;
    }
    else if(a<-0.3)
    {
        motor.run(X[mode],sa*duty/1000);
        duty-=1;
    }
    else motor.run(X[mode],sa*duty/1000);
 
    //printf("%f %f %f \n",mypoint,point,sa*duty/1000);    
}
void SpeedStay_PID(float point , float mypoint,int mode)
{
    float x = 0;
    //x = PIctrl(point , mypoint);
    if(x<0)x*=-1;
    //if(point<0)point*=-1;
    int X[2] = {Front , Back};
    pid.dPoint = mypoint;
    pid.dTarget = point;//wheel1.getSumangle();
    x = (float)pid.data/1000;
    if(x<0.001&&x>-0.001)motor.run(Stop,x);
    else               motor.run(X[mode],x);
    //printf("%f %f %f %i\n",mypoint,point,x/1000,mode);    
}
char Inflag=0;
void Motor_mode()
{
    static char a=0;
    char IF = Registar[MotorMode]&(~0x80);
    switch(IF)
    {
        case 0:motor.run(Free,0);break;
        case 1:ManiacMotor_Mode(float((int(Registar[MotorPWM])<<8)|Registar[MotorPWM2])/0xffff,Registar[MotorState]);break;
        case 2:AngleStay(short(Registar[TargetAngle]<<8|Registar[TargetAngle2]),pulse*Registar[PulsePerAngle],Registar[MotorMode]>>7);break;
        case 3:AngleStay_PID(short(Registar[TargetAngle]<<8|Registar[TargetAngle2]),pulse*Registar[PulsePerAngle],Registar[MotorMode]>>7);break;
        case 4:SpeedStay(short(Registar[TargetSpeed]<<8|Registar[TargetSpeed2]),(float)PR*Registar[PulsePerAngle]/1000,int((Registar[TargetSpeed]>>7)==1));break;
        case 5:SpeedStay_PID(short(Registar[TargetSpeed]<<8|Registar[TargetSpeed2]),(float)PR*Registar[PulsePerAngle]/1000,int((Registar[TargetSpeed]>>7)==1));break;
        default:motor.run(Stop,0xffff);
    }
    static bool bo=false;
    if(!(IF==3||IF==5)&&bo==false)
    {
        //t.stop();
        //printf("STOP\n");
        pid.stop();
        pid.s_dErrIntg = 0;
        bo=true;
    }
    else if((IF==3||IF==5)&&bo==true)
    {
        bo=false;
        //printf("START\n");
        //t.stop();
        pid.Start();
    }
    //printf("%d",IF);
    if(IF!=a)
        Led.Act((int)IF);
    a=IF;
    //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 Tic(/*void const *argument*/)
{
     Motor_mode();
     Rotate();
     //printf("Tic\n");
}
//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 \n",reg);
            switch(i2c->receive())
            {
                case 0 :break;
                case I2CSlave::ReadAddressed:
                {
                    //char *po = Registar+reg;
                    for(int i=0;i<num;i++)
                    {
                        //char a=*po+i;
                        X[i] = Registar[reg+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];
                        i2c->read(X,num);
                        //Registar[reg]=D;
                        for (int i=0;i<num;i++)
                        {
                            Registar[reg]=X[i];
                         //   printf("%d ",Registar[reg]);
                         //printf(" Registar : %d  ,%d\n",Registar[reg],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);
            wait(.01);
    }
    /*NVIC_ClearPendingIRQ (TIMER_16_0_IRQn);
    NVIC_ClearPendingIRQ (TIMER_16_1_IRQn);
    NVIC_ClearPendingIRQ (TIMER_32_0_IRQn);
    NVIC_ClearPendingIRQ (TIMER_32_1_IRQn);*/
    //wheel.state(0);
    //encode();
    Inflag=1;
}
extern "C" void HardFault_Handler() {
    printf("Hard Fault!\n");
    while(1);
}
int main() {
    //pc.baud(230400);
    Mode = new DigitalIn(MODE);
    Registar[Who_am_I] = 0x67; 
    Registar[MotorP] = GAIN_P;
    Registar[MotorI] = GAIN_I;
    Registar[MotorD] = GAIN_D;
    //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.Wait(0);
    }
    else if(*Mode==0)
    {
        i2c = new I2CSlave(SDA,SCL);
        i2c->frequency(2000000);
        char address[4]={0x02,0x04,0x06,0x08};
        Address = new BusIn(I2C_addr_L,I2C_addr_H);
        i2c->address(0xa0/*address[Address->read()]*/);
        mode=I2C_MODE;
        Led.Wait(1);
        delete Address ;
    }
    NVIC_SetVector( I2C_IRQn , ( uint32_t ) execute_spi_slave_hw ) ;
    NVIC_SetPriority( I2C_IRQn , 1) ;
    NVIC_EnableIRQ( I2C_IRQn ) ;
    
    NVIC_SetPriority(TIMER_16_0_IRQn,3);
    NVIC_SetPriority(TIMER_16_1_IRQn,4);
    NVIC_SetPriority(TIMER_32_0_IRQn,5);
    NVIC_SetPriority(TIMER_32_1_IRQn,6);
    rotateT.attach(&Tic,0.005);
    while(1) {
        
    //motor.run(1,1);
    //    Motor_mode();   
    //    Rotate();
        /*while(10-ti.read_ms()>0)
        {}
            ti.reset();
            encode();*/
        
    }
}