CatPot 2015-2016 / Mbed 2 deprecated CatPot_Main_T_2v00

Main Program

Dependencies:   mbed AQM1602 HMC6352 PID

main_processing/strategy/strategy.cpp@29:b413b0bb07a1, 2016-02-01 (annotated)

Committer:
lilac0112_1
Date:
Mon Feb 01 11:25:32 2016 +0000
Revision:
29:b413b0bb07a1
Parent:
28:8ac6c3c1e643
Child:
30:26070ba1f21f
dangerous

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 29:b413b0bb07a1 10 //uint8_t IrRange[4];
lilac0112_1 28:8ac6c3c1e643 11 if(data.IrFlag==1){
lilac0112_1 28:8ac6c3c1e643 12 ReadIr();
lilac0112_1 28:8ac6c3c1e643 13 data.IrFlag=0;
lilac0112_1 28:8ac6c3c1e643 14 }
lilac0112_1 28:8ac6c3c1e643 15 if(data.PidFlag==1){
lilac0112_1 28:8ac6c3c1e643 16 Line_ticker.detach();
lilac0112_1 28:8ac6c3c1e643 17 PidUpdate();
lilac0112_1 28:8ac6c3c1e643 18 Line_ticker.attach(&ReadLine, 0.005);
lilac0112_1 28:8ac6c3c1e643 19 data.PidFlag=0;
lilac0112_1 28:8ac6c3c1e643 20 }
lilac0112_1 28:8ac6c3c1e643 21 ir_x = ir_move_val[data.irNotice][data.irPosition][IR_X];
lilac0112_1 28:8ac6c3c1e643 22 ir_y = ir_move_val[data.irNotice][data.irPosition][IR_Y];
lilac0112_1 28:8ac6c3c1e643 23 if(data.irPosition<8){
lilac0112_1 28:8ac6c3c1e643 24 ir_x *= data.l_pow;
lilac0112_1 28:8ac6c3c1e643 25 ir_y *= data.l_pow;
lilac0112_1 28:8ac6c3c1e643 26 }
lilac0112_1 28:8ac6c3c1e643 27 else{
lilac0112_1 28:8ac6c3c1e643 28 ir_x *= data.s_pow;
lilac0112_1 28:8ac6c3c1e643 29 ir_y *= data.s_pow;
lilac0112_1 28:8ac6c3c1e643 30 }
lilac0112_1 29:b413b0bb07a1 31 //Lineを考慮していないIrのみの値
lilac0112_1 29:b413b0bb07a1 32 vx = ir_x;
lilac0112_1 29:b413b0bb07a1 33 vy = ir_y;
lilac0112_1 29:b413b0bb07a1 34 //Line検出方向を調べる
lilac0112_1 28:8ac6c3c1e643 35 LineDir[A_SPOT] = (!((vx>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==0))));
lilac0112_1 28:8ac6c3c1e643 36 LineDir[B_SPOT] = (!((vx<0)&&((data.lnFlag[A_SPOT]==0)&&(data.lnFlag[B_SPOT]==1))));
lilac0112_1 28:8ac6c3c1e643 37 LineDir[C_SPOT] = (!((vy<0)&&((data.lnFlag[C_SPOT]==1)&&(1))));
lilac0112_1 28:8ac6c3c1e643 38 LineDir[AB_SPOT] = (!((vy>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==1))));
lilac0112_1 28:8ac6c3c1e643 39
lilac0112_1 28:8ac6c3c1e643 40 LineStop[X_AXIS] = LineDir[A_SPOT]*LineDir[B_SPOT];
lilac0112_1 28:8ac6c3c1e643 41 LineStop[Y_AXIS] = LineDir[C_SPOT]*LineDir[AB_SPOT];
lilac0112_1 28:8ac6c3c1e643 42
lilac0112_1 28:8ac6c3c1e643 43
lilac0112_1 29:b413b0bb07a1 44 /*
lilac0112_1 28:8ac6c3c1e643 45 IrRange[A_SPOT] = (data.irPosition==19)||(data.irPosition==8)||(data.irPosition==9);
lilac0112_1 29:b413b0bb07a1 46 IrRange[B_SPOT] = (data.irPosition==13)||(data.irPosition==14)||(data.irPosition==15);
lilac0112_1 29:b413b0bb07a1 47 IrRange[C_SPOT] = (data.irPosition==16)||(data.irPosition==17)||(data.irPosition==18);
lilac0112_1 29:b413b0bb07a1 48 IrRange[AB_SPOT] = (data.irPosition==10)||(data.irPosition==11)||(data.irPosition==12);
lilac0112_1 29:b413b0bb07a1 49 */
lilac0112_1 29:b413b0bb07a1 50 /*
lilac0112_1 29:b413b0bb07a1 51 IrRange[A_SPOT] = ((18<=data.irPosition)&&(data.irPosition<=19))||((8<=data.irPosition)&&(data.irPosition<=10))
lilac0112_1 29:b413b0bb07a1 52 ||((6<=data.irPosition)&&(data.irPosition<=7))||(( 1)&&(data.irPosition<=1));
lilac0112_1 29:b413b0bb07a1 53 IrRange[B_SPOT] = ((12<=data.irPosition)&&(data.irPosition<=16))||((2<=data.irPosition)&&(data.irPosition<=5));
lilac0112_1 29:b413b0bb07a1 54 IrRange[C_SPOT] = ((15<=data.irPosition)&&(data.irPosition<=19))||((4<=data.irPosition)&&(data.irPosition<=7));
lilac0112_1 29:b413b0bb07a1 55 IrRange[AB_SPOT] = ((9<=data.irPosition)&&(data.irPosition<=13))||(( 1)&&(data.irPosition<=3));
lilac0112_1 29:b413b0bb07a1 56
lilac0112_1 29:b413b0bb07a1 57 data.LineBind[A_SPOT] = ((IrRange[A_SPOT]==1)&&((LineDir[A_SPOT]==0)||(data.LineBind[A_SPOT]==1)));
lilac0112_1 29:b413b0bb07a1 58 data.LineBind[B_SPOT] = ((IrRange[B_SPOT]==1)&&((LineDir[B_SPOT]==0)||(data.LineBind[B_SPOT]==1)));
lilac0112_1 29:b413b0bb07a1 59 data.LineBind[C_SPOT] = ((IrRange[C_SPOT]==1)&&((LineDir[C_SPOT]==0)||(data.LineBind[C_SPOT]==1)));
lilac0112_1 29:b413b0bb07a1 60 data.LineBind[AB_SPOT] = ((IrRange[AB_SPOT]==1)&&((LineDir[AB_SPOT]==0)||(data.LineBind[AB_SPOT]==1)));
lilac0112_1 29:b413b0bb07a1 61 */
lilac0112_1 28:8ac6c3c1e643 62
lilac0112_1 29:b413b0bb07a1 63 /*
lilac0112_1 29:b413b0bb07a1 64 if(data.LineBind[A_SPOT]||data.LineBind[B_SPOT]||data.LineBind[C_SPOT]||data.LineBind[AB_SPOT]){
lilac0112_1 29:b413b0bb07a1 65 //Line_timeout.attach();
lilac0112_1 29:b413b0bb07a1 66 vx=(LINE_RF*2)*((-1)*data.LineBind[A_SPOT] + data.LineBind[B_SPOT]);
lilac0112_1 29:b413b0bb07a1 67 vy=(LINE_RF*2)*( data.LineBind[C_SPOT] + (-1)*data.LineBind[AB_SPOT]);
lilac0112_1 29:b413b0bb07a1 68 if((LineDir[A_SPOT]==0)||(LineDir[B_SPOT]==0)||(LineDir[C_SPOT]==0)||(LineDir[AB_SPOT]==0)){
lilac0112_1 29:b413b0bb07a1 69 vx=(LINE_RF*2)*(-data.LineBind[A_SPOT] + data.LineBind[B_SPOT]);
lilac0112_1 29:b413b0bb07a1 70 vy=(LINE_RF*2)*(data.LineBind[C_SPOT] - data.LineBind[AB_SPOT]);
lilac0112_1 29:b413b0bb07a1 71 }
lilac0112_1 29:b413b0bb07a1 72 else{
lilac0112_1 29:b413b0bb07a1 73 vx=0;
lilac0112_1 29:b413b0bb07a1 74 vy=0;
lilac0112_1 29:b413b0bb07a1 75 }
lilac0112_1 29:b413b0bb07a1 76 }*/
lilac0112_1 29:b413b0bb07a1 77 /*if(data.LineBind[A_SPOT]||data.LineBind[B_SPOT]||data.LineBind[C_SPOT]||data.LineBind[AB_SPOT]){
lilac0112_1 29:b413b0bb07a1 78 vx=(LINE_RF*2)*((-1.0)*data.LineBind[A_SPOT]*(LineDir[A_SPOT]==0) + data.LineBind[B_SPOT]*(LineDir[B_SPOT]==0));
lilac0112_1 29:b413b0bb07a1 79 vy=(LINE_RF*2)*( data.LineBind[C_SPOT]*(LineDir[C_SPOT]==0) + (-1.0)*data.LineBind[AB_SPOT]*(LineDir[AB_SPOT]==0));
lilac0112_1 29:b413b0bb07a1 80
lilac0112_1 29:b413b0bb07a1 81 }*/
lilac0112_1 28:8ac6c3c1e643 82 vs = data.OutputPID;
lilac0112_1 29:b413b0bb07a1 83 /*
lilac0112_1 29:b413b0bb07a1 84 //ForLineAll
lilac0112_1 29:b413b0bb07a1 85 data.motorlog[X_AXIS] *= LineStop[X_AXIS];
lilac0112_1 29:b413b0bb07a1 86 data.motorlog[Y_AXIS] *= LineStop[Y_AXIS];
lilac0112_1 29:b413b0bb07a1 87 data.motorlog[X_AXIS] += vx;
lilac0112_1 29:b413b0bb07a1 88 data.motorlog[Y_AXIS] += vy;
lilac0112_1 29:b413b0bb07a1 89 */
lilac0112_1 28:8ac6c3c1e643 90 move(
lilac0112_1 29:b413b0bb07a1 91 vx*LineStop[X_AXIS] + (LINE_RF)*(vy!=0)*((-1)*(LineDir[A_SPOT]==0) + (LineDir[B_SPOT]==0)),
lilac0112_1 29:b413b0bb07a1 92 vy*LineStop[Y_AXIS] + (LINE_RF)*(vx!=0)*((LineDir[C_SPOT]==0) + (-1)*(LineDir[AB_SPOT]==0)),
lilac0112_1 28:8ac6c3c1e643 93 vs
lilac0112_1 28:8ac6c3c1e643 94 );
lilac0112_1 28:8ac6c3c1e643 95
lilac0112_1 28:8ac6c3c1e643 96 if(data.MotorFlag==1){
lilac0112_1 28:8ac6c3c1e643 97 tx_motor();
lilac0112_1 28:8ac6c3c1e643 98 data.MotorFlag=0;
lilac0112_1 28:8ac6c3c1e643 99 }
lilac0112_1 28:8ac6c3c1e643 100 return;
lilac0112_1 28:8ac6c3c1e643 101 }
lilac0112_1 28:8ac6c3c1e643 102 void modeAttack1(void){
lilac0112_1 28:8ac6c3c1e643 103 return;
lilac0112_1 28:8ac6c3c1e643 104 }
lilac0112_1 28:8ac6c3c1e643 105 void modeAttack2(void){
lilac0112_1 28:8ac6c3c1e643 106 return;
lilac0112_1 28:8ac6c3c1e643 107 }
lilac0112_1 28:8ac6c3c1e643 108 void modeAttack3(void){
lilac0112_1 28:8ac6c3c1e643 109 return;
lilac0112_1 28:8ac6c3c1e643 110 }
lilac0112_1 28:8ac6c3c1e643 111 void modeAttack4(void){
lilac0112_1 28:8ac6c3c1e643 112 return;
lilac0112_1 28:8ac6c3c1e643 113 }
lilac0112_1 28:8ac6c3c1e643 114 void modeAttack5(void){
lilac0112_1 28:8ac6c3c1e643 115 return;
lilac0112_1 28:8ac6c3c1e643 116 }
lilac0112_1 28:8ac6c3c1e643 117 //Debug
lilac0112_1 28:8ac6c3c1e643 118 void modeDebug0(void){
lilac0112_1 28:8ac6c3c1e643 119 return;
lilac0112_1 28:8ac6c3c1e643 120 }
lilac0112_1 28:8ac6c3c1e643 121 void modeDebug1(void){//ChaseOnly NonLine
lilac0112_1 28:8ac6c3c1e643 122 int vx,vy,vs;
lilac0112_1 24:9fb74ea3c25d 123 if(data.IrFlag==1){
lilac0112_1 24:9fb74ea3c25d 124 ReadIr();
lilac0112_1 24:9fb74ea3c25d 125 data.IrFlag=0;
lilac0112_1 24:9fb74ea3c25d 126 }
lilac0112_1 27:769cb5a7ea37 127 if(data.PidFlag==1){
lilac0112_1 27:769cb5a7ea37 128 PidUpdate();
lilac0112_1 27:769cb5a7ea37 129 data.PidFlag=0;
lilac0112_1 27:769cb5a7ea37 130 }
lilac0112_1 28:8ac6c3c1e643 131
lilac0112_1 28:8ac6c3c1e643 132 vx = ir_move_val[data.irNotice][data.irPosition][IR_X];
lilac0112_1 28:8ac6c3c1e643 133 vy = ir_move_val[data.irNotice][data.irPosition][IR_Y];
lilac0112_1 28:8ac6c3c1e643 134 vs = data.OutputPID;
lilac0112_1 28:8ac6c3c1e643 135 if(data.irPosition<8){
lilac0112_1 28:8ac6c3c1e643 136 vx *= data.l_pow;
lilac0112_1 28:8ac6c3c1e643 137 vy *= data.l_pow;
lilac0112_1 28:8ac6c3c1e643 138 }
lilac0112_1 28:8ac6c3c1e643 139 else{
lilac0112_1 28:8ac6c3c1e643 140 vx *= data.s_pow;
lilac0112_1 28:8ac6c3c1e643 141 vy *= data.s_pow;
lilac0112_1 28:8ac6c3c1e643 142 }
lilac0112_1 28:8ac6c3c1e643 143
lilac0112_1 28:8ac6c3c1e643 144
lilac0112_1 28:8ac6c3c1e643 145 move(vx, vy, vs);
lilac0112_1 28:8ac6c3c1e643 146
lilac0112_1 28:8ac6c3c1e643 147 if(data.MotorFlag==1){
lilac0112_1 28:8ac6c3c1e643 148 tx_motor();
lilac0112_1 28:8ac6c3c1e643 149 data.MotorFlag=0;
lilac0112_1 28:8ac6c3c1e643 150 }
lilac0112_1 28:8ac6c3c1e643 151 return;
lilac0112_1 28:8ac6c3c1e643 152 }
lilac0112_1 28:8ac6c3c1e643 153 void modeDebug2(void){//LineRestoringForce
lilac0112_1 28:8ac6c3c1e643 154 int vx,vy,vs;
lilac0112_1 28:8ac6c3c1e643 155 uint8_t LineStop[2];
lilac0112_1 28:8ac6c3c1e643 156 int LineForce[2];
lilac0112_1 28:8ac6c3c1e643 157 if(data.IrFlag==1){
lilac0112_1 28:8ac6c3c1e643 158 ReadIr();
lilac0112_1 28:8ac6c3c1e643 159 data.IrFlag=0;
lilac0112_1 28:8ac6c3c1e643 160 }
lilac0112_1 28:8ac6c3c1e643 161 if(data.PidFlag==1){
lilac0112_1 28:8ac6c3c1e643 162 PidUpdate();
lilac0112_1 28:8ac6c3c1e643 163 data.PidFlag=0;
lilac0112_1 28:8ac6c3c1e643 164 }
lilac0112_1 27:769cb5a7ea37 165 vx = ir_move_val[data.irNotice][data.irPosition][IR_X];
lilac0112_1 27:769cb5a7ea37 166 vy = ir_move_val[data.irNotice][data.irPosition][IR_Y];
lilac0112_1 27:769cb5a7ea37 167 vs = data.OutputPID;
lilac0112_1 28:8ac6c3c1e643 168 if(data.irPosition<8){
lilac0112_1 28:8ac6c3c1e643 169 vx *= data.l_pow;
lilac0112_1 28:8ac6c3c1e643 170 vy *= data.l_pow;
lilac0112_1 28:8ac6c3c1e643 171 }
lilac0112_1 28:8ac6c3c1e643 172 else{
lilac0112_1 28:8ac6c3c1e643 173 vx *= data.s_pow;
lilac0112_1 28:8ac6c3c1e643 174 vy *= data.s_pow;
lilac0112_1 28:8ac6c3c1e643 175 }
lilac0112_1 28:8ac6c3c1e643 176
lilac0112_1 28:8ac6c3c1e643 177
lilac0112_1 28:8ac6c3c1e643 178 LineStop[X_AXIS] = (!((vx>0)&&((data.lnFlag[A_SPOT]==1)&&(1))))*(!((vx<0)&&((data.lnFlag[B_SPOT]==1)&&(1))));
lilac0112_1 28:8ac6c3c1e643 179 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 180
lilac0112_1 28:8ac6c3c1e643 181 LineForce[X_AXIS] = LineForce[Y_AXIS] = 0;
lilac0112_1 28:8ac6c3c1e643 182 if(((vx>0)&&((data.lnFlag[A_SPOT]==1)&&(1)))){
lilac0112_1 28:8ac6c3c1e643 183 LineForce[X_AXIS] = -LINE_RF;
lilac0112_1 28:8ac6c3c1e643 184 }
lilac0112_1 28:8ac6c3c1e643 185 if(((vx<0)&&((data.lnFlag[B_SPOT]==1)&&(1)))){
lilac0112_1 28:8ac6c3c1e643 186 LineForce[X_AXIS] = LINE_RF;
lilac0112_1 28:8ac6c3c1e643 187 }
lilac0112_1 28:8ac6c3c1e643 188 if(((vy>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==1)))){
lilac0112_1 28:8ac6c3c1e643 189 LineForce[Y_AXIS] = -LINE_RF;
lilac0112_1 28:8ac6c3c1e643 190 }
lilac0112_1 28:8ac6c3c1e643 191 if(((vy<0)&&((data.lnFlag[C_SPOT]==1)&&(1)))){
lilac0112_1 28:8ac6c3c1e643 192 LineForce[Y_AXIS] = LINE_RF;
lilac0112_1 28:8ac6c3c1e643 193 }
lilac0112_1 28:8ac6c3c1e643 194
lilac0112_1 28:8ac6c3c1e643 195 move(
lilac0112_1 28:8ac6c3c1e643 196 vx*LineStop[X_AXIS] + LineForce[X_AXIS],
lilac0112_1 28:8ac6c3c1e643 197 vy*LineStop[Y_AXIS] + LineForce[Y_AXIS],
lilac0112_1 28:8ac6c3c1e643 198 vs
lilac0112_1 28:8ac6c3c1e643 199 );
lilac0112_1 28:8ac6c3c1e643 200 if(data.MotorFlag==1){
lilac0112_1 28:8ac6c3c1e643 201 tx_motor();
lilac0112_1 28:8ac6c3c1e643 202 data.MotorFlag=0;
lilac0112_1 28:8ac6c3c1e643 203 }
lilac0112_1 28:8ac6c3c1e643 204 return;
lilac0112_1 28:8ac6c3c1e643 205 }
lilac0112_1 28:8ac6c3c1e643 206 void modeDebug3(void){//movesum
lilac0112_1 28:8ac6c3c1e643 207 int vx,vy,vs;
lilac0112_1 28:8ac6c3c1e643 208 uint8_t LineStop[2];
lilac0112_1 28:8ac6c3c1e643 209 static uint8_t moveLnFlag[4]={1, 1, 1, 1};
lilac0112_1 28:8ac6c3c1e643 210 static int moveLnlog[4];
lilac0112_1 28:8ac6c3c1e643 211 if(data.IrFlag==1){
lilac0112_1 28:8ac6c3c1e643 212 ReadIr();
lilac0112_1 28:8ac6c3c1e643 213 data.IrFlag=0;
lilac0112_1 28:8ac6c3c1e643 214 }
lilac0112_1 28:8ac6c3c1e643 215 if(data.PidFlag==1){
lilac0112_1 28:8ac6c3c1e643 216 PidUpdate();
lilac0112_1 28:8ac6c3c1e643 217 data.PidFlag=0;
lilac0112_1 28:8ac6c3c1e643 218 }
lilac0112_1 28:8ac6c3c1e643 219 vx = ir_move_val[data.irNotice][data.irPosition][IR_X];
lilac0112_1 28:8ac6c3c1e643 220 vy = ir_move_val[data.irNotice][data.irPosition][IR_Y];
lilac0112_1 28:8ac6c3c1e643 221 vs = data.OutputPID;
lilac0112_1 28:8ac6c3c1e643 222 if(data.irPosition<8){
lilac0112_1 28:8ac6c3c1e643 223 vx *= data.l_pow;
lilac0112_1 28:8ac6c3c1e643 224 vy *= data.l_pow;
lilac0112_1 28:8ac6c3c1e643 225 }
lilac0112_1 28:8ac6c3c1e643 226 else{
lilac0112_1 28:8ac6c3c1e643 227 vx *= data.s_pow;
lilac0112_1 28:8ac6c3c1e643 228 vy *= data.s_pow;
lilac0112_1 28:8ac6c3c1e643 229 }
lilac0112_1 27:769cb5a7ea37 230
lilac0112_1 27:769cb5a7ea37 231 data.motorlog[X_AXIS] += vx;
lilac0112_1 27:769cb5a7ea37 232 data.motorlog[Y_AXIS] += vy;
lilac0112_1 27:769cb5a7ea37 233
lilac0112_1 27:769cb5a7ea37 234 if((data.lnFlag[A_SPOT]==1)&&(1)){
lilac0112_1 27:769cb5a7ea37 235 moveLnFlag[A_SPOT]=0;
lilac0112_1 27:769cb5a7ea37 236 moveLnlog[A_SPOT] = data.motorlog[X_AXIS];
lilac0112_1 27:769cb5a7ea37 237 }
lilac0112_1 27:769cb5a7ea37 238 if((data.lnFlag[B_SPOT]==1)&&(1)){
lilac0112_1 27:769cb5a7ea37 239 moveLnFlag[B_SPOT]=0;
lilac0112_1 27:769cb5a7ea37 240 moveLnlog[B_SPOT] = data.motorlog[X_AXIS];
lilac0112_1 27:769cb5a7ea37 241 }
lilac0112_1 27:769cb5a7ea37 242 if((data.lnFlag[C_SPOT]==1)&&(1)){
lilac0112_1 27:769cb5a7ea37 243 moveLnFlag[C_SPOT]=0;
lilac0112_1 27:769cb5a7ea37 244 moveLnlog[C_SPOT] = data.motorlog[Y_AXIS];
lilac0112_1 27:769cb5a7ea37 245 }
lilac0112_1 27:769cb5a7ea37 246 if((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==1)){
lilac0112_1 27:769cb5a7ea37 247 moveLnFlag[AB_SPOT]=0;
lilac0112_1 27:769cb5a7ea37 248 moveLnlog[AB_SPOT] = data.motorlog[Y_AXIS];
lilac0112_1 27:769cb5a7ea37 249 }
lilac0112_1 27:769cb5a7ea37 250
lilac0112_1 27:769cb5a7ea37 251 if(((data.motorlog[X_AXIS]-moveLnlog[A_SPOT])<(-RELEASE_VAL))&&(moveLnFlag[A_SPOT]==0)) moveLnFlag[A_SPOT]=1;
lilac0112_1 27:769cb5a7ea37 252 if(((data.motorlog[X_AXIS]-moveLnlog[B_SPOT])>(RELEASE_VAL))&&(moveLnFlag[B_SPOT]==0)) moveLnFlag[B_SPOT]=1;
lilac0112_1 27:769cb5a7ea37 253 if(((data.motorlog[Y_AXIS]-moveLnlog[C_SPOT])>(RELEASE_VAL))&&(moveLnFlag[C_SPOT]==0)) moveLnFlag[C_SPOT]=1;
lilac0112_1 27:769cb5a7ea37 254 if(((data.motorlog[Y_AXIS]-moveLnlog[AB_SPOT])<(-RELEASE_VAL))&&(moveLnFlag[AB_SPOT]==0)) moveLnFlag[AB_SPOT]=1;
lilac0112_1 27:769cb5a7ea37 255
lilac0112_1 27:769cb5a7ea37 256 LineStop[X_AXIS] = (!((vx>0)&&(moveLnFlag[A_SPOT]==0)))*(!((vx<0)&&(moveLnFlag[B_SPOT]==0)));
lilac0112_1 27:769cb5a7ea37 257 LineStop[Y_AXIS] = (!((vy>0)&&(moveLnFlag[C_SPOT]==0)))*(!((vy<0)&&(moveLnFlag[AB_SPOT]==0)));
lilac0112_1 26:fbb03281fc7d 258
lilac0112_1 27:769cb5a7ea37 259 move(
lilac0112_1 27:769cb5a7ea37 260 vx*LineStop[X_AXIS],
lilac0112_1 27:769cb5a7ea37 261 vy*LineStop[Y_AXIS],
lilac0112_1 27:769cb5a7ea37 262 vs
lilac0112_1 26:fbb03281fc7d 263 );
lilac0112_1 27:769cb5a7ea37 264 if(data.MotorFlag==1){
lilac0112_1 27:769cb5a7ea37 265 tx_motor();
lilac0112_1 27:769cb5a7ea37 266 data.MotorFlag=0;
lilac0112_1 27:769cb5a7ea37 267 }
lilac0112_1 21:d69a8f3c76e1 268 return;
lilac0112_1 21:d69a8f3c76e1 269 }
lilac0112_1 28:8ac6c3c1e643 270 void modeDebug4(void){//solenoid
lilac0112_1 28:8ac6c3c1e643 271 if(data.KickFlag==1){
lilac0112_1 28:8ac6c3c1e643 272 DriveSolenoid();
lilac0112_1 28:8ac6c3c1e643 273 }
lilac0112_1 21:d69a8f3c76e1 274 return;
lilac0112_1 21:d69a8f3c76e1 275 }
lilac0112_1 28:8ac6c3c1e643 276 void modeDebug5(void){//cmpsTest
lilac0112_1 28:8ac6c3c1e643 277 if(data.IrFlag==1){
lilac0112_1 28:8ac6c3c1e643 278 ReadIr();
lilac0112_1 28:8ac6c3c1e643 279 data.IrFlag=0;
lilac0112_1 28:8ac6c3c1e643 280 }
lilac0112_1 28:8ac6c3c1e643 281 if(data.PidFlag==1){
lilac0112_1 28:8ac6c3c1e643 282 PidUpdate();
lilac0112_1 28:8ac6c3c1e643 283 data.PidFlag=0;
lilac0112_1 28:8ac6c3c1e643 284 }
lilac0112_1 28:8ac6c3c1e643 285 move(0,0,data.OutputPID);
lilac0112_1 28:8ac6c3c1e643 286 if(data.MotorFlag==1){
lilac0112_1 28:8ac6c3c1e643 287 tx_motor();
lilac0112_1 28:8ac6c3c1e643 288 data.MotorFlag=0;
lilac0112_1 28:8ac6c3c1e643 289 }
lilac0112_1 28:8ac6c3c1e643 290
lilac0112_1 19:342da3a5a474 291 return;
lilac0112_1 19:342da3a5a474 292 }