CatPot 2015-2016 / Mbed 2 deprecated CatPot_Main_T_2v00

Main Program

Dependencies:   mbed AQM1602 HMC6352 PID

main_processing/strategy/strategy.cpp@41:3cdcdec9c885, 2016-02-04 (annotated)

Committer:
lilac0112_1
Date:
Thu Feb 04 11:43:53 2016 +0000
Revision:
41:3cdcdec9c885
Parent:
40:f68474b1d5e7
Child:
42:02aaa806d929
Good Line?

Who changed what in which revision?

UserRevisionLine numberNew contents of line
lilac0112_1 19:342da3a5a474 1 #include "mbed.h"
lilac0112_1 19:342da3a5a474 2 #include "extern.h"
lilac0112_1 19:342da3a5a474 3
lilac0112_1 21:d69a8f3c76e1 4 //Atk
lilac0112_1 21:d69a8f3c76e1 5 void modeAttack0(void){
lilac0112_1 28:8ac6c3c1e643 6 double ir_x, ir_y;
lilac0112_1 27:769cb5a7ea37 7 int vx,vy,vs;
lilac0112_1 28:8ac6c3c1e643 8 uint8_t LineDir[4];
lilac0112_1 27:769cb5a7ea37 9 uint8_t LineStop[2];
lilac0112_1 31:0b0f64831771 10 //uint8_t IrRange[4];
lilac0112_1 31:0b0f64831771 11 //uint8_t LineBind[4];
lilac0112_1 28:8ac6c3c1e643 12 if(data.IrFlag==1){
lilac0112_1 28:8ac6c3c1e643 13 ReadIr();
lilac0112_1 28:8ac6c3c1e643 14 data.IrFlag=0;
lilac0112_1 28:8ac6c3c1e643 15 }
lilac0112_1 28:8ac6c3c1e643 16 if(data.PidFlag==1){
lilac0112_1 28:8ac6c3c1e643 17 Line_ticker.detach();
lilac0112_1 28:8ac6c3c1e643 18 PidUpdate();
lilac0112_1 28:8ac6c3c1e643 19 Line_ticker.attach(&ReadLine, 0.005);
lilac0112_1 28:8ac6c3c1e643 20 data.PidFlag=0;
lilac0112_1 28:8ac6c3c1e643 21 }
lilac0112_1 28:8ac6c3c1e643 22 ir_x = ir_move_val[data.irNotice][data.irPosition][IR_X];
lilac0112_1 28:8ac6c3c1e643 23 ir_y = ir_move_val[data.irNotice][data.irPosition][IR_Y];
lilac0112_1 28:8ac6c3c1e643 24 if(data.irPosition<8){
lilac0112_1 34:69bdf29a4442 25 ir_x *= (double)(data.l_pow);
lilac0112_1 34:69bdf29a4442 26 ir_y *= (double)(data.l_pow);
lilac0112_1 28:8ac6c3c1e643 27 }
lilac0112_1 28:8ac6c3c1e643 28 else{
lilac0112_1 34:69bdf29a4442 29 ir_x *= (double)(data.s_pow);
lilac0112_1 34:69bdf29a4442 30 ir_y *= (double)(data.s_pow);
lilac0112_1 28:8ac6c3c1e643 31 }
lilac0112_1 29:b413b0bb07a1 32 //Lineを考慮していないIrのみの値
lilac0112_1 29:b413b0bb07a1 33 vx = ir_x;
lilac0112_1 29:b413b0bb07a1 34 vy = ir_y;
lilac0112_1 29:b413b0bb07a1 35 //Line検出方向を調べる
lilac0112_1 28:8ac6c3c1e643 36 LineDir[A_SPOT] = (!((vx>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==0))));
lilac0112_1 28:8ac6c3c1e643 37 LineDir[B_SPOT] = (!((vx<0)&&((data.lnFlag[A_SPOT]==0)&&(data.lnFlag[B_SPOT]==1))));
lilac0112_1 28:8ac6c3c1e643 38 LineDir[C_SPOT] = (!((vy<0)&&((data.lnFlag[C_SPOT]==1)&&(1))));
lilac0112_1 28:8ac6c3c1e643 39 LineDir[AB_SPOT] = (!((vy>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==1))));
lilac0112_1 28:8ac6c3c1e643 40
lilac0112_1 28:8ac6c3c1e643 41 LineStop[X_AXIS] = LineDir[A_SPOT]*LineDir[B_SPOT];
lilac0112_1 28:8ac6c3c1e643 42 LineStop[Y_AXIS] = LineDir[C_SPOT]*LineDir[AB_SPOT];
lilac0112_1 28:8ac6c3c1e643 43
lilac0112_1 31:0b0f64831771 44 //Ir
lilac0112_1 29:b413b0bb07a1 45 /*
lilac0112_1 29:b413b0bb07a1 46 //ForLineAll
lilac0112_1 29:b413b0bb07a1 47 data.motorlog[X_AXIS] *= LineStop[X_AXIS];
lilac0112_1 29:b413b0bb07a1 48 data.motorlog[Y_AXIS] *= LineStop[Y_AXIS];
lilac0112_1 29:b413b0bb07a1 49 data.motorlog[X_AXIS] += vx;
lilac0112_1 29:b413b0bb07a1 50 data.motorlog[Y_AXIS] += vy;
lilac0112_1 29:b413b0bb07a1 51 */
lilac0112_1 28:8ac6c3c1e643 52 move(
lilac0112_1 29:b413b0bb07a1 53 vx*LineStop[X_AXIS] + (LINE_RF)*(vy!=0)*((-1)*(LineDir[A_SPOT]==0) + (LineDir[B_SPOT]==0)),
lilac0112_1 29:b413b0bb07a1 54 vy*LineStop[Y_AXIS] + (LINE_RF)*(vx!=0)*((LineDir[C_SPOT]==0) + (-1)*(LineDir[AB_SPOT]==0)),
lilac0112_1 28:8ac6c3c1e643 55 vs
lilac0112_1 28:8ac6c3c1e643 56 );
lilac0112_1 28:8ac6c3c1e643 57 if(data.MotorFlag==1){
lilac0112_1 28:8ac6c3c1e643 58 tx_motor();
lilac0112_1 28:8ac6c3c1e643 59 data.MotorFlag=0;
lilac0112_1 28:8ac6c3c1e643 60 }
lilac0112_1 28:8ac6c3c1e643 61 return;
lilac0112_1 28:8ac6c3c1e643 62 }
lilac0112_1 35:a0421268ee95 63
lilac0112_1 35:a0421268ee95 64
lilac0112_1 35:a0421268ee95 65 uint8_t LineReverseFlag;
lilac0112_1 35:a0421268ee95 66 void LineReverse(void){
lilac0112_1 35:a0421268ee95 67 LineReverseFlag=0;
lilac0112_1 35:a0421268ee95 68 }
lilac0112_1 40:f68474b1d5e7 69 uint8_t LineSign[3];
lilac0112_1 40:f68474b1d5e7 70 uint8_t LineFirst[2];
lilac0112_1 41:3cdcdec9c885 71 void LineClear_A(void){LineSign[A_SPOT]=0;data.lnFlag[A_SPOT]=0;}
lilac0112_1 41:3cdcdec9c885 72 void LineClear_B(void){LineSign[B_SPOT]=0;data.lnFlag[B_SPOT]=0;}
lilac0112_1 41:3cdcdec9c885 73 void LineClear_C(void){LineSign[C_SPOT]=0;data.lnFlag[C_SPOT]=0;}
lilac0112_1 40:f68474b1d5e7 74 void LineCall_A(void){
lilac0112_1 41:3cdcdec9c885 75 //
lilac0112_1 40:f68474b1d5e7 76 LineSign[A_SPOT] = 1;
lilac0112_1 40:f68474b1d5e7 77 if(LineSign[B_SPOT]==0){
lilac0112_1 40:f68474b1d5e7 78 LineFirst[X_AXIS] = A_SPOT;
lilac0112_1 40:f68474b1d5e7 79 }
lilac0112_1 40:f68474b1d5e7 80 if((LineSign[A_SPOT]==1)&&(LineSign[B_SPOT]==1)){
lilac0112_1 40:f68474b1d5e7 81 if(LineSign[C_SPOT]==0){
lilac0112_1 40:f68474b1d5e7 82 LineFirst[Y_AXIS] = AB_SPOT;
lilac0112_1 40:f68474b1d5e7 83 }
lilac0112_1 40:f68474b1d5e7 84 }
lilac0112_1 41:3cdcdec9c885 85 //
lilac0112_1 41:3cdcdec9c885 86 if((Line[A_SPOT].read()==1)||(1)) data.lnFlag[A_SPOT]=1;
lilac0112_1 41:3cdcdec9c885 87 //
lilac0112_1 40:f68474b1d5e7 88 Line_timeout[A_SPOT].attach(&LineClear_A, 0.5);
lilac0112_1 40:f68474b1d5e7 89 }
lilac0112_1 40:f68474b1d5e7 90 void LineCall_B(void){
lilac0112_1 41:3cdcdec9c885 91 //
lilac0112_1 40:f68474b1d5e7 92 LineSign[B_SPOT] = 1;
lilac0112_1 40:f68474b1d5e7 93 if(LineSign[A_SPOT]==0){
lilac0112_1 40:f68474b1d5e7 94 LineFirst[X_AXIS] = B_SPOT;
lilac0112_1 40:f68474b1d5e7 95 }
lilac0112_1 40:f68474b1d5e7 96 if((LineSign[A_SPOT]==1)&&(LineSign[B_SPOT]==1)){
lilac0112_1 40:f68474b1d5e7 97 if(LineSign[C_SPOT]==0){
lilac0112_1 40:f68474b1d5e7 98 LineFirst[Y_AXIS] = AB_SPOT;
lilac0112_1 40:f68474b1d5e7 99 }
lilac0112_1 40:f68474b1d5e7 100 }
lilac0112_1 41:3cdcdec9c885 101 //
lilac0112_1 41:3cdcdec9c885 102 if((Line[B_SPOT].read()==1)||(1)) data.lnFlag[B_SPOT]=1;
lilac0112_1 41:3cdcdec9c885 103 //
lilac0112_1 40:f68474b1d5e7 104 Line_timeout[B_SPOT].attach(&LineClear_B, 0.5);
lilac0112_1 40:f68474b1d5e7 105 }
lilac0112_1 40:f68474b1d5e7 106 void LineCall_C(void){
lilac0112_1 41:3cdcdec9c885 107 //
lilac0112_1 40:f68474b1d5e7 108 LineSign[C_SPOT] = 1;
lilac0112_1 40:f68474b1d5e7 109 if(!((LineSign[A_SPOT]==1)&&(LineSign[B_SPOT]==1))){
lilac0112_1 40:f68474b1d5e7 110 LineFirst[Y_AXIS] = C_SPOT;
lilac0112_1 40:f68474b1d5e7 111 }
lilac0112_1 41:3cdcdec9c885 112 //
lilac0112_1 41:3cdcdec9c885 113 if((Line[C_SPOT].read()==1)||(1)) data.lnFlag[C_SPOT]=1;
lilac0112_1 41:3cdcdec9c885 114 //
lilac0112_1 40:f68474b1d5e7 115 Line_timeout[C_SPOT].attach(&LineClear_C, 0.5);
lilac0112_1 40:f68474b1d5e7 116 }
lilac0112_1 28:8ac6c3c1e643 117 void modeAttack1(void){
lilac0112_1 31:0b0f64831771 118 double ir_x, ir_y;
lilac0112_1 31:0b0f64831771 119 int vx,vy,vs;
lilac0112_1 31:0b0f64831771 120 uint8_t LineDir[4];
lilac0112_1 31:0b0f64831771 121 uint8_t LineStop[2];
lilac0112_1 34:69bdf29a4442 122 uint8_t IrRange[4];
lilac0112_1 35:a0421268ee95 123 uint8_t LinePulse[4];
lilac0112_1 34:69bdf29a4442 124 uint8_t static LineBind[4];
lilac0112_1 34:69bdf29a4442 125 if(data.KickOffFlag==1){
lilac0112_1 34:69bdf29a4442 126 LineBind[0]=0;
lilac0112_1 34:69bdf29a4442 127 LineBind[1]=0;
lilac0112_1 34:69bdf29a4442 128 LineBind[2]=0;
lilac0112_1 34:69bdf29a4442 129 LineBind[3]=0;
lilac0112_1 35:a0421268ee95 130 LineReverseFlag=0;
lilac0112_1 40:f68474b1d5e7 131
lilac0112_1 40:f68474b1d5e7 132 LineSign[A_SPOT]=0;
lilac0112_1 40:f68474b1d5e7 133 LineSign[B_SPOT]=0;
lilac0112_1 40:f68474b1d5e7 134 LineSign[C_SPOT]=0;
lilac0112_1 40:f68474b1d5e7 135
lilac0112_1 41:3cdcdec9c885 136 data.lnFlag[A_SPOT]=0;
lilac0112_1 41:3cdcdec9c885 137 data.lnFlag[B_SPOT]=0;
lilac0112_1 41:3cdcdec9c885 138 data.lnFlag[C_SPOT]=0;
lilac0112_1 41:3cdcdec9c885 139
lilac0112_1 34:69bdf29a4442 140 data.KickOffFlag=0;
lilac0112_1 34:69bdf29a4442 141 }
lilac0112_1 31:0b0f64831771 142 if(data.IrFlag==1){
lilac0112_1 31:0b0f64831771 143 ReadIr();
lilac0112_1 31:0b0f64831771 144 data.IrFlag=0;
lilac0112_1 31:0b0f64831771 145 }
lilac0112_1 31:0b0f64831771 146 if(data.PidFlag==1){
lilac0112_1 41:3cdcdec9c885 147 //Line_ticker.detach();
lilac0112_1 31:0b0f64831771 148 PidUpdate();
lilac0112_1 41:3cdcdec9c885 149 //Line_ticker.attach(&ReadLine, 0.005);
lilac0112_1 31:0b0f64831771 150 data.PidFlag=0;
lilac0112_1 31:0b0f64831771 151 }
lilac0112_1 31:0b0f64831771 152 ir_x = ir_move_val[data.irNotice][data.irPosition][IR_X];
lilac0112_1 31:0b0f64831771 153 ir_y = ir_move_val[data.irNotice][data.irPosition][IR_Y];
lilac0112_1 31:0b0f64831771 154 if(data.irPosition<8){
lilac0112_1 31:0b0f64831771 155 ir_x *= data.l_pow;
lilac0112_1 31:0b0f64831771 156 ir_y *= data.l_pow;
lilac0112_1 31:0b0f64831771 157 }
lilac0112_1 31:0b0f64831771 158 else{
lilac0112_1 31:0b0f64831771 159 ir_x *= data.s_pow;
lilac0112_1 31:0b0f64831771 160 ir_y *= data.s_pow;
lilac0112_1 31:0b0f64831771 161 }
lilac0112_1 31:0b0f64831771 162
lilac0112_1 31:0b0f64831771 163 //Lineを考慮していないIrのみの値
lilac0112_1 31:0b0f64831771 164 vx = ir_x;
lilac0112_1 31:0b0f64831771 165 vy = ir_y;
lilac0112_1 31:0b0f64831771 166 //Line検出方向を調べる
lilac0112_1 35:a0421268ee95 167 LineDir[A_SPOT] = (!((vx>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==0))&&(1)));
lilac0112_1 35:a0421268ee95 168 LineDir[B_SPOT] = (!((vx<0)&&((data.lnFlag[A_SPOT]==0)&&(data.lnFlag[B_SPOT]==1))&&(1)));
lilac0112_1 35:a0421268ee95 169 LineDir[C_SPOT] = (!((vy<0)&&((data.lnFlag[C_SPOT]==1)&&(1 ))&&(1)));
lilac0112_1 35:a0421268ee95 170 LineDir[AB_SPOT] = (!((vy>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==1))&&(1)));
lilac0112_1 31:0b0f64831771 171
lilac0112_1 31:0b0f64831771 172 LineStop[X_AXIS] = LineDir[A_SPOT]*LineDir[B_SPOT];
lilac0112_1 31:0b0f64831771 173 LineStop[Y_AXIS] = LineDir[C_SPOT]*LineDir[AB_SPOT];
lilac0112_1 31:0b0f64831771 174
lilac0112_1 34:69bdf29a4442 175 //Ir
lilac0112_1 36:50092d488f78 176 //strict
lilac0112_1 36:50092d488f78 177 /*
lilac0112_1 34:69bdf29a4442 178 IrRange[A_SPOT] = ((18<=data.irPosition)&&(data.irPosition<=19))||((8<=data.irPosition)&&(data.irPosition<=10))
lilac0112_1 34:69bdf29a4442 179 ||((6<=data.irPosition)&&(data.irPosition<=7))||(( 1)&&(data.irPosition<=1));
lilac0112_1 34:69bdf29a4442 180 IrRange[B_SPOT] = ((12<=data.irPosition)&&(data.irPosition<=16))||((2<=data.irPosition)&&(data.irPosition<=5));
lilac0112_1 34:69bdf29a4442 181 IrRange[C_SPOT] = ((15<=data.irPosition)&&(data.irPosition<=19))||((4<=data.irPosition)&&(data.irPosition<=7));
lilac0112_1 34:69bdf29a4442 182 IrRange[AB_SPOT] = ((9<=data.irPosition)&&(data.irPosition<=13))||(( 1)&&(data.irPosition<=3));
lilac0112_1 36:50092d488f78 183 */
lilac0112_1 36:50092d488f78 184 //sweet
lilac0112_1 36:50092d488f78 185 IrRange[A_SPOT] = ((19<=data.irPosition)&&(data.irPosition<=19))||((8<=data.irPosition)&&(data.irPosition<=9))
lilac0112_1 36:50092d488f78 186 ||((6<=data.irPosition)&&(data.irPosition<=7))||(( 1)&&(data.irPosition<=1));
lilac0112_1 36:50092d488f78 187 IrRange[B_SPOT] = ((13<=data.irPosition)&&(data.irPosition<=15))||((2<=data.irPosition)&&(data.irPosition<=5));
lilac0112_1 36:50092d488f78 188 IrRange[C_SPOT] = ((16<=data.irPosition)&&(data.irPosition<=18))||((4<=data.irPosition)&&(data.irPosition<=7));
lilac0112_1 36:50092d488f78 189 IrRange[AB_SPOT] = ((10<=data.irPosition)&&(data.irPosition<=12))||(( 1)&&(data.irPosition<=3));
lilac0112_1 34:69bdf29a4442 190
lilac0112_1 35:a0421268ee95 191
lilac0112_1 35:a0421268ee95 192 LinePulse[A_SPOT] = ((IrRange[A_SPOT]==1)&&(LineDir[A_SPOT]==0));
lilac0112_1 35:a0421268ee95 193 LinePulse[B_SPOT] = ((IrRange[B_SPOT]==1)&&(LineDir[B_SPOT]==0));
lilac0112_1 35:a0421268ee95 194 LinePulse[C_SPOT] = ((IrRange[C_SPOT]==1)&&(LineDir[C_SPOT]==0));
lilac0112_1 35:a0421268ee95 195 LinePulse[AB_SPOT] = ((IrRange[AB_SPOT]==1)&&(LineDir[AB_SPOT]==0));
lilac0112_1 35:a0421268ee95 196
lilac0112_1 35:a0421268ee95 197 LineBind[A_SPOT] = ((LinePulse[A_SPOT])||((IrRange[A_SPOT]==1)&&(LineBind[A_SPOT]==1)))&&(LineBind[B_SPOT]==0);
lilac0112_1 35:a0421268ee95 198 LineBind[B_SPOT] = ((LinePulse[B_SPOT])||((IrRange[B_SPOT]==1)&&(LineBind[B_SPOT]==1)))&&(LineBind[A_SPOT]==0);
lilac0112_1 35:a0421268ee95 199 LineBind[C_SPOT] = ((LinePulse[C_SPOT])||((IrRange[C_SPOT]==1)&&(LineBind[C_SPOT]==1)))&&(LineBind[AB_SPOT]==0);
lilac0112_1 35:a0421268ee95 200 LineBind[AB_SPOT] = ((LinePulse[AB_SPOT])||((IrRange[AB_SPOT]==1)&&(LineBind[AB_SPOT]==1)))&&(LineBind[C_SPOT]==0);
lilac0112_1 35:a0421268ee95 201 /*
lilac0112_1 34:69bdf29a4442 202 LineBind[A_SPOT] = ((IrRange[A_SPOT]==1)&&((LineDir[A_SPOT]==0)||(LineBind[A_SPOT]==1)));
lilac0112_1 34:69bdf29a4442 203 LineBind[B_SPOT] = ((IrRange[B_SPOT]==1)&&((LineDir[B_SPOT]==0)||(LineBind[B_SPOT]==1)));
lilac0112_1 34:69bdf29a4442 204 LineBind[C_SPOT] = ((IrRange[C_SPOT]==1)&&((LineDir[C_SPOT]==0)||(LineBind[C_SPOT]==1)));
lilac0112_1 34:69bdf29a4442 205 LineBind[AB_SPOT] = ((IrRange[AB_SPOT]==1)&&((LineDir[AB_SPOT]==0)||(LineBind[AB_SPOT]==1)));
lilac0112_1 35:a0421268ee95 206 */
lilac0112_1 37:ae5661306900 207 vx = vx*LineStop[X_AXIS] + (LINE_RF)*(vy!=0)*((-1)*(LineDir[A_SPOT]==0) + (LineDir[B_SPOT]==0));
lilac0112_1 37:ae5661306900 208 vy = vy*LineStop[Y_AXIS] + (LINE_RF)*(vx!=0)*((LineDir[C_SPOT]==0) + (-1)*(LineDir[AB_SPOT]==0));
lilac0112_1 37:ae5661306900 209 vs = data.OutputPID;
lilac0112_1 34:69bdf29a4442 210 if((LineBind[A_SPOT]==1)||(LineBind[B_SPOT]==1)||(LineBind[C_SPOT]==1)||(LineBind[AB_SPOT]==1)){
lilac0112_1 37:ae5661306900 211 if(RawLineSignal>0){
lilac0112_1 37:ae5661306900 212 vx=(LINE_RF*2)*((-1)*IrRange[A_SPOT] + IrRange[B_SPOT]);
lilac0112_1 37:ae5661306900 213 vy=(LINE_RF*2)*( IrRange[C_SPOT] + (-1)*IrRange[AB_SPOT]);
lilac0112_1 40:f68474b1d5e7 214 /*vx=(LINE_RF*2)*((-1)*(LineFirst[X_AXIS] == A_SPOT) + (LineFirst[X_AXIS] == B_SPOT));
lilac0112_1 40:f68474b1d5e7 215 vy=(LINE_RF*2)*( (LineFirst[Y_AXIS] == C_SPOT) + (-1)*(LineFirst[Y_AXIS] == AB_SPOT));
lilac0112_1 40:f68474b1d5e7 216 Line_timeout[A_SPOT].attach(&LineClear_A, 0.5);
lilac0112_1 40:f68474b1d5e7 217 Line_timeout[B_SPOT].attach(&LineClear_B, 0.5);
lilac0112_1 40:f68474b1d5e7 218 Line_timeout[C_SPOT].attach(&LineClear_C, 0.5);*/
lilac0112_1 35:a0421268ee95 219 }
lilac0112_1 35:a0421268ee95 220 else{
lilac0112_1 35:a0421268ee95 221 vx=0;
lilac0112_1 35:a0421268ee95 222 vy=0;
lilac0112_1 35:a0421268ee95 223 }
lilac0112_1 31:0b0f64831771 224 }
lilac0112_1 41:3cdcdec9c885 225 if(RawLineSignal>0){
lilac0112_1 41:3cdcdec9c885 226 Line_timeout[A_SPOT].attach(&LineClear_A, 0.5);
lilac0112_1 41:3cdcdec9c885 227 Line_timeout[B_SPOT].attach(&LineClear_B, 0.5);
lilac0112_1 41:3cdcdec9c885 228 Line_timeout[C_SPOT].attach(&LineClear_C, 0.5);
lilac0112_1 41:3cdcdec9c885 229 }
lilac0112_1 31:0b0f64831771 230 move(
lilac0112_1 37:ae5661306900 231 vx,
lilac0112_1 37:ae5661306900 232 vy,
lilac0112_1 31:0b0f64831771 233 vs
lilac0112_1 31:0b0f64831771 234 );
lilac0112_1 31:0b0f64831771 235 if(data.MotorFlag==1){
lilac0112_1 31:0b0f64831771 236 tx_motor();
lilac0112_1 31:0b0f64831771 237 data.MotorFlag=0;
lilac0112_1 31:0b0f64831771 238 }
lilac0112_1 28:8ac6c3c1e643 239 return;
lilac0112_1 28:8ac6c3c1e643 240 }
lilac0112_1 28:8ac6c3c1e643 241 void modeAttack2(void){
lilac0112_1 34:69bdf29a4442 242 if(data.IrFlag==1){
lilac0112_1 34:69bdf29a4442 243 ReadIr();
lilac0112_1 34:69bdf29a4442 244 data.IrFlag=0;
lilac0112_1 34:69bdf29a4442 245 }
lilac0112_1 34:69bdf29a4442 246 if(data.PidFlag==1){
lilac0112_1 34:69bdf29a4442 247 PidUpdate();
lilac0112_1 34:69bdf29a4442 248 data.PidFlag=0;
lilac0112_1 34:69bdf29a4442 249 }
lilac0112_1 34:69bdf29a4442 250 move(0,0,10);
lilac0112_1 34:69bdf29a4442 251 if(data.MotorFlag==1){
lilac0112_1 34:69bdf29a4442 252 tx_motor();
lilac0112_1 34:69bdf29a4442 253 data.MotorFlag=0;
lilac0112_1 34:69bdf29a4442 254 }
lilac0112_1 34:69bdf29a4442 255
lilac0112_1 28:8ac6c3c1e643 256 return;
lilac0112_1 28:8ac6c3c1e643 257 }
lilac0112_1 28:8ac6c3c1e643 258 void modeAttack3(void){
lilac0112_1 34:69bdf29a4442 259 if(data.KickFlag==1){
lilac0112_1 34:69bdf29a4442 260 DriveSolenoid();
lilac0112_1 34:69bdf29a4442 261 }
lilac0112_1 28:8ac6c3c1e643 262 return;
lilac0112_1 28:8ac6c3c1e643 263 }
lilac0112_1 28:8ac6c3c1e643 264 void modeAttack4(void){
lilac0112_1 28:8ac6c3c1e643 265 return;
lilac0112_1 28:8ac6c3c1e643 266 }
lilac0112_1 28:8ac6c3c1e643 267 void modeAttack5(void){
lilac0112_1 28:8ac6c3c1e643 268 return;
lilac0112_1 28:8ac6c3c1e643 269 }
lilac0112_1 28:8ac6c3c1e643 270 //Debug
lilac0112_1 28:8ac6c3c1e643 271 void modeDebug0(void){
lilac0112_1 28:8ac6c3c1e643 272 return;
lilac0112_1 28:8ac6c3c1e643 273 }
lilac0112_1 28:8ac6c3c1e643 274 void modeDebug1(void){//ChaseOnly NonLine
lilac0112_1 28:8ac6c3c1e643 275 int vx,vy,vs;
lilac0112_1 24:9fb74ea3c25d 276 if(data.IrFlag==1){
lilac0112_1 24:9fb74ea3c25d 277 ReadIr();
lilac0112_1 24:9fb74ea3c25d 278 data.IrFlag=0;
lilac0112_1 24:9fb74ea3c25d 279 }
lilac0112_1 27:769cb5a7ea37 280 if(data.PidFlag==1){
lilac0112_1 27:769cb5a7ea37 281 PidUpdate();
lilac0112_1 27:769cb5a7ea37 282 data.PidFlag=0;
lilac0112_1 27:769cb5a7ea37 283 }
lilac0112_1 28:8ac6c3c1e643 284
lilac0112_1 28:8ac6c3c1e643 285 vx = ir_move_val[data.irNotice][data.irPosition][IR_X];
lilac0112_1 28:8ac6c3c1e643 286 vy = ir_move_val[data.irNotice][data.irPosition][IR_Y];
lilac0112_1 28:8ac6c3c1e643 287 vs = data.OutputPID;
lilac0112_1 28:8ac6c3c1e643 288 if(data.irPosition<8){
lilac0112_1 28:8ac6c3c1e643 289 vx *= data.l_pow;
lilac0112_1 28:8ac6c3c1e643 290 vy *= data.l_pow;
lilac0112_1 28:8ac6c3c1e643 291 }
lilac0112_1 28:8ac6c3c1e643 292 else{
lilac0112_1 28:8ac6c3c1e643 293 vx *= data.s_pow;
lilac0112_1 28:8ac6c3c1e643 294 vy *= data.s_pow;
lilac0112_1 28:8ac6c3c1e643 295 }
lilac0112_1 28:8ac6c3c1e643 296
lilac0112_1 28:8ac6c3c1e643 297
lilac0112_1 28:8ac6c3c1e643 298 move(vx, vy, vs);
lilac0112_1 28:8ac6c3c1e643 299
lilac0112_1 28:8ac6c3c1e643 300 if(data.MotorFlag==1){
lilac0112_1 28:8ac6c3c1e643 301 tx_motor();
lilac0112_1 28:8ac6c3c1e643 302 data.MotorFlag=0;
lilac0112_1 28:8ac6c3c1e643 303 }
lilac0112_1 28:8ac6c3c1e643 304 return;
lilac0112_1 28:8ac6c3c1e643 305 }
lilac0112_1 28:8ac6c3c1e643 306 void modeDebug2(void){//LineRestoringForce
lilac0112_1 28:8ac6c3c1e643 307 int vx,vy,vs;
lilac0112_1 28:8ac6c3c1e643 308 uint8_t LineStop[2];
lilac0112_1 28:8ac6c3c1e643 309 int LineForce[2];
lilac0112_1 28:8ac6c3c1e643 310 if(data.IrFlag==1){
lilac0112_1 28:8ac6c3c1e643 311 ReadIr();
lilac0112_1 28:8ac6c3c1e643 312 data.IrFlag=0;
lilac0112_1 28:8ac6c3c1e643 313 }
lilac0112_1 28:8ac6c3c1e643 314 if(data.PidFlag==1){
lilac0112_1 28:8ac6c3c1e643 315 PidUpdate();
lilac0112_1 28:8ac6c3c1e643 316 data.PidFlag=0;
lilac0112_1 28:8ac6c3c1e643 317 }
lilac0112_1 27:769cb5a7ea37 318 vx = ir_move_val[data.irNotice][data.irPosition][IR_X];
lilac0112_1 27:769cb5a7ea37 319 vy = ir_move_val[data.irNotice][data.irPosition][IR_Y];
lilac0112_1 27:769cb5a7ea37 320 vs = data.OutputPID;
lilac0112_1 28:8ac6c3c1e643 321 if(data.irPosition<8){
lilac0112_1 28:8ac6c3c1e643 322 vx *= data.l_pow;
lilac0112_1 28:8ac6c3c1e643 323 vy *= data.l_pow;
lilac0112_1 28:8ac6c3c1e643 324 }
lilac0112_1 28:8ac6c3c1e643 325 else{
lilac0112_1 28:8ac6c3c1e643 326 vx *= data.s_pow;
lilac0112_1 28:8ac6c3c1e643 327 vy *= data.s_pow;
lilac0112_1 28:8ac6c3c1e643 328 }
lilac0112_1 28:8ac6c3c1e643 329
lilac0112_1 28:8ac6c3c1e643 330
lilac0112_1 28:8ac6c3c1e643 331 LineStop[X_AXIS] = (!((vx>0)&&((data.lnFlag[A_SPOT]==1)&&(1))))*(!((vx<0)&&((data.lnFlag[B_SPOT]==1)&&(1))));
lilac0112_1 28:8ac6c3c1e643 332 LineStop[Y_AXIS] = (!((vy>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==1))))*(!((vy<0)&&((data.lnFlag[C_SPOT]==1)&&(1))));
lilac0112_1 28:8ac6c3c1e643 333
lilac0112_1 28:8ac6c3c1e643 334 LineForce[X_AXIS] = LineForce[Y_AXIS] = 0;
lilac0112_1 28:8ac6c3c1e643 335 if(((vx>0)&&((data.lnFlag[A_SPOT]==1)&&(1)))){
lilac0112_1 28:8ac6c3c1e643 336 LineForce[X_AXIS] = -LINE_RF;
lilac0112_1 28:8ac6c3c1e643 337 }
lilac0112_1 28:8ac6c3c1e643 338 if(((vx<0)&&((data.lnFlag[B_SPOT]==1)&&(1)))){
lilac0112_1 28:8ac6c3c1e643 339 LineForce[X_AXIS] = LINE_RF;
lilac0112_1 28:8ac6c3c1e643 340 }
lilac0112_1 28:8ac6c3c1e643 341 if(((vy>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==1)))){
lilac0112_1 28:8ac6c3c1e643 342 LineForce[Y_AXIS] = -LINE_RF;
lilac0112_1 28:8ac6c3c1e643 343 }
lilac0112_1 28:8ac6c3c1e643 344 if(((vy<0)&&((data.lnFlag[C_SPOT]==1)&&(1)))){
lilac0112_1 28:8ac6c3c1e643 345 LineForce[Y_AXIS] = LINE_RF;
lilac0112_1 28:8ac6c3c1e643 346 }
lilac0112_1 28:8ac6c3c1e643 347
lilac0112_1 28:8ac6c3c1e643 348 move(
lilac0112_1 28:8ac6c3c1e643 349 vx*LineStop[X_AXIS] + LineForce[X_AXIS],
lilac0112_1 28:8ac6c3c1e643 350 vy*LineStop[Y_AXIS] + LineForce[Y_AXIS],
lilac0112_1 28:8ac6c3c1e643 351 vs
lilac0112_1 28:8ac6c3c1e643 352 );
lilac0112_1 28:8ac6c3c1e643 353 if(data.MotorFlag==1){
lilac0112_1 28:8ac6c3c1e643 354 tx_motor();
lilac0112_1 28:8ac6c3c1e643 355 data.MotorFlag=0;
lilac0112_1 28:8ac6c3c1e643 356 }
lilac0112_1 28:8ac6c3c1e643 357 return;
lilac0112_1 28:8ac6c3c1e643 358 }
lilac0112_1 28:8ac6c3c1e643 359 void modeDebug3(void){//movesum
lilac0112_1 28:8ac6c3c1e643 360 int vx,vy,vs;
lilac0112_1 28:8ac6c3c1e643 361 uint8_t LineStop[2];
lilac0112_1 28:8ac6c3c1e643 362 static uint8_t moveLnFlag[4]={1, 1, 1, 1};
lilac0112_1 28:8ac6c3c1e643 363 static int moveLnlog[4];
lilac0112_1 28:8ac6c3c1e643 364 if(data.IrFlag==1){
lilac0112_1 28:8ac6c3c1e643 365 ReadIr();
lilac0112_1 28:8ac6c3c1e643 366 data.IrFlag=0;
lilac0112_1 28:8ac6c3c1e643 367 }
lilac0112_1 28:8ac6c3c1e643 368 if(data.PidFlag==1){
lilac0112_1 28:8ac6c3c1e643 369 PidUpdate();
lilac0112_1 28:8ac6c3c1e643 370 data.PidFlag=0;
lilac0112_1 28:8ac6c3c1e643 371 }
lilac0112_1 28:8ac6c3c1e643 372 vx = ir_move_val[data.irNotice][data.irPosition][IR_X];
lilac0112_1 28:8ac6c3c1e643 373 vy = ir_move_val[data.irNotice][data.irPosition][IR_Y];
lilac0112_1 28:8ac6c3c1e643 374 vs = data.OutputPID;
lilac0112_1 28:8ac6c3c1e643 375 if(data.irPosition<8){
lilac0112_1 28:8ac6c3c1e643 376 vx *= data.l_pow;
lilac0112_1 28:8ac6c3c1e643 377 vy *= data.l_pow;
lilac0112_1 28:8ac6c3c1e643 378 }
lilac0112_1 28:8ac6c3c1e643 379 else{
lilac0112_1 28:8ac6c3c1e643 380 vx *= data.s_pow;
lilac0112_1 28:8ac6c3c1e643 381 vy *= data.s_pow;
lilac0112_1 28:8ac6c3c1e643 382 }
lilac0112_1 27:769cb5a7ea37 383
lilac0112_1 27:769cb5a7ea37 384 data.motorlog[X_AXIS] += vx;
lilac0112_1 27:769cb5a7ea37 385 data.motorlog[Y_AXIS] += vy;
lilac0112_1 27:769cb5a7ea37 386
lilac0112_1 27:769cb5a7ea37 387 if((data.lnFlag[A_SPOT]==1)&&(1)){
lilac0112_1 27:769cb5a7ea37 388 moveLnFlag[A_SPOT]=0;
lilac0112_1 27:769cb5a7ea37 389 moveLnlog[A_SPOT] = data.motorlog[X_AXIS];
lilac0112_1 27:769cb5a7ea37 390 }
lilac0112_1 27:769cb5a7ea37 391 if((data.lnFlag[B_SPOT]==1)&&(1)){
lilac0112_1 27:769cb5a7ea37 392 moveLnFlag[B_SPOT]=0;
lilac0112_1 27:769cb5a7ea37 393 moveLnlog[B_SPOT] = data.motorlog[X_AXIS];
lilac0112_1 27:769cb5a7ea37 394 }
lilac0112_1 27:769cb5a7ea37 395 if((data.lnFlag[C_SPOT]==1)&&(1)){
lilac0112_1 27:769cb5a7ea37 396 moveLnFlag[C_SPOT]=0;
lilac0112_1 27:769cb5a7ea37 397 moveLnlog[C_SPOT] = data.motorlog[Y_AXIS];
lilac0112_1 27:769cb5a7ea37 398 }
lilac0112_1 27:769cb5a7ea37 399 if((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==1)){
lilac0112_1 27:769cb5a7ea37 400 moveLnFlag[AB_SPOT]=0;
lilac0112_1 27:769cb5a7ea37 401 moveLnlog[AB_SPOT] = data.motorlog[Y_AXIS];
lilac0112_1 27:769cb5a7ea37 402 }
lilac0112_1 27:769cb5a7ea37 403
lilac0112_1 27:769cb5a7ea37 404 if(((data.motorlog[X_AXIS]-moveLnlog[A_SPOT])<(-RELEASE_VAL))&&(moveLnFlag[A_SPOT]==0)) moveLnFlag[A_SPOT]=1;
lilac0112_1 27:769cb5a7ea37 405 if(((data.motorlog[X_AXIS]-moveLnlog[B_SPOT])>(RELEASE_VAL))&&(moveLnFlag[B_SPOT]==0)) moveLnFlag[B_SPOT]=1;
lilac0112_1 27:769cb5a7ea37 406 if(((data.motorlog[Y_AXIS]-moveLnlog[C_SPOT])>(RELEASE_VAL))&&(moveLnFlag[C_SPOT]==0)) moveLnFlag[C_SPOT]=1;
lilac0112_1 27:769cb5a7ea37 407 if(((data.motorlog[Y_AXIS]-moveLnlog[AB_SPOT])<(-RELEASE_VAL))&&(moveLnFlag[AB_SPOT]==0)) moveLnFlag[AB_SPOT]=1;
lilac0112_1 27:769cb5a7ea37 408
lilac0112_1 27:769cb5a7ea37 409 LineStop[X_AXIS] = (!((vx>0)&&(moveLnFlag[A_SPOT]==0)))*(!((vx<0)&&(moveLnFlag[B_SPOT]==0)));
lilac0112_1 27:769cb5a7ea37 410 LineStop[Y_AXIS] = (!((vy>0)&&(moveLnFlag[C_SPOT]==0)))*(!((vy<0)&&(moveLnFlag[AB_SPOT]==0)));
lilac0112_1 26:fbb03281fc7d 411
lilac0112_1 27:769cb5a7ea37 412 move(
lilac0112_1 27:769cb5a7ea37 413 vx*LineStop[X_AXIS],
lilac0112_1 27:769cb5a7ea37 414 vy*LineStop[Y_AXIS],
lilac0112_1 27:769cb5a7ea37 415 vs
lilac0112_1 26:fbb03281fc7d 416 );
lilac0112_1 27:769cb5a7ea37 417 if(data.MotorFlag==1){
lilac0112_1 27:769cb5a7ea37 418 tx_motor();
lilac0112_1 27:769cb5a7ea37 419 data.MotorFlag=0;
lilac0112_1 27:769cb5a7ea37 420 }
lilac0112_1 21:d69a8f3c76e1 421 return;
lilac0112_1 21:d69a8f3c76e1 422 }
lilac0112_1 28:8ac6c3c1e643 423 void modeDebug4(void){//solenoid
lilac0112_1 28:8ac6c3c1e643 424 if(data.KickFlag==1){
lilac0112_1 28:8ac6c3c1e643 425 DriveSolenoid();
lilac0112_1 28:8ac6c3c1e643 426 }
lilac0112_1 21:d69a8f3c76e1 427 return;
lilac0112_1 21:d69a8f3c76e1 428 }
lilac0112_1 28:8ac6c3c1e643 429 void modeDebug5(void){//cmpsTest
lilac0112_1 28:8ac6c3c1e643 430 if(data.IrFlag==1){
lilac0112_1 28:8ac6c3c1e643 431 ReadIr();
lilac0112_1 28:8ac6c3c1e643 432 data.IrFlag=0;
lilac0112_1 28:8ac6c3c1e643 433 }
lilac0112_1 28:8ac6c3c1e643 434 if(data.PidFlag==1){
lilac0112_1 28:8ac6c3c1e643 435 PidUpdate();
lilac0112_1 28:8ac6c3c1e643 436 data.PidFlag=0;
lilac0112_1 28:8ac6c3c1e643 437 }
lilac0112_1 28:8ac6c3c1e643 438 move(0,0,data.OutputPID);
lilac0112_1 28:8ac6c3c1e643 439 if(data.MotorFlag==1){
lilac0112_1 28:8ac6c3c1e643 440 tx_motor();
lilac0112_1 28:8ac6c3c1e643 441 data.MotorFlag=0;
lilac0112_1 28:8ac6c3c1e643 442 }
lilac0112_1 28:8ac6c3c1e643 443
lilac0112_1 19:342da3a5a474 444 return;
lilac0112_1 19:342da3a5a474 445 }