CatPot 2015-2016 / Mbed 2 deprecated CatPot_Main_T_2v00

Main Program

Dependencies:   mbed AQM1602 HMC6352 PID

main_processing/strategy/strategy.cpp@31:0b0f64831771, 2016-02-02 (annotated)

Committer:
lilac0112_1
Date:
Tue Feb 02 04:07:06 2016 +0000
Revision:
31:0b0f64831771
Parent:
30:26070ba1f21f
Child:
34:69bdf29a4442
goodline

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