ジャパンオープン用のメインプログラム
Dependencies: mbed AQM1602 HMC6352 PID
main_processing/strategy_parts/input.cpp
- Committer:
- lilac0112_1
- Date:
- 2016-03-16
- Revision:
- 22:b7720bba2ca6
- Parent:
- 21:378470320524
- Child:
- 24:34ef6379b0df
File content as of revision 22:b7720bba2ca6:
#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; } } } //ball uint8_t ReadBall(void){//1or0 return (BallCheckerA.read_u16()<20000); } 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_5){ data.irNotice=IR_NONE; } else{ if(data.irDif[IR_LONG]<=DIS_6){ 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(); }