CatPot 2015-2016 / Mbed 2 deprecated CatPot_2v10_T_Main

ジャパンオープン用のメインプログラム

Dependencies:   mbed AQM1602 HMC6352 PID

main_processing/strategy_parts/input.cpp@25:a7460e23e02e, 2016-03-17 (annotated)

Committer:
lilac0112_1
Date:
Thu Mar 17 13:00:24 2016 +0000
Revision:
25:a7460e23e02e
Parent:
24:34ef6379b0df
Child:
27:825c6835e3db
BackHomeOnly

Who changed what in which revision?

UserRevisionLine numberNew contents of line
lilac0112_1 0:ea35c18c85fc 1 #include "mbed.h"
lilac0112_1 0:ea35c18c85fc 2 #include "extern.h"
lilac0112_1 0:ea35c18c85fc 3
lilac0112_1 0:ea35c18c85fc 4 //line
lilac0112_1 0:ea35c18c85fc 5 void ReadLine(void){
lilac0112_1 0:ea35c18c85fc 6 static uint8_t line_buf[3]={0,0,0};
lilac0112_1 0:ea35c18c85fc 7 wait_us(100);
lilac0112_1 0:ea35c18c85fc 8 if(Line[0].read()==1) line_buf[0] = 100;
lilac0112_1 2:635947de1583 9 if(line_buf[0]==0) data.lnFlag[0]=0;
lilac0112_1 2:635947de1583 10 else data.lnFlag[0]=1;
lilac0112_1 0:ea35c18c85fc 11 if(line_buf[0]>0) line_buf[0]--;
lilac0112_1 0:ea35c18c85fc 12
lilac0112_1 0:ea35c18c85fc 13 if(Line[1].read()==1) line_buf[1] = 100;
lilac0112_1 2:635947de1583 14 if(line_buf[1]==0) data.lnFlag[1]=0;
lilac0112_1 2:635947de1583 15 else data.lnFlag[1]=1;
lilac0112_1 0:ea35c18c85fc 16 if(line_buf[1]>0) line_buf[1]--;
lilac0112_1 0:ea35c18c85fc 17
lilac0112_1 0:ea35c18c85fc 18 if(Line[2].read()==1) line_buf[2] = 100;
lilac0112_1 2:635947de1583 19 if(line_buf[2]==0) data.lnFlag[2]=0;
lilac0112_1 2:635947de1583 20 else data.lnFlag[2]=1;
lilac0112_1 0:ea35c18c85fc 21 if(line_buf[2]>0) line_buf[2]--;
lilac0112_1 0:ea35c18c85fc 22 }
lilac0112_1 16:4fadb7a87497 23 //line_hold
lilac0112_1 16:4fadb7a87497 24 void LineRanking_A(void){
lilac0112_1 16:4fadb7a87497 25 if(data.lnOrder[0]==LINE_EMPTY){
lilac0112_1 16:4fadb7a87497 26 data.lnOrder[0]=A_SPOT;
lilac0112_1 16:4fadb7a87497 27 }
lilac0112_1 16:4fadb7a87497 28 else if(data.lnOrder[1]==LINE_EMPTY){
lilac0112_1 16:4fadb7a87497 29 data.lnOrder[1]=A_SPOT;
lilac0112_1 16:4fadb7a87497 30 }
lilac0112_1 16:4fadb7a87497 31 else if(data.lnOrder[2]==LINE_EMPTY){
lilac0112_1 16:4fadb7a87497 32 data.lnOrder[2]=A_SPOT;
lilac0112_1 16:4fadb7a87497 33 }
lilac0112_1 16:4fadb7a87497 34 }
lilac0112_1 16:4fadb7a87497 35 void LineRanking_B(void){
lilac0112_1 16:4fadb7a87497 36 if(data.lnOrder[0]==LINE_EMPTY){
lilac0112_1 16:4fadb7a87497 37 data.lnOrder[0]=B_SPOT;
lilac0112_1 16:4fadb7a87497 38 }
lilac0112_1 16:4fadb7a87497 39 else if(data.lnOrder[1]==LINE_EMPTY){
lilac0112_1 16:4fadb7a87497 40 data.lnOrder[1]=B_SPOT;
lilac0112_1 16:4fadb7a87497 41 }
lilac0112_1 16:4fadb7a87497 42 else if(data.lnOrder[2]==LINE_EMPTY){
lilac0112_1 16:4fadb7a87497 43 data.lnOrder[2]=B_SPOT;
lilac0112_1 16:4fadb7a87497 44 }
lilac0112_1 16:4fadb7a87497 45 }
lilac0112_1 16:4fadb7a87497 46 void LineRanking_C(void){
lilac0112_1 16:4fadb7a87497 47 if(data.lnOrder[0]==LINE_EMPTY){
lilac0112_1 16:4fadb7a87497 48 data.lnOrder[0]=C_SPOT;
lilac0112_1 16:4fadb7a87497 49 }
lilac0112_1 16:4fadb7a87497 50 else if(data.lnOrder[1]==LINE_EMPTY){
lilac0112_1 16:4fadb7a87497 51 data.lnOrder[1]=C_SPOT;
lilac0112_1 16:4fadb7a87497 52 }
lilac0112_1 16:4fadb7a87497 53 else if(data.lnOrder[2]==LINE_EMPTY){
lilac0112_1 16:4fadb7a87497 54 data.lnOrder[2]=C_SPOT;
lilac0112_1 16:4fadb7a87497 55 }
lilac0112_1 16:4fadb7a87497 56 }
lilac0112_1 16:4fadb7a87497 57 void LineRankClear(void){
lilac0112_1 16:4fadb7a87497 58 data.lnOrder[0]=LINE_EMPTY;
lilac0112_1 16:4fadb7a87497 59 data.lnOrder[1]=LINE_EMPTY;
lilac0112_1 16:4fadb7a87497 60 data.lnOrder[2]=LINE_EMPTY;
lilac0112_1 16:4fadb7a87497 61
lilac0112_1 22:b7720bba2ca6 62 if((0<LineHold)&&(LineHold<7)){
lilac0112_1 22:b7720bba2ca6 63 if((LineHolding[A_SPOT].read()==0)&&(LineHolding[B_SPOT].read()==0)&&(LineHolding[C_SPOT].read()==1)){
lilac0112_1 22:b7720bba2ca6 64 data.lnOrder[0] = C_SPOT;
lilac0112_1 22:b7720bba2ca6 65 data.lnOrder[1] = LINE_EMPTY;
lilac0112_1 16:4fadb7a87497 66 }
lilac0112_1 22:b7720bba2ca6 67 else if((LineHolding[A_SPOT].read()==0)&&(LineHolding[B_SPOT].read()==1)&&(LineHolding[C_SPOT].read()==0)){
lilac0112_1 16:4fadb7a87497 68 data.lnOrder[0] = B_SPOT;
lilac0112_1 22:b7720bba2ca6 69 data.lnOrder[1] = LINE_EMPTY;
lilac0112_1 16:4fadb7a87497 70 }
lilac0112_1 22:b7720bba2ca6 71 else if((LineHolding[A_SPOT].read()==0)&&(LineHolding[B_SPOT].read()==1)&&(LineHolding[C_SPOT].read()==1)){
lilac0112_1 16:4fadb7a87497 72 data.lnOrder[0] = C_SPOT;
lilac0112_1 22:b7720bba2ca6 73 data.lnOrder[1] = B_SPOT;
lilac0112_1 16:4fadb7a87497 74 }
lilac0112_1 22:b7720bba2ca6 75 else if((LineHolding[A_SPOT].read()==1)&&(LineHolding[B_SPOT].read()==0)&&(LineHolding[C_SPOT].read()==0)){
lilac0112_1 22:b7720bba2ca6 76 data.lnOrder[0] = A_SPOT;
lilac0112_1 22:b7720bba2ca6 77 data.lnOrder[1] = LINE_EMPTY;
lilac0112_1 22:b7720bba2ca6 78 }
lilac0112_1 22:b7720bba2ca6 79 else if((LineHolding[A_SPOT].read()==1)&&(LineHolding[B_SPOT].read()==0)&&(LineHolding[C_SPOT].read()==1)){
lilac0112_1 22:b7720bba2ca6 80 data.lnOrder[0] = C_SPOT;
lilac0112_1 22:b7720bba2ca6 81 data.lnOrder[1] = A_SPOT;
lilac0112_1 22:b7720bba2ca6 82 }
lilac0112_1 22:b7720bba2ca6 83 else if((LineHolding[A_SPOT].read()==1)&&(LineHolding[B_SPOT].read()==1)&&(LineHolding[C_SPOT].read()==0)){
lilac0112_1 21:378470320524 84 data.lnOrder[0] = A_SPOT;
lilac0112_1 21:378470320524 85 data.lnOrder[1] = B_SPOT;
lilac0112_1 21:378470320524 86 }
lilac0112_1 16:4fadb7a87497 87 }
lilac0112_1 16:4fadb7a87497 88 }
lilac0112_1 0:ea35c18c85fc 89 //ball
lilac0112_1 0:ea35c18c85fc 90 uint8_t ReadBall(void){//1or0
lilac0112_1 24:34ef6379b0df 91 return (BallCheckerA.read_u16()<40000);
lilac0112_1 0:ea35c18c85fc 92 }
lilac0112_1 25:a7460e23e02e 93 void JudgeBallHolding(void){
lilac0112_1 25:a7460e23e02e 94 //ホールド判定
lilac0112_1 25:a7460e23e02e 95 if(sys.DriBlind==0){
lilac0112_1 25:a7460e23e02e 96 if(
lilac0112_1 25:a7460e23e02e 97 ((data.ball==0)&&(sys.BallHoldFlag==0))||
lilac0112_1 25:a7460e23e02e 98 ((data.ball==1)&&(sys.BallHoldFlag==1))
lilac0112_1 25:a7460e23e02e 99 ){
lilac0112_1 25:a7460e23e02e 100 sys.BallHoldJudgeFlag=0;
lilac0112_1 25:a7460e23e02e 101 Ball_judge.detach();
lilac0112_1 25:a7460e23e02e 102 }
lilac0112_1 25:a7460e23e02e 103 if(
lilac0112_1 25:a7460e23e02e 104 ((data.ball==0)&&(sys.BallHoldFlag==1))||
lilac0112_1 25:a7460e23e02e 105 ((data.ball==1)&&(sys.BallHoldFlag==0))
lilac0112_1 25:a7460e23e02e 106 ){
lilac0112_1 25:a7460e23e02e 107 if(sys.BallHoldJudgeFlag==0){
lilac0112_1 25:a7460e23e02e 108 sys.BallHoldJudgeFlag=1;
lilac0112_1 25:a7460e23e02e 109 Ball_judge.attach(&JudgeBallHold, .5);
lilac0112_1 25:a7460e23e02e 110 }
lilac0112_1 25:a7460e23e02e 111 }
lilac0112_1 25:a7460e23e02e 112 /*
lilac0112_1 25:a7460e23e02e 113 if(
lilac0112_1 25:a7460e23e02e 114 !(
lilac0112_1 25:a7460e23e02e 115 (data.irNotice==IR_CLOSER)&&
lilac0112_1 25:a7460e23e02e 116 ((data.irPosition==10)||(data.irPosition==11)||(data.irPosition==12))
lilac0112_1 25:a7460e23e02e 117 )
lilac0112_1 25:a7460e23e02e 118 ){
lilac0112_1 25:a7460e23e02e 119 sys.BallHoldJudgeFlag=0;
lilac0112_1 25:a7460e23e02e 120 sys.BallHoldFlag=0;
lilac0112_1 25:a7460e23e02e 121 Ball_judge.detach();
lilac0112_1 25:a7460e23e02e 122 }
lilac0112_1 25:a7460e23e02e 123 */
lilac0112_1 25:a7460e23e02e 124 }
lilac0112_1 25:a7460e23e02e 125 else{
lilac0112_1 25:a7460e23e02e 126 sys.BallHoldFlag=0;
lilac0112_1 25:a7460e23e02e 127 }
lilac0112_1 25:a7460e23e02e 128 }
lilac0112_1 25:a7460e23e02e 129 //readsensor
lilac0112_1 0:ea35c18c85fc 130 void ReadCmps(void){
lilac0112_1 0:ea35c18c85fc 131 cmps_set.cmps = hmc.sample()/10.0;
lilac0112_1 0:ea35c18c85fc 132 }
lilac0112_1 0:ea35c18c85fc 133 void ReadPing(void){
lilac0112_1 0:ea35c18c85fc 134 uint16_t spi_data;
lilac0112_1 0:ea35c18c85fc 135 //Ultra Sonic Wave
lilac0112_1 0:ea35c18c85fc 136 spi_ss[1]=0;
lilac0112_1 0:ea35c18c85fc 137 wait_us(200);
lilac0112_1 0:ea35c18c85fc 138
lilac0112_1 0:ea35c18c85fc 139 spi_data = spi.write(0xABCD);
lilac0112_1 0:ea35c18c85fc 140
lilac0112_1 0:ea35c18c85fc 141 wait_us(200);
lilac0112_1 0:ea35c18c85fc 142 spi_ss[1]=1;
lilac0112_1 0:ea35c18c85fc 143
lilac0112_1 2:635947de1583 144 data.ping[0] = (spi_data&0x00FF)>>0;
lilac0112_1 2:635947de1583 145 data.ping[1] = (spi_data&0xFF00)>>8;
lilac0112_1 0:ea35c18c85fc 146 }
lilac0112_1 10:6df631c39f9b 147 void ReadPing2(void){
lilac0112_1 0:ea35c18c85fc 148 uint16_t spi_data;
lilac0112_1 0:ea35c18c85fc 149 //Ultra Sonic Wave
lilac0112_1 0:ea35c18c85fc 150 spi_ss[0]=0;
lilac0112_1 0:ea35c18c85fc 151 wait_us(200);
lilac0112_1 0:ea35c18c85fc 152
lilac0112_1 0:ea35c18c85fc 153 spi_data = spi.write(0xABCD);
lilac0112_1 0:ea35c18c85fc 154
lilac0112_1 0:ea35c18c85fc 155 wait_us(200);
lilac0112_1 0:ea35c18c85fc 156 spi_ss[0]=1;
lilac0112_1 0:ea35c18c85fc 157
lilac0112_1 2:635947de1583 158 data.ping[2] = (spi_data&0x00FF)>>0;
lilac0112_1 2:635947de1583 159 data.ping[3] = (spi_data&0xFF00)>>8;
lilac0112_1 0:ea35c18c85fc 160 }
lilac0112_1 0:ea35c18c85fc 161 void ReadIr(void){
lilac0112_1 0:ea35c18c85fc 162 uint16_t spi_data;
lilac0112_1 0:ea35c18c85fc 163
lilac0112_1 0:ea35c18c85fc 164 //Ir
lilac0112_1 0:ea35c18c85fc 165 spi_ss[3]=0;
lilac0112_1 0:ea35c18c85fc 166 wait_us(200);
lilac0112_1 0:ea35c18c85fc 167
lilac0112_1 0:ea35c18c85fc 168 spi_data = spi.write(0xABCD);
lilac0112_1 0:ea35c18c85fc 169
lilac0112_1 0:ea35c18c85fc 170 wait_us(200);
lilac0112_1 0:ea35c18c85fc 171 spi_ss[3]=1;
lilac0112_1 2:635947de1583 172 /*
lilac0112_1 1:3e013ae7900d 173 //key/phaseL/phaseS/long/short/
lilac0112_1 1:3e013ae7900d 174 // 2/ 3/ 3/ 4/ 4/
lilac0112_1 2:635947de1583 175 data.irKey = (spi_data&0xC000)>>14;//1100000000000000
lilac0112_1 2:635947de1583 176 data.irValPhase[IR_LONG] = (spi_data&0x3800)>>11;//0011100000000000
lilac0112_1 2:635947de1583 177 data.irValPhase[IR_SHORT] = (spi_data&0x0700)>>8;///0000011100000000
lilac0112_1 2:635947de1583 178 data.irSpot[IR_LONG] = (spi_data&0x00F0)>>4;///0000000011110000
lilac0112_1 2:635947de1583 179 data.irSpot[IR_SHORT] = (spi_data&0x000F)>>0;///0000000000001111
lilac0112_1 2:635947de1583 180 */
lilac0112_1 0:ea35c18c85fc 181
lilac0112_1 2:635947de1583 182 //key/phaseL/phaseS/diffL/position/
lilac0112_1 2:635947de1583 183 // 2/ 3/ 3/ 3/ 5/
lilac0112_1 2:635947de1583 184 data.irKey = (spi_data&0xC000)>>14;//1100000000000000
lilac0112_1 2:635947de1583 185 data.irValPhase[IR_LONG] = (spi_data&0x3800)>>11;//0011100000000000
lilac0112_1 2:635947de1583 186 data.irValPhase[IR_SHORT] = (spi_data&0x0700)>>8;///0000011100000000
lilac0112_1 2:635947de1583 187 data.irDif[IR_LONG] = (spi_data&0x00E0)>>5;///0000000011100000
lilac0112_1 2:635947de1583 188 data.irPosition = (spi_data&0x001F)>>0;///0000000000011111
lilac0112_1 1:3e013ae7900d 189
lilac0112_1 2:635947de1583 190 if(data.irKey!=0x2) data.irNotice=IR_NONE;
lilac0112_1 2:635947de1583 191
lilac0112_1 2:635947de1583 192
lilac0112_1 2:635947de1583 193 if(data.irValPhase[IR_SHORT]>=DIS_7){
lilac0112_1 10:6df631c39f9b 194 if(data.irValPhase[IR_LONG]>=DIS_5){
lilac0112_1 2:635947de1583 195 data.irNotice=IR_NONE;
lilac0112_1 1:3e013ae7900d 196 }
lilac0112_1 1:3e013ae7900d 197 else{
lilac0112_1 2:635947de1583 198 if(data.irDif[IR_LONG]<=DIS_6){
lilac0112_1 2:635947de1583 199 data.irNotice=IR_NONE;
lilac0112_1 1:3e013ae7900d 200 }
lilac0112_1 1:3e013ae7900d 201 else{
lilac0112_1 2:635947de1583 202 data.irNotice=IR_FAR;
lilac0112_1 1:3e013ae7900d 203 }
lilac0112_1 1:3e013ae7900d 204 }
lilac0112_1 1:3e013ae7900d 205 }
lilac0112_1 2:635947de1583 206 else{
lilac0112_1 2:635947de1583 207 if(data.irValPhase[IR_SHORT]>=DIS_3){
lilac0112_1 2:635947de1583 208 data.irNotice = IR_CLOSE;
lilac0112_1 2:635947de1583 209 }
lilac0112_1 2:635947de1583 210 else{
lilac0112_1 2:635947de1583 211 data.irNotice = IR_CLOSER;
lilac0112_1 2:635947de1583 212 }
lilac0112_1 2:635947de1583 213 }
lilac0112_1 1:3e013ae7900d 214
lilac0112_1 2:635947de1583 215 /*if(data.irValPhase[IR_LONG]>=DIS_7){
lilac0112_1 2:635947de1583 216 data.irNotice=IR_NONE;
lilac0112_1 2:635947de1583 217 }
lilac0112_1 2:635947de1583 218 else{
lilac0112_1 2:635947de1583 219 if(data.irValPhase[IR_SHORT]>=DIS_7){
lilac0112_1 2:635947de1583 220 data.irNotice=IR_FAR;
lilac0112_1 2:635947de1583 221 }
lilac0112_1 2:635947de1583 222 else{
lilac0112_1 2:635947de1583 223 if(data.irValPhase[IR_LONG]>=DIS_4){
lilac0112_1 2:635947de1583 224 data.irNotice = IR_CLOSE;
lilac0112_1 2:635947de1583 225 }
lilac0112_1 2:635947de1583 226 else{
lilac0112_1 2:635947de1583 227 data.irNotice = IR_CLOSER;
lilac0112_1 2:635947de1583 228 }
lilac0112_1 2:635947de1583 229 }
lilac0112_1 2:635947de1583 230 }*/
lilac0112_1 2:635947de1583 231 /*
lilac0112_1 2:635947de1583 232 if(data.irNotice == IR_NONE) data.irPosition=11;
lilac0112_1 2:635947de1583 233 if(data.irNotice == IR_FAR) data.irPosition=data.irSpot[IR_LONG]-1;
lilac0112_1 2:635947de1583 234 if(data.irNotice == IR_CLOSE) data.irPosition=data.irSpot[IR_SHORT]+7;
lilac0112_1 2:635947de1583 235 if(data.irNotice == IR_CLOSER) data.irPosition=data.irSpot[IR_SHORT]+7;
lilac0112_1 2:635947de1583 236 */
lilac0112_1 5:5ff3a7d5d8c2 237 //LED[1] = LED[0];
lilac0112_1 5:5ff3a7d5d8c2 238 //LED[0] = !LED[0];
lilac0112_1 0:ea35c18c85fc 239 }
lilac0112_1 19:967207de919d 240 //info
lilac0112_1 19:967207de919d 241 void ValidInfo(void){
lilac0112_1 21:378470320524 242 //LED=0xA;
lilac0112_1 19:967207de919d 243 if(sys.InfoFlag==0){
lilac0112_1 19:967207de919d 244 sys.InfoFlag=1;
lilac0112_1 19:967207de919d 245 }
lilac0112_1 19:967207de919d 246 }
lilac0112_1 0:ea35c18c85fc 247 void ReadInfo(void){
lilac0112_1 19:967207de919d 248 ReadIr();
lilac0112_1 19:967207de919d 249 PidUpdate();
lilac0112_1 19:967207de919d 250 ReadPing();
lilac0112_1 19:967207de919d 251 ReadPing2();
lilac0112_1 0:ea35c18c85fc 252 }