CatPot 2015-2016 / Mbed 2 deprecated CatPot_2v10_T_Main

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed AQM1602 HMC6352 PID

main_processing/strategy/strategy.cpp@10:6df631c39f9b, 2016-03-10 (annotated)

Committer:
lilac0112_1
Date:
Thu Mar 10 03:03:50 2016 +0000
Revision:
10:6df631c39f9b
Parent:
5:5ff3a7d5d8c2
Child:
11:3efae754e6ef
pid+alpha

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 //Atk
lilac0112_1 0:ea35c18c85fc 5 void modeAttack0(void){
lilac0112_1 0:ea35c18c85fc 6 double ir_x, ir_y;
lilac0112_1 0:ea35c18c85fc 7 int vx,vy,vs;
lilac0112_1 0:ea35c18c85fc 8 uint8_t LineDir[4];
lilac0112_1 0:ea35c18c85fc 9 uint8_t LineStop[2];
lilac0112_1 0:ea35c18c85fc 10 //uint8_t IrRange[4];
lilac0112_1 0:ea35c18c85fc 11 //uint8_t LineBind[4];
lilac0112_1 0:ea35c18c85fc 12 if(sys.IrFlag==1){
lilac0112_1 0:ea35c18c85fc 13 ReadIr();
lilac0112_1 0:ea35c18c85fc 14 sys.IrFlag=0;
lilac0112_1 0:ea35c18c85fc 15 }
lilac0112_1 0:ea35c18c85fc 16 if(sys.PidFlag==1){
lilac0112_1 0:ea35c18c85fc 17 Line_ticker.detach();
lilac0112_1 0:ea35c18c85fc 18 PidUpdate();
lilac0112_1 0:ea35c18c85fc 19 Line_ticker.attach(&ReadLine, 0.005);
lilac0112_1 0:ea35c18c85fc 20 sys.PidFlag=0;
lilac0112_1 0:ea35c18c85fc 21 }
lilac0112_1 5:5ff3a7d5d8c2 22 ir_x = ir_move_val_old[data.irNotice][data.irPosition][IR_X];
lilac0112_1 5:5ff3a7d5d8c2 23 ir_y = ir_move_val_old[data.irNotice][data.irPosition][IR_Y];
lilac0112_1 2:635947de1583 24 if(data.irPosition<8){
lilac0112_1 0:ea35c18c85fc 25 ir_x *= (double)(sys.l_pow);
lilac0112_1 0:ea35c18c85fc 26 ir_y *= (double)(sys.l_pow);
lilac0112_1 0:ea35c18c85fc 27 }
lilac0112_1 0:ea35c18c85fc 28 else{
lilac0112_1 0:ea35c18c85fc 29 ir_x *= (double)(sys.s_pow);
lilac0112_1 0:ea35c18c85fc 30 ir_y *= (double)(sys.s_pow);
lilac0112_1 0:ea35c18c85fc 31 }
lilac0112_1 0:ea35c18c85fc 32 //Lineを考慮していないIrのみの値
lilac0112_1 0:ea35c18c85fc 33 vx = ir_x;
lilac0112_1 0:ea35c18c85fc 34 vy = ir_y;
lilac0112_1 0:ea35c18c85fc 35 //Line検出方向を調べる
lilac0112_1 2:635947de1583 36 LineDir[A_SPOT] = (!((vx>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==0))));
lilac0112_1 2:635947de1583 37 LineDir[B_SPOT] = (!((vx<0)&&((data.lnFlag[A_SPOT]==0)&&(data.lnFlag[B_SPOT]==1))));
lilac0112_1 2:635947de1583 38 LineDir[C_SPOT] = (!((vy<0)&&((data.lnFlag[C_SPOT]==1)&&(1))));
lilac0112_1 2:635947de1583 39 LineDir[AB_SPOT] = (!((vy>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==1))));
lilac0112_1 0:ea35c18c85fc 40
lilac0112_1 0:ea35c18c85fc 41 LineStop[X_AXIS] = LineDir[A_SPOT]*LineDir[B_SPOT];
lilac0112_1 0:ea35c18c85fc 42 LineStop[Y_AXIS] = LineDir[C_SPOT]*LineDir[AB_SPOT];
lilac0112_1 0:ea35c18c85fc 43
lilac0112_1 0:ea35c18c85fc 44 //Ir
lilac0112_1 0:ea35c18c85fc 45 move(
lilac0112_1 0:ea35c18c85fc 46 vx*LineStop[X_AXIS] + (LINE_RF)*(vy!=0)*((-1)*(LineDir[A_SPOT]==0) + (LineDir[B_SPOT]==0)),
lilac0112_1 0:ea35c18c85fc 47 vy*LineStop[Y_AXIS] + (LINE_RF)*(vx!=0)*((LineDir[C_SPOT]==0) + (-1)*(LineDir[AB_SPOT]==0)),
lilac0112_1 0:ea35c18c85fc 48 vs
lilac0112_1 0:ea35c18c85fc 49 );
lilac0112_1 0:ea35c18c85fc 50 if(sys.MotorFlag==1){
lilac0112_1 0:ea35c18c85fc 51 tx_motor();
lilac0112_1 0:ea35c18c85fc 52 sys.MotorFlag=0;
lilac0112_1 0:ea35c18c85fc 53 }
lilac0112_1 0:ea35c18c85fc 54 return;
lilac0112_1 0:ea35c18c85fc 55 }
lilac0112_1 0:ea35c18c85fc 56 uint8_t HmcResetFlag;
lilac0112_1 0:ea35c18c85fc 57 void HmcReset(void){
lilac0112_1 10:6df631c39f9b 58 hmc_reset=HMC_RST;
lilac0112_1 0:ea35c18c85fc 59 wait_us(100);
lilac0112_1 10:6df631c39f9b 60 hmc_reset=HMC_RUN;
lilac0112_1 0:ea35c18c85fc 61 }
lilac0112_1 0:ea35c18c85fc 62 uint8_t LineReverseFlag;
lilac0112_1 0:ea35c18c85fc 63 void LineReverse(void){
lilac0112_1 0:ea35c18c85fc 64 LineReverseFlag=0;
lilac0112_1 0:ea35c18c85fc 65 }
lilac0112_1 0:ea35c18c85fc 66 uint8_t LineSign[3];
lilac0112_1 0:ea35c18c85fc 67 uint8_t LineFirst[2];
lilac0112_1 0:ea35c18c85fc 68 uint8_t LinePriority[2];//0を後に，1を優先
lilac0112_1 2:635947de1583 69 void LineClear_A(void){LineSign[A_SPOT]=0;data.lnFlag[A_SPOT]=0;}
lilac0112_1 2:635947de1583 70 void LineClear_B(void){LineSign[B_SPOT]=0;data.lnFlag[B_SPOT]=0;}
lilac0112_1 2:635947de1583 71 void LineClear_C(void){LineSign[C_SPOT]=0;data.lnFlag[C_SPOT]=0;}
lilac0112_1 0:ea35c18c85fc 72 void LineCall_A(void){
lilac0112_1 0:ea35c18c85fc 73 //
lilac0112_1 0:ea35c18c85fc 74 LineSign[A_SPOT] = 1;
lilac0112_1 0:ea35c18c85fc 75 if(LineSign[B_SPOT]==0){
lilac0112_1 0:ea35c18c85fc 76 LineFirst[X_AXIS] = A_SPOT;
lilac0112_1 0:ea35c18c85fc 77 }
lilac0112_1 0:ea35c18c85fc 78 if((LineSign[A_SPOT]==1)&&(LineSign[B_SPOT]==1)){
lilac0112_1 0:ea35c18c85fc 79 if(LineSign[C_SPOT]==0){
lilac0112_1 0:ea35c18c85fc 80 LineFirst[Y_AXIS] = AB_SPOT;
lilac0112_1 0:ea35c18c85fc 81 }
lilac0112_1 0:ea35c18c85fc 82 }
lilac0112_1 0:ea35c18c85fc 83 //
lilac0112_1 2:635947de1583 84 if((Line[A_SPOT].read()==1)||(1)) data.lnFlag[A_SPOT]=1;
lilac0112_1 0:ea35c18c85fc 85 //
lilac0112_1 0:ea35c18c85fc 86 Line_timeout[A_SPOT].attach(&LineClear_A, LINE_DELAY);
lilac0112_1 0:ea35c18c85fc 87 }
lilac0112_1 0:ea35c18c85fc 88 void LineCall_B(void){
lilac0112_1 0:ea35c18c85fc 89 //
lilac0112_1 0:ea35c18c85fc 90 LineSign[B_SPOT] = 1;
lilac0112_1 0:ea35c18c85fc 91 if(LineSign[A_SPOT]==0){
lilac0112_1 0:ea35c18c85fc 92 LineFirst[X_AXIS] = B_SPOT;
lilac0112_1 0:ea35c18c85fc 93 }
lilac0112_1 0:ea35c18c85fc 94 if((LineSign[A_SPOT]==1)&&(LineSign[B_SPOT]==1)){
lilac0112_1 0:ea35c18c85fc 95 if(LineSign[C_SPOT]==0){
lilac0112_1 0:ea35c18c85fc 96 LineFirst[Y_AXIS] = AB_SPOT;
lilac0112_1 0:ea35c18c85fc 97 }
lilac0112_1 0:ea35c18c85fc 98 }
lilac0112_1 0:ea35c18c85fc 99 //
lilac0112_1 2:635947de1583 100 if((Line[B_SPOT].read()==1)||(1)) data.lnFlag[B_SPOT]=1;
lilac0112_1 0:ea35c18c85fc 101 //
lilac0112_1 0:ea35c18c85fc 102 Line_timeout[B_SPOT].attach(&LineClear_B, LINE_DELAY);
lilac0112_1 0:ea35c18c85fc 103 }
lilac0112_1 0:ea35c18c85fc 104 void LineCall_C(void){
lilac0112_1 0:ea35c18c85fc 105 //
lilac0112_1 0:ea35c18c85fc 106 LineSign[C_SPOT] = 1;
lilac0112_1 0:ea35c18c85fc 107 if(!((LineSign[A_SPOT]==1)&&(LineSign[B_SPOT]==1))){
lilac0112_1 0:ea35c18c85fc 108 LineFirst[Y_AXIS] = C_SPOT;
lilac0112_1 0:ea35c18c85fc 109 }
lilac0112_1 0:ea35c18c85fc 110 //
lilac0112_1 2:635947de1583 111 if((Line[C_SPOT].read()==1)||(1)) data.lnFlag[C_SPOT]=1;
lilac0112_1 0:ea35c18c85fc 112 //
lilac0112_1 0:ea35c18c85fc 113 Line_timeout[C_SPOT].attach(&LineClear_C, LINE_DELAY);
lilac0112_1 0:ea35c18c85fc 114 }
lilac0112_1 0:ea35c18c85fc 115 void modeAttack1(void){
lilac0112_1 0:ea35c18c85fc 116 double ir_x, ir_y;
lilac0112_1 0:ea35c18c85fc 117 int vx,vy,vs;
lilac0112_1 0:ea35c18c85fc 118 uint8_t LineDir[4];
lilac0112_1 0:ea35c18c85fc 119 uint8_t LineStop[2];
lilac0112_1 0:ea35c18c85fc 120 uint8_t IrRange[4];
lilac0112_1 0:ea35c18c85fc 121 uint8_t LinePulse[4];
lilac0112_1 0:ea35c18c85fc 122 uint8_t static LineBind[4];
lilac0112_1 0:ea35c18c85fc 123 if(sys.KickOffFlag==1){
lilac0112_1 0:ea35c18c85fc 124 LineBind[0]=0;
lilac0112_1 0:ea35c18c85fc 125 LineBind[1]=0;
lilac0112_1 0:ea35c18c85fc 126 LineBind[2]=0;
lilac0112_1 0:ea35c18c85fc 127 LineBind[3]=0;
lilac0112_1 0:ea35c18c85fc 128 LineReverseFlag=0;
lilac0112_1 0:ea35c18c85fc 129
lilac0112_1 0:ea35c18c85fc 130 LineSign[A_SPOT]=0;
lilac0112_1 0:ea35c18c85fc 131 LineSign[B_SPOT]=0;
lilac0112_1 0:ea35c18c85fc 132 LineSign[C_SPOT]=0;
lilac0112_1 0:ea35c18c85fc 133
lilac0112_1 2:635947de1583 134 data.lnFlag[A_SPOT]=0;
lilac0112_1 2:635947de1583 135 data.lnFlag[B_SPOT]=0;
lilac0112_1 2:635947de1583 136 data.lnFlag[C_SPOT]=0;
lilac0112_1 0:ea35c18c85fc 137
lilac0112_1 0:ea35c18c85fc 138 sys.KickOffFlag=0;
lilac0112_1 0:ea35c18c85fc 139 }
lilac0112_1 0:ea35c18c85fc 140 if(sys.IrFlag==1){
lilac0112_1 0:ea35c18c85fc 141 ReadIr();
lilac0112_1 0:ea35c18c85fc 142 sys.IrFlag=0;
lilac0112_1 0:ea35c18c85fc 143 }
lilac0112_1 0:ea35c18c85fc 144 if(sys.PidFlag==1){
lilac0112_1 0:ea35c18c85fc 145 //Line_ticker.detach();
lilac0112_1 0:ea35c18c85fc 146 PidUpdate();
lilac0112_1 0:ea35c18c85fc 147 //Line_ticker.attach(&ReadLine, 0.005);
lilac0112_1 0:ea35c18c85fc 148 sys.PidFlag=0;
lilac0112_1 0:ea35c18c85fc 149 }
lilac0112_1 5:5ff3a7d5d8c2 150 ir_x = ir_move_val_old[data.irNotice][data.irPosition][IR_X];
lilac0112_1 5:5ff3a7d5d8c2 151 ir_y = ir_move_val_old[data.irNotice][data.irPosition][IR_Y];
lilac0112_1 2:635947de1583 152 if(data.irPosition<8){
lilac0112_1 0:ea35c18c85fc 153 ir_x *= sys.l_pow;
lilac0112_1 0:ea35c18c85fc 154 ir_y *= sys.l_pow;
lilac0112_1 0:ea35c18c85fc 155 }
lilac0112_1 0:ea35c18c85fc 156 else{
lilac0112_1 0:ea35c18c85fc 157 ir_x *= sys.s_pow;
lilac0112_1 0:ea35c18c85fc 158 ir_y *= sys.s_pow;
lilac0112_1 0:ea35c18c85fc 159 }
lilac0112_1 0:ea35c18c85fc 160
lilac0112_1 0:ea35c18c85fc 161 //Lineを考慮していないIrのみの値
lilac0112_1 0:ea35c18c85fc 162 vx = ir_x;
lilac0112_1 0:ea35c18c85fc 163 vy = ir_y;
lilac0112_1 0:ea35c18c85fc 164 //Line検出方向を調べる
lilac0112_1 2:635947de1583 165 LineDir[A_SPOT] = (!((vx>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==0))&&(1)));
lilac0112_1 2:635947de1583 166 LineDir[B_SPOT] = (!((vx<0)&&((data.lnFlag[A_SPOT]==0)&&(data.lnFlag[B_SPOT]==1))&&(1)));
lilac0112_1 2:635947de1583 167 LineDir[C_SPOT] = (!((vy<0)&&((data.lnFlag[C_SPOT]==1)&&(1 ))&&(1)));
lilac0112_1 2:635947de1583 168 LineDir[AB_SPOT] = (!((vy>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==1))&&(1)));
lilac0112_1 0:ea35c18c85fc 169
lilac0112_1 0:ea35c18c85fc 170 LineStop[X_AXIS] = LineDir[A_SPOT]*LineDir[B_SPOT];
lilac0112_1 0:ea35c18c85fc 171 LineStop[Y_AXIS] = LineDir[C_SPOT]*LineDir[AB_SPOT];
lilac0112_1 0:ea35c18c85fc 172
lilac0112_1 0:ea35c18c85fc 173 //Ir
lilac0112_1 0:ea35c18c85fc 174 //strict
lilac0112_1 0:ea35c18c85fc 175 /*
lilac0112_1 2:635947de1583 176 IrRange[A_SPOT] = ((18<=data.irPosition)&&(data.irPosition<=19))||((8<=data.irPosition)&&(data.irPosition<=10))
lilac0112_1 2:635947de1583 177 ||((6<=data.irPosition)&&(data.irPosition<=7))||(( 1)&&(data.irPosition<=1));
lilac0112_1 2:635947de1583 178 IrRange[B_SPOT] = ((12<=data.irPosition)&&(data.irPosition<=16))||((2<=data.irPosition)&&(data.irPosition<=5));
lilac0112_1 2:635947de1583 179 IrRange[C_SPOT] = ((15<=data.irPosition)&&(data.irPosition<=19))||((4<=data.irPosition)&&(data.irPosition<=7));
lilac0112_1 2:635947de1583 180 IrRange[AB_SPOT] = ((9<=data.irPosition)&&(data.irPosition<=13))||(( 1)&&(data.irPosition<=3));
lilac0112_1 0:ea35c18c85fc 181 */
lilac0112_1 0:ea35c18c85fc 182 //sweet
lilac0112_1 2:635947de1583 183 IrRange[A_SPOT] = ((19<=data.irPosition)&&(data.irPosition<=19))||((8<=data.irPosition)&&(data.irPosition<=9))
lilac0112_1 2:635947de1583 184 ||((6<=data.irPosition)&&(data.irPosition<=7))||(( 1)&&(data.irPosition<=1));
lilac0112_1 2:635947de1583 185 IrRange[B_SPOT] = ((13<=data.irPosition)&&(data.irPosition<=15))||((2<=data.irPosition)&&(data.irPosition<=5));
lilac0112_1 2:635947de1583 186 IrRange[C_SPOT] = ((16<=data.irPosition)&&(data.irPosition<=18))||((4<=data.irPosition)&&(data.irPosition<=7));
lilac0112_1 2:635947de1583 187 IrRange[AB_SPOT] = ((10<=data.irPosition)&&(data.irPosition<=12))||(( 1)&&(data.irPosition<=3));
lilac0112_1 0:ea35c18c85fc 188
lilac0112_1 0:ea35c18c85fc 189
lilac0112_1 0:ea35c18c85fc 190 LinePulse[A_SPOT] = ((IrRange[A_SPOT]==1)&&(LineDir[A_SPOT]==0));
lilac0112_1 0:ea35c18c85fc 191 LinePulse[B_SPOT] = ((IrRange[B_SPOT]==1)&&(LineDir[B_SPOT]==0));
lilac0112_1 0:ea35c18c85fc 192 LinePulse[C_SPOT] = ((IrRange[C_SPOT]==1)&&(LineDir[C_SPOT]==0));
lilac0112_1 0:ea35c18c85fc 193 LinePulse[AB_SPOT] = ((IrRange[AB_SPOT]==1)&&(LineDir[AB_SPOT]==0));
lilac0112_1 0:ea35c18c85fc 194
lilac0112_1 0:ea35c18c85fc 195 LineBind[A_SPOT] = ((LinePulse[A_SPOT])||((IrRange[A_SPOT]==1)&&(LineBind[A_SPOT]==1)))&&(LineBind[B_SPOT]==0);
lilac0112_1 0:ea35c18c85fc 196 LineBind[B_SPOT] = ((LinePulse[B_SPOT])||((IrRange[B_SPOT]==1)&&(LineBind[B_SPOT]==1)))&&(LineBind[A_SPOT]==0);
lilac0112_1 0:ea35c18c85fc 197 LineBind[C_SPOT] = ((LinePulse[C_SPOT])||((IrRange[C_SPOT]==1)&&(LineBind[C_SPOT]==1)))&&(LineBind[AB_SPOT]==0);
lilac0112_1 0:ea35c18c85fc 198 LineBind[AB_SPOT] = ((LinePulse[AB_SPOT])||((IrRange[AB_SPOT]==1)&&(LineBind[AB_SPOT]==1)))&&(LineBind[C_SPOT]==0);
lilac0112_1 0:ea35c18c85fc 199 /*
lilac0112_1 0:ea35c18c85fc 200 LineBind[A_SPOT] = ((IrRange[A_SPOT]==1)&&((LineDir[A_SPOT]==0)||(LineBind[A_SPOT]==1)));
lilac0112_1 0:ea35c18c85fc 201 LineBind[B_SPOT] = ((IrRange[B_SPOT]==1)&&((LineDir[B_SPOT]==0)||(LineBind[B_SPOT]==1)));
lilac0112_1 0:ea35c18c85fc 202 LineBind[C_SPOT] = ((IrRange[C_SPOT]==1)&&((LineDir[C_SPOT]==0)||(LineBind[C_SPOT]==1)));
lilac0112_1 0:ea35c18c85fc 203 LineBind[AB_SPOT] = ((IrRange[AB_SPOT]==1)&&((LineDir[AB_SPOT]==0)||(LineBind[AB_SPOT]==1)));
lilac0112_1 0:ea35c18c85fc 204 */
lilac0112_1 0:ea35c18c85fc 205 vx = vx*LineStop[X_AXIS] + (LINE_RF)*(vy!=0)*((-1)*(LineDir[A_SPOT]==0) + (LineDir[B_SPOT]==0));
lilac0112_1 0:ea35c18c85fc 206 vy = vy*LineStop[Y_AXIS] + (LINE_RF)*(vx!=0)*((LineDir[C_SPOT]==0) + (-1)*(LineDir[AB_SPOT]==0));
lilac0112_1 0:ea35c18c85fc 207 vs = cmps_set.OutputPID;
lilac0112_1 0:ea35c18c85fc 208 if((LineBind[A_SPOT]==1)||(LineBind[B_SPOT]==1)||(LineBind[C_SPOT]==1)||(LineBind[AB_SPOT]==1)){
lilac0112_1 2:635947de1583 209 if(LineRaw>0){
lilac0112_1 0:ea35c18c85fc 210 vx=(LINE_RF*2)*((-1)*IrRange[A_SPOT] + IrRange[B_SPOT]);
lilac0112_1 0:ea35c18c85fc 211 vy=(LINE_RF*2)*( IrRange[C_SPOT] + (-1)*IrRange[AB_SPOT]);
lilac0112_1 0:ea35c18c85fc 212 /*vx=(LINE_RF*2)*((-1)*(LineFirst[X_AXIS] == A_SPOT) + (LineFirst[X_AXIS] == B_SPOT));
lilac0112_1 0:ea35c18c85fc 213 vy=(LINE_RF*2)*( (LineFirst[Y_AXIS] == C_SPOT) + (-1)*(LineFirst[Y_AXIS] == AB_SPOT));
lilac0112_1 0:ea35c18c85fc 214 Line_timeout[A_SPOT].attach(&LineClear_A, LINE_DELAY);
lilac0112_1 0:ea35c18c85fc 215 Line_timeout[B_SPOT].attach(&LineClear_B, LINE_DELAY);
lilac0112_1 0:ea35c18c85fc 216 Line_timeout[C_SPOT].attach(&LineClear_C, LINE_DELAY);*/
lilac0112_1 0:ea35c18c85fc 217 }
lilac0112_1 0:ea35c18c85fc 218 else{
lilac0112_1 0:ea35c18c85fc 219 vx=0;
lilac0112_1 0:ea35c18c85fc 220 vy=0;
lilac0112_1 0:ea35c18c85fc 221 }
lilac0112_1 0:ea35c18c85fc 222 }
lilac0112_1 2:635947de1583 223 if(LineRaw>0){
lilac0112_1 0:ea35c18c85fc 224 Line_timeout[A_SPOT].attach(&LineClear_A, LINE_DELAY);
lilac0112_1 0:ea35c18c85fc 225 Line_timeout[B_SPOT].attach(&LineClear_B, LINE_DELAY);
lilac0112_1 0:ea35c18c85fc 226 Line_timeout[C_SPOT].attach(&LineClear_C, LINE_DELAY);
lilac0112_1 0:ea35c18c85fc 227 }
lilac0112_1 0:ea35c18c85fc 228 move(
lilac0112_1 0:ea35c18c85fc 229 vx,
lilac0112_1 0:ea35c18c85fc 230 vy,
lilac0112_1 0:ea35c18c85fc 231 vs
lilac0112_1 0:ea35c18c85fc 232 );
lilac0112_1 0:ea35c18c85fc 233 if(sys.MotorFlag==1){
lilac0112_1 0:ea35c18c85fc 234 tx_motor();
lilac0112_1 0:ea35c18c85fc 235 sys.MotorFlag=0;
lilac0112_1 0:ea35c18c85fc 236 }
lilac0112_1 0:ea35c18c85fc 237 return;
lilac0112_1 0:ea35c18c85fc 238 }
lilac0112_1 0:ea35c18c85fc 239 void modeAttack2(void){
lilac0112_1 0:ea35c18c85fc 240 double ir_x, ir_y;
lilac0112_1 5:5ff3a7d5d8c2 241 int vx,vy,vs;
lilac0112_1 5:5ff3a7d5d8c2 242 /*int LineForce[2];
lilac0112_1 0:ea35c18c85fc 243 uint8_t LineDir[4];
lilac0112_1 0:ea35c18c85fc 244 uint8_t LineOn[4];
lilac0112_1 0:ea35c18c85fc 245 uint8_t LineReturn[4];
lilac0112_1 0:ea35c18c85fc 246 uint8_t LineStop[2];
lilac0112_1 0:ea35c18c85fc 247 uint8_t IrRange[4];
lilac0112_1 5:5ff3a7d5d8c2 248 uint8_t static LineBind[4];*/
lilac0112_1 0:ea35c18c85fc 249 //buint8_t static spi_count;
lilac0112_1 0:ea35c18c85fc 250 if(sys.KickOffFlag==1){
lilac0112_1 5:5ff3a7d5d8c2 251
lilac0112_1 5:5ff3a7d5d8c2 252 sys.MotorFlag=0;
lilac0112_1 5:5ff3a7d5d8c2 253 sys.IrFlag=0;
lilac0112_1 5:5ff3a7d5d8c2 254 /*LineBind[0]=0;
lilac0112_1 0:ea35c18c85fc 255 LineBind[1]=0;
lilac0112_1 0:ea35c18c85fc 256 LineBind[2]=0;
lilac0112_1 0:ea35c18c85fc 257 LineBind[3]=0;
lilac0112_1 0:ea35c18c85fc 258 LineReverseFlag=0;
lilac0112_1 0:ea35c18c85fc 259
lilac0112_1 0:ea35c18c85fc 260 LineSign[A_SPOT]=0;
lilac0112_1 0:ea35c18c85fc 261 LineSign[B_SPOT]=0;
lilac0112_1 0:ea35c18c85fc 262 LineSign[C_SPOT]=0;
lilac0112_1 0:ea35c18c85fc 263
lilac0112_1 2:635947de1583 264 data.lnFlag[A_SPOT]=0;
lilac0112_1 2:635947de1583 265 data.lnFlag[B_SPOT]=0;
lilac0112_1 2:635947de1583 266 data.lnFlag[C_SPOT]=0;
lilac0112_1 0:ea35c18c85fc 267
lilac0112_1 0:ea35c18c85fc 268 HmcResetFlag = 0;
lilac0112_1 10:6df631c39f9b 269 sys.UswFlag = 0;
lilac0112_1 0:ea35c18c85fc 270 //spi_count=0;
lilac0112_1 5:5ff3a7d5d8c2 271 */
lilac0112_1 10:6df631c39f9b 272 hmc_reset=HMC_RUN;
lilac0112_1 0:ea35c18c85fc 273 sys.KickFlag = 0;
lilac0112_1 0:ea35c18c85fc 274
lilac0112_1 0:ea35c18c85fc 275 sys.KickOffFlag=0;
lilac0112_1 0:ea35c18c85fc 276 //while((Sw[2].read()==1)&&(Sw[3].read()==1));//押して離すとスタート
lilac0112_1 0:ea35c18c85fc 277 }
lilac0112_1 0:ea35c18c85fc 278 if(sys.IrFlag==1){
lilac0112_1 5:5ff3a7d5d8c2 279 //LED[0] = 0;
lilac0112_1 5:5ff3a7d5d8c2 280 //LED[1] = 1;
lilac0112_1 0:ea35c18c85fc 281 /*spi_count++;
lilac0112_1 0:ea35c18c85fc 282 if(spi_count%10 == 0){
lilac0112_1 0:ea35c18c85fc 283 ReadPing();
lilac0112_1 0:ea35c18c85fc 284 }
lilac0112_1 0:ea35c18c85fc 285 else{
lilac0112_1 0:ea35c18c85fc 286 ReadIr();
lilac0112_1 0:ea35c18c85fc 287 }
lilac0112_1 0:ea35c18c85fc 288 if(spi_count==20) spi_count=0;
lilac0112_1 0:ea35c18c85fc 289 */
lilac0112_1 0:ea35c18c85fc 290 ReadIr();
lilac0112_1 0:ea35c18c85fc 291 sys.IrFlag=0;
lilac0112_1 0:ea35c18c85fc 292 }
lilac0112_1 0:ea35c18c85fc 293 if(sys.PidFlag==1){
lilac0112_1 0:ea35c18c85fc 294 PidUpdate();
lilac0112_1 0:ea35c18c85fc 295 sys.PidFlag=0;
lilac0112_1 0:ea35c18c85fc 296 }
lilac0112_1 10:6df631c39f9b 297 /*if(sys.UswFlag==1){
lilac0112_1 0:ea35c18c85fc 298 ReadPing();
lilac0112_1 10:6df631c39f9b 299 sys.UswFlag=0;
lilac0112_1 5:5ff3a7d5d8c2 300 }*/
lilac0112_1 0:ea35c18c85fc 301 /*
lilac0112_1 0:ea35c18c85fc 302 if(HmcResetFlag==1){
lilac0112_1 0:ea35c18c85fc 303 HmcReset();
lilac0112_1 0:ea35c18c85fc 304 HmcResetFlag=0;
lilac0112_1 0:ea35c18c85fc 305 }
lilac0112_1 0:ea35c18c85fc 306 */
lilac0112_1 5:5ff3a7d5d8c2 307 //ir_x = ir_move_val_old[data.irNotice][data.irPosition][IR_X];
lilac0112_1 5:5ff3a7d5d8c2 308 //ir_y = ir_move_val_old[data.irNotice][data.irPosition][IR_Y];
lilac0112_1 5:5ff3a7d5d8c2 309 ir_x = ir_move_val[0][data.irNotice][data.irPosition][IR_X_DIR]+ir_move_val[0][data.irNotice][data.irPosition][IR_X_TURN];
lilac0112_1 5:5ff3a7d5d8c2 310 ir_y = ir_move_val[0][data.irNotice][data.irPosition][IR_Y_DIR]+ir_move_val[0][data.irNotice][data.irPosition][IR_Y_TURN];
lilac0112_1 5:5ff3a7d5d8c2 311
lilac0112_1 2:635947de1583 312 if(data.irPosition<8){
lilac0112_1 0:ea35c18c85fc 313 ir_x *= sys.l_pow;
lilac0112_1 0:ea35c18c85fc 314 ir_y *= sys.l_pow;
lilac0112_1 0:ea35c18c85fc 315 }
lilac0112_1 0:ea35c18c85fc 316 else{
lilac0112_1 0:ea35c18c85fc 317 ir_x *= sys.s_pow;
lilac0112_1 0:ea35c18c85fc 318 ir_y *= sys.s_pow;
lilac0112_1 0:ea35c18c85fc 319 }
lilac0112_1 0:ea35c18c85fc 320
lilac0112_1 0:ea35c18c85fc 321 //Lineを考慮していないIrのみの値
lilac0112_1 0:ea35c18c85fc 322 vx = ir_x;
lilac0112_1 0:ea35c18c85fc 323 vy = ir_y;
lilac0112_1 5:5ff3a7d5d8c2 324 /*
lilac0112_1 2:635947de1583 325 if((data.irPosition==10)&&(vy>0)){
lilac0112_1 0:ea35c18c85fc 326 vy += 0;//前進加速
lilac0112_1 0:ea35c18c85fc 327 }
lilac0112_1 2:635947de1583 328 if((data.irPosition==11)&&(vy>0)){
lilac0112_1 0:ea35c18c85fc 329 vy += 0;//前進加速
lilac0112_1 0:ea35c18c85fc 330 if(sys.KickFlag==1){
lilac0112_1 0:ea35c18c85fc 331 DriveSolenoid();
lilac0112_1 0:ea35c18c85fc 332 }
lilac0112_1 0:ea35c18c85fc 333 }
lilac0112_1 2:635947de1583 334 if((data.irPosition==12)&&(vy>0)){
lilac0112_1 0:ea35c18c85fc 335 vy += 0;//前進加速
lilac0112_1 0:ea35c18c85fc 336 }
lilac0112_1 2:635947de1583 337 if((data.irPosition==1)&&(vy>0)){
lilac0112_1 0:ea35c18c85fc 338 vy += 0;//前進加速
lilac0112_1 0:ea35c18c85fc 339 }
lilac0112_1 2:635947de1583 340 if((data.irPosition==2)&&(vy>0)){
lilac0112_1 0:ea35c18c85fc 341 vy += 0;//前進加速
lilac0112_1 0:ea35c18c85fc 342 }
lilac0112_1 0:ea35c18c85fc 343
lilac0112_1 2:635947de1583 344 if((data.irPosition==17)&&(data.ping[L_PING]>data.ping[R_PING])){
lilac0112_1 0:ea35c18c85fc 345 vx *= -1.0;//背後回り込みの左右判断
lilac0112_1 5:5ff3a7d5d8c2 346 }*/
lilac0112_1 0:ea35c18c85fc 347 /*
lilac0112_1 0:ea35c18c85fc 348 if((cmps_set.InputPID<(REFERENCE-30))||(cmps_set.InputPID>(REFERENCE+30))){
lilac0112_1 0:ea35c18c85fc 349 vx = vx*(0.75);
lilac0112_1 0:ea35c18c85fc 350 vy = vy*(0.75);
lilac0112_1 0:ea35c18c85fc 351 }
lilac0112_1 0:ea35c18c85fc 352 */
lilac0112_1 0:ea35c18c85fc 353 //Lineを踏み始めた方向を調べる
lilac0112_1 2:635947de1583 354 /*LineDir[A_SPOT] = (vx>0)&&((data.lnFlag[A_SPOT]==1)&&(1 ))&&(LineFirst[X_AXIS] == A_SPOT);
lilac0112_1 2:635947de1583 355 LineDir[B_SPOT] = (vx<0)&&((data.lnFlag[B_SPOT]==1)&&(1 ))&&(LineFirst[X_AXIS] == B_SPOT);
lilac0112_1 2:635947de1583 356 LineDir[C_SPOT] = (vy<0)&&((data.lnFlag[C_SPOT]==1)&&(1 ))&&(LineFirst[Y_AXIS] == C_SPOT);
lilac0112_1 2:635947de1583 357 LineDir[AB_SPOT]= (vy>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==1))&&(LineFirst[Y_AXIS] == AB_SPOT);*/
lilac0112_1 5:5ff3a7d5d8c2 358 /*
lilac0112_1 0:ea35c18c85fc 359 LineDir[A_SPOT] = (vx>0)&&((LineSign[A_SPOT]==1)&&(1 ))&&(LineFirst[X_AXIS] == A_SPOT);
lilac0112_1 0:ea35c18c85fc 360 LineDir[B_SPOT] = (vx<0)&&((LineSign[B_SPOT]==1)&&(1 ))&&(LineFirst[X_AXIS] == B_SPOT);
lilac0112_1 0:ea35c18c85fc 361 LineDir[C_SPOT] = (vy<0)&&((LineSign[C_SPOT]==1)&&(1 ))&&(LineFirst[Y_AXIS] == C_SPOT);
lilac0112_1 0:ea35c18c85fc 362 LineDir[AB_SPOT]= (vy>0)&&((LineSign[A_SPOT]==1)&&(LineSign[B_SPOT]==1))&&(LineFirst[Y_AXIS] == AB_SPOT);
lilac0112_1 0:ea35c18c85fc 363
lilac0112_1 0:ea35c18c85fc 364 //Irボールの方向
lilac0112_1 0:ea35c18c85fc 365 //strict
lilac0112_1 0:ea35c18c85fc 366
lilac0112_1 2:635947de1583 367 IrRange[A_SPOT] = ((18<=data.irPosition)&&(data.irPosition<=19))||((8<=data.irPosition)&&(data.irPosition<=10))
lilac0112_1 2:635947de1583 368 ||((6<=data.irPosition)&&(data.irPosition<=7))||(( 1)&&(data.irPosition<=1));
lilac0112_1 2:635947de1583 369 IrRange[B_SPOT] = ((12<=data.irPosition)&&(data.irPosition<=16))||((2<=data.irPosition)&&(data.irPosition<=5));
lilac0112_1 2:635947de1583 370 IrRange[C_SPOT] = ((15<=data.irPosition)&&(data.irPosition<=19))||((4<=data.irPosition)&&(data.irPosition<=7));
lilac0112_1 2:635947de1583 371 IrRange[AB_SPOT] = ((9<=data.irPosition)&&(data.irPosition<=13))||(( 1)&&(data.irPosition<=3));
lilac0112_1 5:5ff3a7d5d8c2 372 */
lilac0112_1 0:ea35c18c85fc 373 //sweet
lilac0112_1 2:635947de1583 374 /*IrRange[A_SPOT] = ((19<=data.irPosition)&&(data.irPosition<=19))||((8<=data.irPosition)&&(data.irPosition<=9))
lilac0112_1 2:635947de1583 375 ||((6<=data.irPosition)&&(data.irPosition<=7))||(( 1)&&(data.irPosition<=1));
lilac0112_1 2:635947de1583 376 IrRange[B_SPOT] = ((13<=data.irPosition)&&(data.irPosition<=15))||((2<=data.irPosition)&&(data.irPosition<=5));
lilac0112_1 2:635947de1583 377 IrRange[C_SPOT] = ((16<=data.irPosition)&&(data.irPosition<=18))||((4<=data.irPosition)&&(data.irPosition<=7));
lilac0112_1 2:635947de1583 378 IrRange[AB_SPOT] = ((10<=data.irPosition)&&(data.irPosition<=12))||(( 1)&&(data.irPosition<=3));*/
lilac0112_1 5:5ff3a7d5d8c2 379 /*
lilac0112_1 0:ea35c18c85fc 380 //none
lilac0112_1 2:635947de1583 381 if(data.irNotice==IR_NONE){
lilac0112_1 0:ea35c18c85fc 382 IrRange[A_SPOT] = 0;
lilac0112_1 0:ea35c18c85fc 383 IrRange[B_SPOT] = 0;
lilac0112_1 0:ea35c18c85fc 384 IrRange[C_SPOT] = 0;
lilac0112_1 0:ea35c18c85fc 385 IrRange[AB_SPOT] = 0;
lilac0112_1 0:ea35c18c85fc 386 }
lilac0112_1 0:ea35c18c85fc 387 //白線を踏み始めた方向とボールの方向が一致．(SelfHold)
lilac0112_1 0:ea35c18c85fc 388 LineBind[A_SPOT] = (IrRange[A_SPOT]==1)&&((LineDir[A_SPOT]==1)||(LineBind[A_SPOT]==1));
lilac0112_1 0:ea35c18c85fc 389 LineBind[B_SPOT] = (IrRange[B_SPOT]==1)&&((LineDir[B_SPOT]==1)||(LineBind[B_SPOT]==1));
lilac0112_1 0:ea35c18c85fc 390 LineBind[C_SPOT] = (IrRange[C_SPOT]==1)&&((LineDir[C_SPOT]==1)||(LineBind[C_SPOT]==1));
lilac0112_1 0:ea35c18c85fc 391 LineBind[AB_SPOT] = (IrRange[AB_SPOT]==1)&&((LineDir[AB_SPOT]==1)||(LineBind[AB_SPOT]==1));
lilac0112_1 0:ea35c18c85fc 392
lilac0112_1 0:ea35c18c85fc 393 LineStop[X_AXIS] = (LineBind[A_SPOT]==0)*(LineBind[B_SPOT]==0);
lilac0112_1 0:ea35c18c85fc 394 LineStop[Y_AXIS] = (LineBind[C_SPOT]==0)*(LineBind[AB_SPOT]==0);
lilac0112_1 0:ea35c18c85fc 395
lilac0112_1 0:ea35c18c85fc 396 //白線踏んでる
lilac0112_1 2:635947de1583 397 if(LineRaw>0){
lilac0112_1 0:ea35c18c85fc 398 LineOn[A_SPOT] = (LineSign[A_SPOT]==1) &&(LineFirst[X_AXIS]==A_SPOT);
lilac0112_1 0:ea35c18c85fc 399 LineOn[B_SPOT] = (LineSign[B_SPOT]==1) &&(LineFirst[X_AXIS]==B_SPOT);
lilac0112_1 0:ea35c18c85fc 400 LineOn[C_SPOT] = (LineSign[C_SPOT]==1) &&(LineFirst[Y_AXIS]==C_SPOT);
lilac0112_1 0:ea35c18c85fc 401 LineOn[AB_SPOT] = ((LineSign[A_SPOT]==1)&&(LineSign[B_SPOT]==1))&&(LineFirst[Y_AXIS]==AB_SPOT);
lilac0112_1 0:ea35c18c85fc 402 //外側に向かう力を消す．
lilac0112_1 0:ea35c18c85fc 403 //x
lilac0112_1 0:ea35c18c85fc 404 if(((LineOn[A_SPOT]==1)&&(vx>0))||((LineOn[B_SPOT]==1)&&(vx<0))){
lilac0112_1 0:ea35c18c85fc 405 vx=0;
lilac0112_1 0:ea35c18c85fc 406 //yの力を加える．
lilac0112_1 0:ea35c18c85fc 407 if(vy>0){vy += 10;}
lilac0112_1 0:ea35c18c85fc 408 if(vy<0){vy -= 10;}
lilac0112_1 0:ea35c18c85fc 409 }
lilac0112_1 0:ea35c18c85fc 410 //y
lilac0112_1 0:ea35c18c85fc 411 if(((LineOn[C_SPOT]==1)&&(vy<0))||((LineOn[AB_SPOT]==1)&&(vy>0))){
lilac0112_1 0:ea35c18c85fc 412 vy=0;
lilac0112_1 0:ea35c18c85fc 413 }
lilac0112_1 0:ea35c18c85fc 414 //内側に向かう力を加える．
lilac0112_1 0:ea35c18c85fc 415 LineReturn[A_SPOT] = (LineOn[A_SPOT]==1);
lilac0112_1 0:ea35c18c85fc 416 if((LineReturn[A_SPOT]==1)&&(LineOn[AB_SPOT]==1)){
lilac0112_1 0:ea35c18c85fc 417 if(LineOn[C_SPOT]==0){
lilac0112_1 0:ea35c18c85fc 418 LineReturn[A_SPOT]=0;
lilac0112_1 0:ea35c18c85fc 419 }
lilac0112_1 0:ea35c18c85fc 420 else{
lilac0112_1 0:ea35c18c85fc 421 LineReturn[A_SPOT]=1;
lilac0112_1 0:ea35c18c85fc 422 }
lilac0112_1 0:ea35c18c85fc 423 }
lilac0112_1 0:ea35c18c85fc 424
lilac0112_1 0:ea35c18c85fc 425 LineReturn[B_SPOT] = (LineOn[B_SPOT]==1);
lilac0112_1 0:ea35c18c85fc 426 if((LineReturn[B_SPOT]==1)&&(LineOn[AB_SPOT]==1)){
lilac0112_1 0:ea35c18c85fc 427 if(LineOn[C_SPOT]==0){
lilac0112_1 0:ea35c18c85fc 428 LineReturn[B_SPOT]=0;
lilac0112_1 0:ea35c18c85fc 429 }
lilac0112_1 0:ea35c18c85fc 430 else{
lilac0112_1 0:ea35c18c85fc 431 LineReturn[B_SPOT]=1;
lilac0112_1 0:ea35c18c85fc 432 }
lilac0112_1 0:ea35c18c85fc 433 }
lilac0112_1 0:ea35c18c85fc 434 LineReturn[C_SPOT] = (LineOn[C_SPOT]==1);
lilac0112_1 0:ea35c18c85fc 435 if(LineReturn[C_SPOT]==1){
lilac0112_1 0:ea35c18c85fc 436 LineReturn[A_SPOT]=0;
lilac0112_1 0:ea35c18c85fc 437 LineReturn[B_SPOT]=0;
lilac0112_1 0:ea35c18c85fc 438 }
lilac0112_1 0:ea35c18c85fc 439 LineReturn[AB_SPOT] = (LineOn[AB_SPOT]==1);
lilac0112_1 0:ea35c18c85fc 440
lilac0112_1 0:ea35c18c85fc 441 LineForce[X_AXIS] = (LINE_RF*2)*((-1)*(LineReturn[A_SPOT]==1) + ( 1)*(LineReturn[B_SPOT]==1)) +
lilac0112_1 0:ea35c18c85fc 442 (LINE_RF*2)*(LineReturn[AB_SPOT]==1)*(LineReturn[A_SPOT]==0)*(LineReturn[B_SPOT]==0)
lilac0112_1 2:635947de1583 443 *(( 1)*(data.ping[L_PING]<40) + (-1)*(data.ping[R_PING]<40)) +
lilac0112_1 0:ea35c18c85fc 444 (LINE_RF*2)*(LineReturn[C_SPOT]==1)*(LineReturn[A_SPOT]==0)*(LineReturn[B_SPOT]==0)
lilac0112_1 2:635947de1583 445 *(( 1)*(data.ping[L_PING]<40) + (-1)*(data.ping[R_PING]<40));
lilac0112_1 0:ea35c18c85fc 446 LineForce[Y_AXIS] = (LINE_RF*2)*(( 1)*(LineReturn[C_SPOT]==1) + (-1)*(LineReturn[AB_SPOT]==1));
lilac0112_1 0:ea35c18c85fc 447
lilac0112_1 0:ea35c18c85fc 448 Line_timeout[A_SPOT].attach(&LineClear_A, LINE_DELAY);
lilac0112_1 0:ea35c18c85fc 449 Line_timeout[B_SPOT].attach(&LineClear_B, LINE_DELAY);
lilac0112_1 0:ea35c18c85fc 450 Line_timeout[C_SPOT].attach(&LineClear_C, LINE_DELAY);
lilac0112_1 0:ea35c18c85fc 451 }
lilac0112_1 0:ea35c18c85fc 452 else{
lilac0112_1 0:ea35c18c85fc 453 LineForce[X_AXIS] = 0;
lilac0112_1 0:ea35c18c85fc 454 LineForce[Y_AXIS] = 0;
lilac0112_1 0:ea35c18c85fc 455 }
lilac0112_1 5:5ff3a7d5d8c2 456 */
lilac0112_1 5:5ff3a7d5d8c2 457 //vx = vx*LineStop[X_AXIS] + LineForce[X_AXIS];
lilac0112_1 5:5ff3a7d5d8c2 458 //vy = vy*LineStop[Y_AXIS] + LineForce[Y_AXIS];
lilac0112_1 0:ea35c18c85fc 459 vs = cmps_set.OutputPID;
lilac0112_1 0:ea35c18c85fc 460 move(
lilac0112_1 0:ea35c18c85fc 461 vx,
lilac0112_1 0:ea35c18c85fc 462 vy,
lilac0112_1 0:ea35c18c85fc 463 vs
lilac0112_1 0:ea35c18c85fc 464 );
lilac0112_1 0:ea35c18c85fc 465 /*move(
lilac0112_1 0:ea35c18c85fc 466 0,
lilac0112_1 0:ea35c18c85fc 467 0,
lilac0112_1 0:ea35c18c85fc 468 10
lilac0112_1 0:ea35c18c85fc 469 );*/
lilac0112_1 0:ea35c18c85fc 470 if(sys.MotorFlag==1){
lilac0112_1 0:ea35c18c85fc 471 tx_motor();
lilac0112_1 0:ea35c18c85fc 472 sys.MotorFlag=0;
lilac0112_1 0:ea35c18c85fc 473 }
lilac0112_1 0:ea35c18c85fc 474 return;
lilac0112_1 0:ea35c18c85fc 475 }
lilac0112_1 0:ea35c18c85fc 476 void modeAttack3(void){
lilac0112_1 0:ea35c18c85fc 477 double ir_x, ir_y;
lilac0112_1 0:ea35c18c85fc 478 int vx,vy,vs, LineForce[2];
lilac0112_1 0:ea35c18c85fc 479 uint8_t LineDir[4];
lilac0112_1 0:ea35c18c85fc 480 uint8_t LineOn[4];
lilac0112_1 0:ea35c18c85fc 481 uint8_t LineReturn[4];
lilac0112_1 0:ea35c18c85fc 482 uint8_t LineStop[2];
lilac0112_1 0:ea35c18c85fc 483 uint8_t IrRange[4];
lilac0112_1 0:ea35c18c85fc 484 uint8_t static LineBind[4];
lilac0112_1 0:ea35c18c85fc 485 //buint8_t static spi_count;
lilac0112_1 0:ea35c18c85fc 486 if(sys.KickOffFlag==1){
lilac0112_1 0:ea35c18c85fc 487 LineBind[0]=0;
lilac0112_1 0:ea35c18c85fc 488 LineBind[1]=0;
lilac0112_1 0:ea35c18c85fc 489 LineBind[2]=0;
lilac0112_1 0:ea35c18c85fc 490 LineBind[3]=0;
lilac0112_1 0:ea35c18c85fc 491 LineReverseFlag=0;
lilac0112_1 0:ea35c18c85fc 492
lilac0112_1 0:ea35c18c85fc 493 LineSign[A_SPOT]=0;
lilac0112_1 0:ea35c18c85fc 494 LineSign[B_SPOT]=0;
lilac0112_1 0:ea35c18c85fc 495 LineSign[C_SPOT]=0;
lilac0112_1 0:ea35c18c85fc 496
lilac0112_1 2:635947de1583 497 data.lnFlag[A_SPOT]=0;
lilac0112_1 2:635947de1583 498 data.lnFlag[B_SPOT]=0;
lilac0112_1 2:635947de1583 499 data.lnFlag[C_SPOT]=0;
lilac0112_1 0:ea35c18c85fc 500
lilac0112_1 0:ea35c18c85fc 501 LinePriority[X_AXIS]=0;
lilac0112_1 0:ea35c18c85fc 502 LinePriority[Y_AXIS]=0;
lilac0112_1 0:ea35c18c85fc 503
lilac0112_1 0:ea35c18c85fc 504 HmcResetFlag = 0;
lilac0112_1 10:6df631c39f9b 505 sys.UswFlag = 0;
lilac0112_1 0:ea35c18c85fc 506 //spi_count=0;
lilac0112_1 0:ea35c18c85fc 507
lilac0112_1 10:6df631c39f9b 508 hmc_reset=HMC_RUN;
lilac0112_1 0:ea35c18c85fc 509 sys.KickFlag = 0;
lilac0112_1 0:ea35c18c85fc 510
lilac0112_1 0:ea35c18c85fc 511 sys.KickOffFlag=0;
lilac0112_1 0:ea35c18c85fc 512 //while((Sw[2].read()==1)&&(Sw[3].read()==1));//押して離すとスタート
lilac0112_1 0:ea35c18c85fc 513 }
lilac0112_1 0:ea35c18c85fc 514 if(sys.IrFlag==1){
lilac0112_1 0:ea35c18c85fc 515 /*spi_count++;
lilac0112_1 0:ea35c18c85fc 516 if(spi_count%10 == 0){
lilac0112_1 0:ea35c18c85fc 517 ReadPing();
lilac0112_1 0:ea35c18c85fc 518 }
lilac0112_1 0:ea35c18c85fc 519 else{
lilac0112_1 0:ea35c18c85fc 520 ReadIr();
lilac0112_1 0:ea35c18c85fc 521 }
lilac0112_1 0:ea35c18c85fc 522 if(spi_count==20) spi_count=0;
lilac0112_1 0:ea35c18c85fc 523 */
lilac0112_1 0:ea35c18c85fc 524 ReadIr();
lilac0112_1 0:ea35c18c85fc 525 sys.IrFlag=0;
lilac0112_1 0:ea35c18c85fc 526 }
lilac0112_1 0:ea35c18c85fc 527 if(sys.PidFlag==1){
lilac0112_1 0:ea35c18c85fc 528 PidUpdate();
lilac0112_1 0:ea35c18c85fc 529 sys.PidFlag=0;
lilac0112_1 0:ea35c18c85fc 530 }
lilac0112_1 10:6df631c39f9b 531 if(sys.UswFlag==1){
lilac0112_1 0:ea35c18c85fc 532 ReadPing();
lilac0112_1 10:6df631c39f9b 533 sys.UswFlag=0;
lilac0112_1 0:ea35c18c85fc 534 }
lilac0112_1 0:ea35c18c85fc 535 /*
lilac0112_1 0:ea35c18c85fc 536 if(HmcResetFlag==1){
lilac0112_1 0:ea35c18c85fc 537 HmcReset();
lilac0112_1 0:ea35c18c85fc 538 HmcResetFlag=0;
lilac0112_1 0:ea35c18c85fc 539 }
lilac0112_1 0:ea35c18c85fc 540 */
lilac0112_1 5:5ff3a7d5d8c2 541 ir_x = ir_move_val_old[data.irNotice][data.irPosition][IR_X];
lilac0112_1 5:5ff3a7d5d8c2 542 ir_y = ir_move_val_old[data.irNotice][data.irPosition][IR_Y];
lilac0112_1 2:635947de1583 543 if(data.irPosition<8){
lilac0112_1 0:ea35c18c85fc 544 ir_x *= sys.l_pow;
lilac0112_1 0:ea35c18c85fc 545 ir_y *= sys.l_pow;
lilac0112_1 0:ea35c18c85fc 546 }
lilac0112_1 0:ea35c18c85fc 547 else{
lilac0112_1 0:ea35c18c85fc 548 ir_x *= sys.s_pow;
lilac0112_1 0:ea35c18c85fc 549 ir_y *= sys.s_pow;
lilac0112_1 0:ea35c18c85fc 550 }
lilac0112_1 0:ea35c18c85fc 551
lilac0112_1 0:ea35c18c85fc 552 //Lineを考慮していないIrのみの値
lilac0112_1 0:ea35c18c85fc 553 vx = ir_x;
lilac0112_1 0:ea35c18c85fc 554 vy = ir_y;
lilac0112_1 0:ea35c18c85fc 555
lilac0112_1 2:635947de1583 556 if((data.irPosition==10)&&(vy>0)){
lilac0112_1 0:ea35c18c85fc 557 vy += 15;//前進加速
lilac0112_1 0:ea35c18c85fc 558 }
lilac0112_1 2:635947de1583 559 if((data.irPosition==11)&&(vy>0)){
lilac0112_1 0:ea35c18c85fc 560 vy += 15;//前進加速
lilac0112_1 0:ea35c18c85fc 561 if(sys.KickFlag==1){
lilac0112_1 0:ea35c18c85fc 562 DriveSolenoid();
lilac0112_1 0:ea35c18c85fc 563 }
lilac0112_1 0:ea35c18c85fc 564 }
lilac0112_1 2:635947de1583 565 if((data.irPosition==12)&&(vy>0)){
lilac0112_1 0:ea35c18c85fc 566 vy += 15;//前進加速
lilac0112_1 0:ea35c18c85fc 567 }
lilac0112_1 2:635947de1583 568 if((data.irPosition==1)&&(vy>0)){
lilac0112_1 0:ea35c18c85fc 569 vy += 25;//前進加速
lilac0112_1 0:ea35c18c85fc 570 }
lilac0112_1 2:635947de1583 571 if((data.irPosition==2)&&(vy>0)){
lilac0112_1 0:ea35c18c85fc 572 vy += 25;//前進加速
lilac0112_1 0:ea35c18c85fc 573 }
lilac0112_1 0:ea35c18c85fc 574
lilac0112_1 0:ea35c18c85fc 575
lilac0112_1 2:635947de1583 576 if((data.irPosition==17)&&(data.ping[L_PING]>data.ping[R_PING])){
lilac0112_1 0:ea35c18c85fc 577 vx *= -1.0;//背後回り込みの左右判断
lilac0112_1 0:ea35c18c85fc 578 }
lilac0112_1 0:ea35c18c85fc 579 /*
lilac0112_1 0:ea35c18c85fc 580 if((cmps_set.InputPID<(REFERENCE-30))||(cmps_set.InputPID>(REFERENCE+30))){
lilac0112_1 0:ea35c18c85fc 581 vx = vx*(0.75);
lilac0112_1 0:ea35c18c85fc 582 vy = vy*(0.75);
lilac0112_1 0:ea35c18c85fc 583 }
lilac0112_1 0:ea35c18c85fc 584 */
lilac0112_1 0:ea35c18c85fc 585 //Lineを踏み始めた方向を調べる
lilac0112_1 2:635947de1583 586 /*LineDir[A_SPOT] = (vx>0)&&((data.lnFlag[A_SPOT]==1)&&(1 ))&&(LineFirst[X_AXIS] == A_SPOT);
lilac0112_1 2:635947de1583 587 LineDir[B_SPOT] = (vx<0)&&((data.lnFlag[B_SPOT]==1)&&(1 ))&&(LineFirst[X_AXIS] == B_SPOT);
lilac0112_1 2:635947de1583 588 LineDir[C_SPOT] = (vy<0)&&((data.lnFlag[C_SPOT]==1)&&(1 ))&&(LineFirst[Y_AXIS] == C_SPOT);
lilac0112_1 2:635947de1583 589 LineDir[AB_SPOT]= (vy>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==1))&&(LineFirst[Y_AXIS] == AB_SPOT);*/
lilac0112_1 0:ea35c18c85fc 590
lilac0112_1 0:ea35c18c85fc 591 LineDir[A_SPOT] = (vx>0)&&((LineSign[A_SPOT]==1)&&(1 ))&&(LineFirst[X_AXIS] == A_SPOT);
lilac0112_1 0:ea35c18c85fc 592 LineDir[B_SPOT] = (vx<0)&&((LineSign[B_SPOT]==1)&&(1 ))&&(LineFirst[X_AXIS] == B_SPOT);
lilac0112_1 0:ea35c18c85fc 593 LineDir[C_SPOT] = (vy<0)&&((LineSign[C_SPOT]==1)&&(1 ))&&(LineFirst[Y_AXIS] == C_SPOT);
lilac0112_1 0:ea35c18c85fc 594 LineDir[AB_SPOT]= (vy>0)&&((LineSign[A_SPOT]==1)&&(LineSign[B_SPOT]==1))&&(LineFirst[Y_AXIS] == AB_SPOT);
lilac0112_1 0:ea35c18c85fc 595
lilac0112_1 0:ea35c18c85fc 596 //Irボールの方向
lilac0112_1 0:ea35c18c85fc 597 //strict
lilac0112_1 0:ea35c18c85fc 598
lilac0112_1 2:635947de1583 599 IrRange[A_SPOT] = ((18<=data.irPosition)&&(data.irPosition<=19))||((8<=data.irPosition)&&(data.irPosition<=10))
lilac0112_1 2:635947de1583 600 ||((6<=data.irPosition)&&(data.irPosition<=7))||(( 1)&&(data.irPosition<=1));
lilac0112_1 2:635947de1583 601 IrRange[B_SPOT] = ((12<=data.irPosition)&&(data.irPosition<=16))||((2<=data.irPosition)&&(data.irPosition<=5));
lilac0112_1 2:635947de1583 602 IrRange[C_SPOT] = ((15<=data.irPosition)&&(data.irPosition<=19))||((4<=data.irPosition)&&(data.irPosition<=7));
lilac0112_1 2:635947de1583 603 IrRange[AB_SPOT] = ((9<=data.irPosition)&&(data.irPosition<=13))||(( 1)&&(data.irPosition<=3));
lilac0112_1 0:ea35c18c85fc 604
lilac0112_1 0:ea35c18c85fc 605 //sweet
lilac0112_1 2:635947de1583 606 /*IrRange[A_SPOT] = ((19<=data.irPosition)&&(data.irPosition<=19))||((8<=data.irPosition)&&(data.irPosition<=9))
lilac0112_1 2:635947de1583 607 ||((6<=data.irPosition)&&(data.irPosition<=7))||(( 1)&&(data.irPosition<=1));
lilac0112_1 2:635947de1583 608 IrRange[B_SPOT] = ((13<=data.irPosition)&&(data.irPosition<=15))||((2<=data.irPosition)&&(data.irPosition<=5));
lilac0112_1 2:635947de1583 609 IrRange[C_SPOT] = ((16<=data.irPosition)&&(data.irPosition<=18))||((4<=data.irPosition)&&(data.irPosition<=7));
lilac0112_1 2:635947de1583 610 IrRange[AB_SPOT] = ((10<=data.irPosition)&&(data.irPosition<=12))||(( 1)&&(data.irPosition<=3));*/
lilac0112_1 0:ea35c18c85fc 611 //none
lilac0112_1 2:635947de1583 612 if(data.irNotice==IR_NONE){
lilac0112_1 0:ea35c18c85fc 613 IrRange[A_SPOT] = 0;
lilac0112_1 0:ea35c18c85fc 614 IrRange[B_SPOT] = 0;
lilac0112_1 0:ea35c18c85fc 615 IrRange[C_SPOT] = 0;
lilac0112_1 0:ea35c18c85fc 616 IrRange[AB_SPOT] = 0;
lilac0112_1 0:ea35c18c85fc 617 }
lilac0112_1 0:ea35c18c85fc 618 //白線を踏み始めた方向とボールの方向が一致．(SelfHold)
lilac0112_1 0:ea35c18c85fc 619 LineBind[A_SPOT] = (IrRange[A_SPOT]==1)&&((LineDir[A_SPOT]==1)||(LineBind[A_SPOT]==1));
lilac0112_1 0:ea35c18c85fc 620 LineBind[B_SPOT] = (IrRange[B_SPOT]==1)&&((LineDir[B_SPOT]==1)||(LineBind[B_SPOT]==1));
lilac0112_1 0:ea35c18c85fc 621 LineBind[C_SPOT] = (IrRange[C_SPOT]==1)&&((LineDir[C_SPOT]==1)||(LineBind[C_SPOT]==1));
lilac0112_1 0:ea35c18c85fc 622 LineBind[AB_SPOT] = (IrRange[AB_SPOT]==1)&&((LineDir[AB_SPOT]==1)||(LineBind[AB_SPOT]==1));
lilac0112_1 0:ea35c18c85fc 623
lilac0112_1 0:ea35c18c85fc 624 LineStop[X_AXIS] = 1;//(LineBind[A_SPOT]==0)*(LineBind[B_SPOT]==0);
lilac0112_1 0:ea35c18c85fc 625 LineStop[Y_AXIS] = 1;//(LineBind[C_SPOT]==0)*(LineBind[AB_SPOT]==0);
lilac0112_1 0:ea35c18c85fc 626
lilac0112_1 0:ea35c18c85fc 627 //白線踏んでる
lilac0112_1 2:635947de1583 628 if(LineRaw>0){
lilac0112_1 0:ea35c18c85fc 629 LineOn[A_SPOT] = (LineSign[A_SPOT]==1) &&(LineFirst[X_AXIS]==A_SPOT);
lilac0112_1 0:ea35c18c85fc 630 LineOn[B_SPOT] = (LineSign[B_SPOT]==1) &&(LineFirst[X_AXIS]==B_SPOT);
lilac0112_1 0:ea35c18c85fc 631 LineOn[C_SPOT] = (LineSign[C_SPOT]==1) &&(LineFirst[Y_AXIS]==C_SPOT);
lilac0112_1 0:ea35c18c85fc 632 LineOn[AB_SPOT] = ((LineSign[A_SPOT]==1)&&(LineSign[B_SPOT]==1))&&(LineFirst[Y_AXIS]==AB_SPOT);
lilac0112_1 0:ea35c18c85fc 633 //外側に向かう力を消す．
lilac0112_1 0:ea35c18c85fc 634 //x
lilac0112_1 0:ea35c18c85fc 635 if(((LineOn[A_SPOT]==1)&&(vx>0))||((LineOn[B_SPOT]==1)&&(vx<0))){
lilac0112_1 0:ea35c18c85fc 636
lilac0112_1 0:ea35c18c85fc 637 if(LinePriority[Y_AXIS]==0){
lilac0112_1 0:ea35c18c85fc 638 LinePriority[X_AXIS]=1;
lilac0112_1 0:ea35c18c85fc 639 LinePriority[Y_AXIS]=0;
lilac0112_1 0:ea35c18c85fc 640 }
lilac0112_1 0:ea35c18c85fc 641 vx=0;
lilac0112_1 0:ea35c18c85fc 642 //yの力を加える．
lilac0112_1 0:ea35c18c85fc 643 if(vy>0){vy += 10;}
lilac0112_1 0:ea35c18c85fc 644 if(vy<0){vy -= 10;}
lilac0112_1 0:ea35c18c85fc 645 }
lilac0112_1 0:ea35c18c85fc 646 //y
lilac0112_1 0:ea35c18c85fc 647 if(((LineOn[C_SPOT]==1)&&(vy<0))||((LineOn[AB_SPOT]==1)&&(vy>0))){
lilac0112_1 0:ea35c18c85fc 648 if(LinePriority[X_AXIS]==0){
lilac0112_1 0:ea35c18c85fc 649 LinePriority[X_AXIS]=0;
lilac0112_1 0:ea35c18c85fc 650 LinePriority[Y_AXIS]=1;
lilac0112_1 0:ea35c18c85fc 651 }
lilac0112_1 0:ea35c18c85fc 652 if((LinePriority[X_AXIS]==1)&&(LineOn[AB_SPOT]==1)){
lilac0112_1 0:ea35c18c85fc 653 LinePriority[X_AXIS]=0;
lilac0112_1 0:ea35c18c85fc 654 LinePriority[Y_AXIS]=1;
lilac0112_1 0:ea35c18c85fc 655 }
lilac0112_1 0:ea35c18c85fc 656 vy=0;
lilac0112_1 0:ea35c18c85fc 657 }
lilac0112_1 0:ea35c18c85fc 658 //内側に向かう力を加える．
lilac0112_1 0:ea35c18c85fc 659 LineReturn[A_SPOT] = (LineOn[A_SPOT]==1);
lilac0112_1 0:ea35c18c85fc 660 /*if((LineReturn[A_SPOT]==1)&&(LineOn[AB_SPOT]==1)){
lilac0112_1 0:ea35c18c85fc 661 if(LineOn[C_SPOT]==0){
lilac0112_1 0:ea35c18c85fc 662 LineReturn[A_SPOT]=0;
lilac0112_1 0:ea35c18c85fc 663 }
lilac0112_1 0:ea35c18c85fc 664 else{
lilac0112_1 0:ea35c18c85fc 665 LineReturn[A_SPOT]=1;
lilac0112_1 0:ea35c18c85fc 666 }
lilac0112_1 0:ea35c18c85fc 667 }*/
lilac0112_1 0:ea35c18c85fc 668
lilac0112_1 0:ea35c18c85fc 669 LineReturn[B_SPOT] = (LineOn[B_SPOT]==1);
lilac0112_1 0:ea35c18c85fc 670 /*if((LineReturn[B_SPOT]==1)&&(LineOn[AB_SPOT]==1)){
lilac0112_1 0:ea35c18c85fc 671 if(LineOn[C_SPOT]==0){
lilac0112_1 0:ea35c18c85fc 672 LineReturn[B_SPOT]=0;
lilac0112_1 0:ea35c18c85fc 673 }
lilac0112_1 0:ea35c18c85fc 674 else{
lilac0112_1 0:ea35c18c85fc 675 LineReturn[B_SPOT]=1;
lilac0112_1 0:ea35c18c85fc 676 }
lilac0112_1 0:ea35c18c85fc 677 }*/
lilac0112_1 0:ea35c18c85fc 678 LineReturn[C_SPOT] = (LineOn[C_SPOT]==1);
lilac0112_1 0:ea35c18c85fc 679 /*if(LineReturn[C_SPOT]==1){
lilac0112_1 0:ea35c18c85fc 680 LineReturn[A_SPOT]=0;
lilac0112_1 0:ea35c18c85fc 681 LineReturn[B_SPOT]=0;
lilac0112_1 0:ea35c18c85fc 682 }*/
lilac0112_1 0:ea35c18c85fc 683 LineReturn[AB_SPOT] = (LineOn[AB_SPOT]==1);
lilac0112_1 0:ea35c18c85fc 684
lilac0112_1 0:ea35c18c85fc 685 LineForce[X_AXIS] = (LINE_RF*2)*(LinePriority[X_AXIS])*((-1)*(LineReturn[A_SPOT]==1) + ( 1)*(LineReturn[B_SPOT]==1)) +
lilac0112_1 2:635947de1583 686 (LINE_RF*2)*(LineReturn[AB_SPOT]==1)*(( 1)*(data.ping[L_PING]<40) + (-1)*(data.ping[R_PING]<40)) +
lilac0112_1 2:635947de1583 687 (LINE_RF*2)*(LineReturn[C_SPOT]==1)*(( 1)*(data.ping[L_PING]<40) + (-1)*(data.ping[R_PING]<40));
lilac0112_1 0:ea35c18c85fc 688 LineForce[Y_AXIS] = (LINE_RF*2)*(LinePriority[Y_AXIS])*(( 1)*(LineReturn[C_SPOT]==1) + (-1)*(LineReturn[AB_SPOT]==1));
lilac0112_1 0:ea35c18c85fc 689 /*
lilac0112_1 0:ea35c18c85fc 690 LineForce[X_AXIS] = (LINE_RF*2)*((-1)*(LineReturn[A_SPOT]==1) + ( 1)*(LineReturn[B_SPOT]==1)) +
lilac0112_1 0:ea35c18c85fc 691 (LINE_RF*2)*(LineReturn[AB_SPOT]==1)*(LineReturn[A_SPOT]==0)*(LineReturn[B_SPOT]==0)
lilac0112_1 2:635947de1583 692 *(( 1)*(data.ping[L_PING]<30) + (-1)*(data.ping[R_PING]<30)) +
lilac0112_1 0:ea35c18c85fc 693 (LINE_RF*2)*(LineReturn[C_SPOT]==1)*(LineReturn[A_SPOT]==0)*(LineReturn[B_SPOT]==0)
lilac0112_1 2:635947de1583 694 *(( 1)*(data.ping[L_PING]<30) + (-1)*(data.ping[R_PING]<30));
lilac0112_1 0:ea35c18c85fc 695 LineForce[Y_AXIS] = (LINE_RF*2)*(( 1)*(LineReturn[C_SPOT]==1) + (-1)*(LineReturn[AB_SPOT]==1));
lilac0112_1 0:ea35c18c85fc 696 */
lilac0112_1 0:ea35c18c85fc 697 Line_timeout[A_SPOT].attach(&LineClear_A, LINE_DELAY);
lilac0112_1 0:ea35c18c85fc 698 Line_timeout[B_SPOT].attach(&LineClear_B, LINE_DELAY);
lilac0112_1 0:ea35c18c85fc 699 Line_timeout[C_SPOT].attach(&LineClear_C, LINE_DELAY);
lilac0112_1 0:ea35c18c85fc 700 }
lilac0112_1 0:ea35c18c85fc 701 else{
lilac0112_1 0:ea35c18c85fc 702 LineForce[X_AXIS] = 0;
lilac0112_1 0:ea35c18c85fc 703 LineForce[Y_AXIS] = 0;
lilac0112_1 0:ea35c18c85fc 704
lilac0112_1 0:ea35c18c85fc 705 LinePriority[X_AXIS]=0;
lilac0112_1 0:ea35c18c85fc 706 LinePriority[Y_AXIS]=0;
lilac0112_1 0:ea35c18c85fc 707 }
lilac0112_1 0:ea35c18c85fc 708
lilac0112_1 0:ea35c18c85fc 709 vx = vx*LineStop[X_AXIS] + LineForce[X_AXIS];
lilac0112_1 0:ea35c18c85fc 710 vy = vy*LineStop[Y_AXIS] + LineForce[Y_AXIS];
lilac0112_1 0:ea35c18c85fc 711 vs = cmps_set.OutputPID;
lilac0112_1 0:ea35c18c85fc 712 move(
lilac0112_1 0:ea35c18c85fc 713 vx,
lilac0112_1 0:ea35c18c85fc 714 vy,
lilac0112_1 0:ea35c18c85fc 715 vs
lilac0112_1 0:ea35c18c85fc 716 );
lilac0112_1 0:ea35c18c85fc 717 /*move(
lilac0112_1 0:ea35c18c85fc 718 10,
lilac0112_1 0:ea35c18c85fc 719 0,
lilac0112_1 0:ea35c18c85fc 720 0
lilac0112_1 0:ea35c18c85fc 721 );*/
lilac0112_1 0:ea35c18c85fc 722 if(sys.MotorFlag==1){
lilac0112_1 0:ea35c18c85fc 723 tx_motor();
lilac0112_1 0:ea35c18c85fc 724 sys.MotorFlag=0;
lilac0112_1 0:ea35c18c85fc 725 }
lilac0112_1 0:ea35c18c85fc 726 if(sys.stopflag==1){
lilac0112_1 0:ea35c18c85fc 727 //停止処理
lilac0112_1 0:ea35c18c85fc 728 }
lilac0112_1 0:ea35c18c85fc 729 return;
lilac0112_1 0:ea35c18c85fc 730 }
lilac0112_1 0:ea35c18c85fc 731 void modeAttack4(void){
lilac0112_1 10:6df631c39f9b 732 double ir_x_dir, ir_y_dir;
lilac0112_1 10:6df631c39f9b 733 double ir_x_turn;
lilac0112_1 10:6df631c39f9b 734 double ir_y_turn;
lilac0112_1 10:6df631c39f9b 735 uint8_t ir_pow;
lilac0112_1 10:6df631c39f9b 736 int vx,vy,vs;
lilac0112_1 10:6df631c39f9b 737 if(sys.KickOffFlag==1){
lilac0112_1 10:6df631c39f9b 738
lilac0112_1 10:6df631c39f9b 739 sys.IrBlind=0;
lilac0112_1 10:6df631c39f9b 740 sys.LineBlind=0;
lilac0112_1 10:6df631c39f9b 741 sys.PingBlind=0;
lilac0112_1 10:6df631c39f9b 742
lilac0112_1 10:6df631c39f9b 743 sys.KickOffFlag=0;
lilac0112_1 0:ea35c18c85fc 744 }
lilac0112_1 10:6df631c39f9b 745 //data
lilac0112_1 10:6df631c39f9b 746 if(sys.IrFlag==1){ReadIr();sys.IrFlag=0;}
lilac0112_1 10:6df631c39f9b 747 if(sys.PidFlag==1){PidUpdate();sys.PidFlag=0;}
lilac0112_1 10:6df631c39f9b 748 if(sys.UswFlag==1){ReadPing();sys.UswFlag=0;}
lilac0112_1 10:6df631c39f9b 749 if(sys.UswFlag2==1){ReadPing2();sys.UswFlag2=0;}
lilac0112_1 10:6df631c39f9b 750 //if(sys.KickFlag==1){DriveSolenoid();}
lilac0112_1 10:6df631c39f9b 751
lilac0112_1 10:6df631c39f9b 752 ir_x_dir = ir_move_val[0][data.irNotice][data.irPosition][IR_X_DIR];
lilac0112_1 10:6df631c39f9b 753 ir_y_dir = ir_move_val[0][data.irNotice][data.irPosition][IR_Y_DIR];
lilac0112_1 10:6df631c39f9b 754 ir_x_turn = ir_move_val[0][data.irNotice][data.irPosition][IR_X_TURN];
lilac0112_1 10:6df631c39f9b 755 ir_y_turn = ir_move_val[0][data.irNotice][data.irPosition][IR_Y_TURN];
lilac0112_1 10:6df631c39f9b 756 if(data.irNotice==IR_CLOSER){
lilac0112_1 10:6df631c39f9b 757 ir_pow = sys.s_pow;
lilac0112_1 10:6df631c39f9b 758 }
lilac0112_1 10:6df631c39f9b 759 else if(data.irNotice==IR_CLOSE){
lilac0112_1 10:6df631c39f9b 760 ir_pow = sys.m_pow;
lilac0112_1 10:6df631c39f9b 761 }
lilac0112_1 10:6df631c39f9b 762 else if(data.irNotice==IR_FAR){
lilac0112_1 10:6df631c39f9b 763 ir_pow = sys.l_pow;
lilac0112_1 10:6df631c39f9b 764 }
lilac0112_1 10:6df631c39f9b 765 else{//data.irNotice==IR_NONE
lilac0112_1 10:6df631c39f9b 766 ir_pow = 0;
lilac0112_1 10:6df631c39f9b 767 }
lilac0112_1 10:6df631c39f9b 768 if(sys.IrBlind==1) data.irNotice=IR_NONE;
lilac0112_1 10:6df631c39f9b 769
lilac0112_1 10:6df631c39f9b 770 //if((LineHold==7)
lilac0112_1 10:6df631c39f9b 771
lilac0112_1 10:6df631c39f9b 772
lilac0112_1 10:6df631c39f9b 773 vx = ir_pow*(ir_x_dir + ir_x_turn);
lilac0112_1 10:6df631c39f9b 774 vy = ir_pow*(ir_y_dir + ir_y_turn);
lilac0112_1 10:6df631c39f9b 775 vs = cmps_set.OutputPID;
lilac0112_1 10:6df631c39f9b 776 move(
lilac0112_1 10:6df631c39f9b 777 vx,
lilac0112_1 10:6df631c39f9b 778 vy,
lilac0112_1 10:6df631c39f9b 779 vs
lilac0112_1 10:6df631c39f9b 780 );
lilac0112_1 10:6df631c39f9b 781 if(sys.MotorFlag==1){tx_motor();sys.MotorFlag=0;}
lilac0112_1 0:ea35c18c85fc 782 if(sys.stopflag==1){
lilac0112_1 10:6df631c39f9b 783
lilac0112_1 0:ea35c18c85fc 784 //停止処理
lilac0112_1 0:ea35c18c85fc 785 }
lilac0112_1 0:ea35c18c85fc 786 return;
lilac0112_1 0:ea35c18c85fc 787 }
lilac0112_1 0:ea35c18c85fc 788 void modeAttack5(void){
lilac0112_1 0:ea35c18c85fc 789 if(sys.IrFlag==1){
lilac0112_1 0:ea35c18c85fc 790 ReadIr();
lilac0112_1 0:ea35c18c85fc 791 sys.IrFlag=0;
lilac0112_1 0:ea35c18c85fc 792 }
lilac0112_1 0:ea35c18c85fc 793 if(sys.PidFlag==1){
lilac0112_1 0:ea35c18c85fc 794 PidUpdate();
lilac0112_1 0:ea35c18c85fc 795 sys.PidFlag=0;
lilac0112_1 0:ea35c18c85fc 796 }
lilac0112_1 10:6df631c39f9b 797 move(0,0,cmps_set.OutputPID);
lilac0112_1 0:ea35c18c85fc 798 if(sys.MotorFlag==1){
lilac0112_1 5:5ff3a7d5d8c2 799 LED[0] = 1;
lilac0112_1 5:5ff3a7d5d8c2 800 LED[1] = 0;
lilac0112_1 0:ea35c18c85fc 801 tx_motor();
lilac0112_1 0:ea35c18c85fc 802 sys.MotorFlag=0;
lilac0112_1 0:ea35c18c85fc 803 }
lilac0112_1 5:5ff3a7d5d8c2 804 else{
lilac0112_1 5:5ff3a7d5d8c2 805 LED[0] = 0;
lilac0112_1 5:5ff3a7d5d8c2 806 LED[1] = 1;
lilac0112_1 5:5ff3a7d5d8c2 807 }
lilac0112_1 0:ea35c18c85fc 808 if(sys.stopflag==1){
lilac0112_1 0:ea35c18c85fc 809 //停止処理
lilac0112_1 0:ea35c18c85fc 810 }
lilac0112_1 0:ea35c18c85fc 811 return;
lilac0112_1 0:ea35c18c85fc 812 }