File content as of revision 38:67bc78f3c0ab:

#include "mbed.h"
#include "extern.h"

//line
void ReadLine(void){
    static uint8_t line_buf[3]={0,0,0};
    wait_us(100);
    if(Line[0].read()==1) line_buf[0] = 100;
    if(line_buf[0]==0)  data.lnFlag[0]=0;
    else                data.lnFlag[0]=1;
    if(line_buf[0]>0) line_buf[0]--;
    
    if(Line[1].read()==1) line_buf[1] = 100;
    if(line_buf[1]==0)  data.lnFlag[1]=0;
    else                data.lnFlag[1]=1;
    if(line_buf[1]>0) line_buf[1]--;
    
    if(Line[2].read()==1) line_buf[2] = 100;
    if(line_buf[2]==0)  data.lnFlag[2]=0;
    else                data.lnFlag[2]=1;
    if(line_buf[2]>0) line_buf[2]--;
}
//line_hold
void LineRanking_A(void){
    if(data.lnOrder[0]==LINE_EMPTY){
        data.lnOrder[0]=A_SPOT;
    }
    else if(data.lnOrder[1]==LINE_EMPTY){
        data.lnOrder[1]=A_SPOT;
    }
    else if(data.lnOrder[2]==LINE_EMPTY){
        data.lnOrder[2]=A_SPOT;
    }
}
void LineRanking_B(void){
    if(data.lnOrder[0]==LINE_EMPTY){
        data.lnOrder[0]=B_SPOT;
    }
    else if(data.lnOrder[1]==LINE_EMPTY){
        data.lnOrder[1]=B_SPOT;
    }
    else if(data.lnOrder[2]==LINE_EMPTY){
        data.lnOrder[2]=B_SPOT;
    }
}
void LineRanking_C(void){
    if(data.lnOrder[0]==LINE_EMPTY){
        data.lnOrder[0]=C_SPOT;
    }
    else if(data.lnOrder[1]==LINE_EMPTY){
        data.lnOrder[1]=C_SPOT;
    }
    else if(data.lnOrder[2]==LINE_EMPTY){
        data.lnOrder[2]=C_SPOT;
    }
}
void LineRankClear(void){
    data.lnOrder[0]=LINE_EMPTY;
    data.lnOrder[1]=LINE_EMPTY;
    data.lnOrder[2]=LINE_EMPTY;
    
    if((0<LineHold)&&(LineHold<7)){
        if((LineHolding[A_SPOT].read()==0)&&(LineHolding[B_SPOT].read()==0)&&(LineHolding[C_SPOT].read()==1)){
            data.lnOrder[0] = C_SPOT;
            data.lnOrder[1] = LINE_EMPTY;
        }
        else if((LineHolding[A_SPOT].read()==0)&&(LineHolding[B_SPOT].read()==1)&&(LineHolding[C_SPOT].read()==0)){
            data.lnOrder[0] = B_SPOT;
            data.lnOrder[1] = LINE_EMPTY;
        }
        else if((LineHolding[A_SPOT].read()==0)&&(LineHolding[B_SPOT].read()==1)&&(LineHolding[C_SPOT].read()==1)){
            data.lnOrder[0] = C_SPOT;
            data.lnOrder[1] = B_SPOT;
        }
        else if((LineHolding[A_SPOT].read()==1)&&(LineHolding[B_SPOT].read()==0)&&(LineHolding[C_SPOT].read()==0)){
            data.lnOrder[0] = A_SPOT;
            data.lnOrder[1] = LINE_EMPTY;
        }
        else if((LineHolding[A_SPOT].read()==1)&&(LineHolding[B_SPOT].read()==0)&&(LineHolding[C_SPOT].read()==1)){
            data.lnOrder[0] = C_SPOT;
            data.lnOrder[1] = A_SPOT;
        }
        else if((LineHolding[A_SPOT].read()==1)&&(LineHolding[B_SPOT].read()==1)&&(LineHolding[C_SPOT].read()==0)){
            data.lnOrder[0] = A_SPOT;
            data.lnOrder[1] = B_SPOT;
        }
    }
}
//line raw
/*
void LineRawCall_A(void){data.lnRawMemory[A_SPOT]=1;Line_timeout[A_SPOT].attach(&LineRawClear_A, LINE_DELAY);}
void LineRawCall_B(void){data.lnRawMemory[B_SPOT]=1;Line_timeout[B_SPOT].attach(&LineRawClear_B, LINE_DELAY);}
void LineRawCall_C(void){data.lnRawMemory[C_SPOT]=1;Line_timeout[C_SPOT].attach(&LineRawClear_C, LINE_DELAY);}
void LineRawClear_A(void){if(Line[A_SPOT].read()==1){LineRawCall_A();}else{data.lnRawMemory[A_SPOT]=0;}}
void LineRawClear_B(void){if(Line[B_SPOT].read()==1){LineRawCall_B();}else{data.lnRawMemory[B_SPOT]=0;}}
void LineRawClear_C(void){if(Line[C_SPOT].read()==1){LineRawCall_C();}else{data.lnRawMemory[C_SPOT]=0;}}
*/
void LineRawCall_ALL(void){
    if(data.lnRawReturn==1){
        if(
            (data.lnRawOrder[0]==A_SPOT)||
            (data.lnRawOrder[0]==B_SPOT)||
            (data.lnRawOrder[0]==C_SPOT)
        ){
            if(
                (
                    (data.lnRawOrder[0]!=LINE_EMPTY)&&
                    (data.lnRawOrder[1]!=LINE_EMPTY)&&
                    ((Line[data.lnRawOrder[0]].read()==1)||(data.lnRawOrder[0]==data.lnRawLastRise))
                )||
                (data.lnRawOrder[0]==C_SPOT)
            ){
                Line_reset.attach(&LineRawClear_ALL, LINE_DELAY_3);
            }
            else{
                Line_reset.attach(&LineRawClear_ALL, LINE_DELAY_2);
            }
        }
        else{
            Line_reset.attach(&LineRawClear_ALL, LINE_DELAY_2);
        }
    }
    else{
        Line_reset.attach(&LineRawClear_ALL, LINE_DELAY_1);
    }
}
void LineRawClear_ALL(void){
    if(LineRaw>0){
        LineRawCall_ALL();
    }
    else if(LineRaw==0){
        LineRawRankClear();
    }
}
void LineRawRankClear(void){
    
    //JSO2
    LineLiberate();
    
    data.lnRawOrderLog2[0]=data.lnRawOrderLog1[0];
    data.lnRawOrderLog2[1]=data.lnRawOrderLog1[1];
    data.lnRawOrderLog2[2]=data.lnRawOrderLog1[2];
    
    data.lnRawOrderLog1[0]=data.lnRawOrder[0];
    data.lnRawOrderLog1[1]=data.lnRawOrder[1];
    data.lnRawOrderLog1[2]=data.lnRawOrder[2];
    
    data.lnRawOrder[0]=LINE_EMPTY;
    data.lnRawOrder[1]=LINE_EMPTY;
    data.lnRawOrder[2]=LINE_EMPTY;
    
    if(
        (data.lnRawOrderLog2[0]==data.lnRawOrderLog1[0])&&
        //(data.lnRawOrderLog2[1]==data.lnRawOrderLog1[1])&&
        //(data.lnRawOrderLog2[2]==data.lnRawOrderLog1[2])
        (data.lnRawOrderLog2[0]!=LINE_EMPTY)
        //(data.lnRawOrderLog2[1]!=LINE_EMPTY)
    ){
        if(data.lnRepeat>100) data.lnRepeat=100;
        data.lnRepeat++;
        
        data.irLastNotice = data.irNotice;
        data.irLastPosition = data.irPosition;
    }
    else{
        data.lnRepeat=0;
    }
    
    
    data.lnRawRise[A_SPOT]=0;
    data.lnRawRise[B_SPOT]=0;
    data.lnRawRise[C_SPOT]=0;
}
void LineRawLogReset(void){
    /*
    if(
        (!(
            (data.irPosition==data.irLastPosition)||
            (data.irPosition==(data.irLastPosition+1))||
            (data.irPosition==(data.irLastPosition-1))||
            (data.irPosition==(data.irLastPosition+11))||
            (data.irPosition==(data.irLastPosition-11))
        ))&&
        (
            (data.irNotice==IR_CLOSE)||
            (data.irNotice==IR_CLOSER)
        )&&
        (
            (data.irLastPosition>7)
        )
    ){
        data.lnRawOrderLog2[0]=LINE_EMPTY;
        data.lnRawOrderLog2[1]=LINE_EMPTY;
        data.lnRawOrderLog2[2]=LINE_EMPTY;
    }
    else{
        data.lnRawOrderLog2[0]=LINE_EMPTY;
        data.lnRawOrderLog2[1]=LINE_EMPTY;
        data.lnRawOrderLog2[2]=LINE_EMPTY;
        
        data.lnRawOrderLog1[0]=LINE_EMPTY;
        data.lnRawOrderLog1[1]=LINE_EMPTY;
        data.lnRawOrderLog1[2]=LINE_EMPTY;
    }*/
    if(data.irNotice==IR_NONE){
        data.lnRawOrderLog2[0]=LINE_EMPTY;
        data.lnRawOrderLog2[1]=LINE_EMPTY;
        data.lnRawOrderLog2[2]=LINE_EMPTY;
        
        data.lnRawOrderLog1[0]=LINE_EMPTY;
        data.lnRawOrderLog1[1]=LINE_EMPTY;
        data.lnRawOrderLog1[2]=LINE_EMPTY;
    }
    else{
        data.lnRawOrderLog2[0]=LINE_EMPTY;
        data.lnRawOrderLog2[1]=LINE_EMPTY;
        data.lnRawOrderLog2[2]=LINE_EMPTY;
    }
    
}
void LineRawRanking_A(void){
    data.lnRawLastRise=A_SPOT;
    LineRawCall_ALL();
    if(data.lnRawRise[A_SPOT]==1) return;
    
    if((data.lnRawOrder[0]==LINE_EMPTY)||(data.lnRawOrder[0]==A_SPOT)){
        data.lnRawOrder[0]=A_SPOT;
        if((sys.DefenceFlag==1)&&(data.lnRawOrder[1]==LINE_EMPTY)){
            data.lnRawOrder[1]=C_SPOT;
        }
    }
    else if((data.lnRawOrder[1]==LINE_EMPTY)||(data.lnRawOrder[1]==A_SPOT)){
        data.lnRawOrder[1]=A_SPOT;
    }
    else if((data.lnRawOrder[2]==LINE_EMPTY)||(data.lnRawOrder[2]==A_SPOT)){
        data.lnRawOrder[2]=A_SPOT;
    }
    data.lnRawRise[A_SPOT]=1;
}
void LineRawRanking_B(void){
    data.lnRawLastRise=B_SPOT;
    LineRawCall_ALL();
    if(data.lnRawRise[B_SPOT]==1) return;
    
    if((data.lnRawOrder[0]==LINE_EMPTY)||(data.lnRawOrder[0]==B_SPOT)){
        data.lnRawOrder[0]=B_SPOT;
        if((sys.DefenceFlag==1)&&(data.lnRawOrder[1]==LINE_EMPTY)){
            data.lnRawOrder[1]=C_SPOT;
        }
    }
    else if((data.lnRawOrder[1]==LINE_EMPTY)||(data.lnRawOrder[1]==B_SPOT)){
        data.lnRawOrder[1]=B_SPOT;
    }
    else if((data.lnRawOrder[2]==LINE_EMPTY)||(data.lnRawOrder[2]==B_SPOT)){
        data.lnRawOrder[2]=B_SPOT;
    }
    data.lnRawRise[B_SPOT]=1;
}
void LineRawRanking_C(void){
    data.lnRawLastRise=C_SPOT;
    LineRawCall_ALL();
    if(data.lnRawRise[C_SPOT]==1) return;
    
    if((data.lnRawOrder[0]==LINE_EMPTY)||(data.lnRawOrder[0]==C_SPOT)){
        data.lnRawOrder[0]=C_SPOT;
    }
    else if((data.lnRawOrder[1]==LINE_EMPTY)||(data.lnRawOrder[1]==C_SPOT)){
        data.lnRawOrder[1]=C_SPOT;
    }
    else if((data.lnRawOrder[2]==LINE_EMPTY)||(data.lnRawOrder[2]==C_SPOT)){
        data.lnRawOrder[2]=C_SPOT;
    }
    data.lnRawRise[C_SPOT]=1;
}
//ball
uint8_t ReadBall(void){//1or0
    return (BallChecker.read()==0);
    //return (BallCheckerA.read_u16()<40000); 
}
void JudgeBallHolding(void){
    //ホールド判定
    if(sys.DriBlind==0){
        if(
            ((data.ball==0)&&(sys.BallHoldFlag==0))||
            ((data.ball==1)&&(sys.BallHoldFlag==1))
        ){
            sys.BallHoldJudgeFlag=0;
            Ball_judge.detach();
        }
        if(
            ((data.ball==0)&&(sys.BallHoldFlag==1))||
            ((data.ball==1)&&(sys.BallHoldFlag==0))
        ){
            if(sys.BallHoldJudgeFlag==0){
                sys.BallHoldJudgeFlag=1;
                Ball_judge.attach(&JudgeBallHold, .25);
            }
        }
        /*
        if(
            !(
                (data.irNotice==IR_CLOSER)&&
                ((data.irPosition==10)||(data.irPosition==11)||(data.irPosition==12))
            )
        ){
            sys.BallHoldJudgeFlag=0;
            sys.BallHoldFlag=0;
            Ball_judge.detach();
        }
        */
    }
    else{
        sys.BallHoldFlag=0;
    }
}
//readsensor
void ReadCmps(void){
    cmps_set.cmps = hmc.sample()/10.0;
}
void ReadPing(void){
    uint16_t spi_data;
    //Ultra Sonic Wave
    spi_ss[1]=0;
    wait_us(200);
    
    spi_data = spi.write(0xABCD);
    
    wait_us(200);
    spi_ss[1]=1;
    
    data.ping[0] = (spi_data&0x00FF)>>0;
    data.ping[1] = (spi_data&0xFF00)>>8;
}
void ReadPing2(void){
    uint16_t spi_data;
    //Ultra Sonic Wave
    spi_ss[0]=0;
    wait_us(200);
    
    spi_data = spi.write(0xABCD);
    
    wait_us(200);
    spi_ss[0]=1;
    
    data.ping[2] = (spi_data&0x00FF)>>0;
    data.ping[3] = (spi_data&0xFF00)>>8;
}
void ReadIr(void){
    uint16_t spi_data;
    
    //Ir
    spi_ss[3]=0;
    wait_us(200);
    
    spi_data = spi.write(0xABCD);
    
    wait_us(200);
    spi_ss[3]=1;
    /*
    //key/phaseL/phaseS/long/short/
    // 2/     3/     3/   4/    4/
    data.irKey                  = (spi_data&0xC000)>>14;//1100000000000000
    data.irValPhase[IR_LONG]    = (spi_data&0x3800)>>11;//0011100000000000
    data.irValPhase[IR_SHORT]   = (spi_data&0x0700)>>8;///0000011100000000
    data.irSpot[IR_LONG]        = (spi_data&0x00F0)>>4;///0000000011110000
    data.irSpot[IR_SHORT]       = (spi_data&0x000F)>>0;///0000000000001111
    */
    
    //key/phaseL/phaseS/diffL/position/
    // 2/     3/     3/    3/       5/
    data.irKey                  = (spi_data&0xC000)>>14;//1100000000000000
    data.irValPhase[IR_LONG]    = (spi_data&0x3800)>>11;//0011100000000000
    data.irValPhase[IR_SHORT]   = (spi_data&0x0700)>>8;///0000011100000000
    data.irDif[IR_LONG]         = (spi_data&0x00E0)>>5;///0000000011100000
    data.irPosition             = (spi_data&0x001F)>>0;///0000000000011111
    
    if(data.irKey!=0x2) data.irNotice=IR_NONE;
    
    
    if(data.irValPhase[IR_SHORT]>=DIS_7){
        if(data.irValPhase[IR_LONG]>=DIS_7){
            data.irNotice=IR_NONE;
        }
        else{
            if(data.irDif[IR_LONG]<=DIS_0){
                data.irNotice=IR_NONE;
            }
            else{
                data.irNotice=IR_FAR;
            }
        }
    }
    else{
        if(data.irValPhase[IR_SHORT]>=DIS_3){
            data.irNotice = IR_CLOSE;
        }
        else{
            data.irNotice = IR_CLOSER;
        }
    }
    
    /*if(data.irValPhase[IR_LONG]>=DIS_7){
        data.irNotice=IR_NONE;
    }
    else{
        if(data.irValPhase[IR_SHORT]>=DIS_7){
            data.irNotice=IR_FAR;
        }
        else{
            if(data.irValPhase[IR_LONG]>=DIS_4){
                data.irNotice = IR_CLOSE;
            }
            else{
                data.irNotice = IR_CLOSER;
            }
        }
    }*/
    /*
    if(data.irNotice == IR_NONE) data.irPosition=11;
    if(data.irNotice == IR_FAR) data.irPosition=data.irSpot[IR_LONG]-1;
    if(data.irNotice == IR_CLOSE) data.irPosition=data.irSpot[IR_SHORT]+7;
    if(data.irNotice == IR_CLOSER) data.irPosition=data.irSpot[IR_SHORT]+7;
    */
    //LED[1] = LED[0];
    //LED[0] = !LED[0];
}
//info
void ValidInfo(void){
    //LED=0xA;
    if(sys.InfoFlag==0){
        sys.InfoFlag=1;
    }
}
void ReadInfo(void){
    ReadIr();
    PidUpdate();
    ReadPing();
    ReadPing2();
}