CatPot 2015-2016 / Mbed 2 deprecated CatPot_Main_T_2v00

Main Program

Dependencies:   mbed AQM1602 HMC6352 PID

main_processing/strategy/strategy.cpp@40:f68474b1d5e7, 2016-02-04 (annotated)

Committer:
lilac0112_1
Date:
Thu Feb 04 07:12:34 2016 +0000
Revision:
40:f68474b1d5e7
Parent:
38:a7eb15b1f813
Child:
41:3cdcdec9c885
Line...Return by IrRange; pow 30 ; Turn good

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