CatPot 2015-2016 / Mbed 2 deprecated CatPot_Main_T_2v00

Main Program

Dependencies:   mbed AQM1602 HMC6352 PID

main_processing/strategy/strategy.cpp@34:69bdf29a4442, 2016-02-02 (annotated)

Committer:
lilac0112_1
Date:
Tue Feb 02 10:31:05 2016 +0000
Revision:
34:69bdf29a4442
Parent:
31:0b0f64831771
Child:
35:a0421268ee95
chase and stop ggggood

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