CatPot 2015-2016 / Mbed 2 deprecated CatPot_Main_T_2v00

Main Program

Dependencies:   mbed AQM1602 HMC6352 PID

main_processing/strategy/strategy.cpp@36:50092d488f78, 2016-02-03 (annotated)

Committer:
lilac0112_1
Date:
Wed Feb 03 07:46:32 2016 +0000
Revision:
36:50092d488f78
Parent:
35:a0421268ee95
Child:
37:ae5661306900
somehow

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