CatPot 2015-2016 / Mbed 2 deprecated CatPot_Main_T_2v00

Main Program

Dependencies:   mbed AQM1602 HMC6352 PID

main_processing/strategy/strategy.cpp@35:a0421268ee95, 2016-02-02 (annotated)

Committer:
lilac0112_1
Date:
Tue Feb 02 11:28:15 2016 +0000
Revision:
35:a0421268ee95
Parent:
34:69bdf29a4442
Child:
36:50092d488f78
motor reverse goodgood

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 34:69bdf29a4442 119 IrRange[A_SPOT] = ((18<=data.irPosition)&&(data.irPosition<=19))||((8<=data.irPosition)&&(data.irPosition<=10))
lilac0112_1 34:69bdf29a4442 120 ||((6<=data.irPosition)&&(data.irPosition<=7))||(( 1)&&(data.irPosition<=1));
lilac0112_1 34:69bdf29a4442 121 IrRange[B_SPOT] = ((12<=data.irPosition)&&(data.irPosition<=16))||((2<=data.irPosition)&&(data.irPosition<=5));
lilac0112_1 34:69bdf29a4442 122 IrRange[C_SPOT] = ((15<=data.irPosition)&&(data.irPosition<=19))||((4<=data.irPosition)&&(data.irPosition<=7));
lilac0112_1 34:69bdf29a4442 123 IrRange[AB_SPOT] = ((9<=data.irPosition)&&(data.irPosition<=13))||(( 1)&&(data.irPosition<=3));
lilac0112_1 34:69bdf29a4442 124
lilac0112_1 35:a0421268ee95 125
lilac0112_1 35:a0421268ee95 126 LinePulse[A_SPOT] = ((IrRange[A_SPOT]==1)&&(LineDir[A_SPOT]==0));
lilac0112_1 35:a0421268ee95 127 LinePulse[B_SPOT] = ((IrRange[B_SPOT]==1)&&(LineDir[B_SPOT]==0));
lilac0112_1 35:a0421268ee95 128 LinePulse[C_SPOT] = ((IrRange[C_SPOT]==1)&&(LineDir[C_SPOT]==0));
lilac0112_1 35:a0421268ee95 129 LinePulse[AB_SPOT] = ((IrRange[AB_SPOT]==1)&&(LineDir[AB_SPOT]==0));
lilac0112_1 35:a0421268ee95 130
lilac0112_1 35:a0421268ee95 131 LineBind[A_SPOT] = ((LinePulse[A_SPOT])||((IrRange[A_SPOT]==1)&&(LineBind[A_SPOT]==1)))&&(LineBind[B_SPOT]==0);
lilac0112_1 35:a0421268ee95 132 LineBind[B_SPOT] = ((LinePulse[B_SPOT])||((IrRange[B_SPOT]==1)&&(LineBind[B_SPOT]==1)))&&(LineBind[A_SPOT]==0);
lilac0112_1 35:a0421268ee95 133 LineBind[C_SPOT] = ((LinePulse[C_SPOT])||((IrRange[C_SPOT]==1)&&(LineBind[C_SPOT]==1)))&&(LineBind[AB_SPOT]==0);
lilac0112_1 35:a0421268ee95 134 LineBind[AB_SPOT] = ((LinePulse[AB_SPOT])||((IrRange[AB_SPOT]==1)&&(LineBind[AB_SPOT]==1)))&&(LineBind[C_SPOT]==0);
lilac0112_1 35:a0421268ee95 135 /*
lilac0112_1 34:69bdf29a4442 136 LineBind[A_SPOT] = ((IrRange[A_SPOT]==1)&&((LineDir[A_SPOT]==0)||(LineBind[A_SPOT]==1)));
lilac0112_1 34:69bdf29a4442 137 LineBind[B_SPOT] = ((IrRange[B_SPOT]==1)&&((LineDir[B_SPOT]==0)||(LineBind[B_SPOT]==1)));
lilac0112_1 34:69bdf29a4442 138 LineBind[C_SPOT] = ((IrRange[C_SPOT]==1)&&((LineDir[C_SPOT]==0)||(LineBind[C_SPOT]==1)));
lilac0112_1 34:69bdf29a4442 139 LineBind[AB_SPOT] = ((IrRange[AB_SPOT]==1)&&((LineDir[AB_SPOT]==0)||(LineBind[AB_SPOT]==1)));
lilac0112_1 35:a0421268ee95 140 */
lilac0112_1 34:69bdf29a4442 141
lilac0112_1 34:69bdf29a4442 142 if((LineBind[A_SPOT]==1)||(LineBind[B_SPOT]==1)||(LineBind[C_SPOT]==1)||(LineBind[AB_SPOT]==1)){
lilac0112_1 35:a0421268ee95 143 if(((LinePulse[A_SPOT]==1)||(LinePulse[B_SPOT]==1)||(LinePulse[C_SPOT]==1)||(LinePulse[AB_SPOT]==1)||1)&&(LineReverseFlag==0)){
lilac0112_1 35:a0421268ee95 144 LineReverseFlag=1;
lilac0112_1 35:a0421268ee95 145 Line_timeout.attach(&LineReverse, 1.0);
lilac0112_1 35:a0421268ee95 146 }
lilac0112_1 35:a0421268ee95 147 if(LineReverseFlag==1){
lilac0112_1 35:a0421268ee95 148 vx=(LINE_RF*2)*((-1)*LineBind[A_SPOT] + LineBind[B_SPOT]);
lilac0112_1 35:a0421268ee95 149 vy=(LINE_RF*2)*( LineBind[C_SPOT] + (-1)*LineBind[AB_SPOT]);
lilac0112_1 35:a0421268ee95 150 }
lilac0112_1 35:a0421268ee95 151 else{
lilac0112_1 35:a0421268ee95 152 vx=0;
lilac0112_1 35:a0421268ee95 153 vy=0;
lilac0112_1 35:a0421268ee95 154 }
lilac0112_1 31:0b0f64831771 155 }
lilac0112_1 31:0b0f64831771 156 vs = data.OutputPID;
lilac0112_1 31:0b0f64831771 157 move(
lilac0112_1 31:0b0f64831771 158 vx*LineStop[X_AXIS] + (LINE_RF)*(vy!=0)*((-1)*(LineDir[A_SPOT]==0) + (LineDir[B_SPOT]==0)),
lilac0112_1 31:0b0f64831771 159 vy*LineStop[Y_AXIS] + (LINE_RF)*(vx!=0)*((LineDir[C_SPOT]==0) + (-1)*(LineDir[AB_SPOT]==0)),
lilac0112_1 31:0b0f64831771 160 vs
lilac0112_1 31:0b0f64831771 161 );
lilac0112_1 31:0b0f64831771 162 if(data.MotorFlag==1){
lilac0112_1 31:0b0f64831771 163 tx_motor();
lilac0112_1 31:0b0f64831771 164 data.MotorFlag=0;
lilac0112_1 31:0b0f64831771 165 }
lilac0112_1 28:8ac6c3c1e643 166 return;
lilac0112_1 28:8ac6c3c1e643 167 }
lilac0112_1 28:8ac6c3c1e643 168 void modeAttack2(void){
lilac0112_1 34:69bdf29a4442 169 if(data.IrFlag==1){
lilac0112_1 34:69bdf29a4442 170 ReadIr();
lilac0112_1 34:69bdf29a4442 171 data.IrFlag=0;
lilac0112_1 34:69bdf29a4442 172 }
lilac0112_1 34:69bdf29a4442 173 if(data.PidFlag==1){
lilac0112_1 34:69bdf29a4442 174 PidUpdate();
lilac0112_1 34:69bdf29a4442 175 data.PidFlag=0;
lilac0112_1 34:69bdf29a4442 176 }
lilac0112_1 34:69bdf29a4442 177 move(0,0,10);
lilac0112_1 34:69bdf29a4442 178 if(data.MotorFlag==1){
lilac0112_1 34:69bdf29a4442 179 tx_motor();
lilac0112_1 34:69bdf29a4442 180 data.MotorFlag=0;
lilac0112_1 34:69bdf29a4442 181 }
lilac0112_1 34:69bdf29a4442 182
lilac0112_1 28:8ac6c3c1e643 183 return;
lilac0112_1 28:8ac6c3c1e643 184 }
lilac0112_1 28:8ac6c3c1e643 185 void modeAttack3(void){
lilac0112_1 34:69bdf29a4442 186 if(data.KickFlag==1){
lilac0112_1 34:69bdf29a4442 187 DriveSolenoid();
lilac0112_1 34:69bdf29a4442 188 }
lilac0112_1 28:8ac6c3c1e643 189 return;
lilac0112_1 28:8ac6c3c1e643 190 }
lilac0112_1 28:8ac6c3c1e643 191 void modeAttack4(void){
lilac0112_1 28:8ac6c3c1e643 192 return;
lilac0112_1 28:8ac6c3c1e643 193 }
lilac0112_1 28:8ac6c3c1e643 194 void modeAttack5(void){
lilac0112_1 28:8ac6c3c1e643 195 return;
lilac0112_1 28:8ac6c3c1e643 196 }
lilac0112_1 28:8ac6c3c1e643 197 //Debug
lilac0112_1 28:8ac6c3c1e643 198 void modeDebug0(void){
lilac0112_1 28:8ac6c3c1e643 199 return;
lilac0112_1 28:8ac6c3c1e643 200 }
lilac0112_1 28:8ac6c3c1e643 201 void modeDebug1(void){//ChaseOnly NonLine
lilac0112_1 28:8ac6c3c1e643 202 int vx,vy,vs;
lilac0112_1 24:9fb74ea3c25d 203 if(data.IrFlag==1){
lilac0112_1 24:9fb74ea3c25d 204 ReadIr();
lilac0112_1 24:9fb74ea3c25d 205 data.IrFlag=0;
lilac0112_1 24:9fb74ea3c25d 206 }
lilac0112_1 27:769cb5a7ea37 207 if(data.PidFlag==1){
lilac0112_1 27:769cb5a7ea37 208 PidUpdate();
lilac0112_1 27:769cb5a7ea37 209 data.PidFlag=0;
lilac0112_1 27:769cb5a7ea37 210 }
lilac0112_1 28:8ac6c3c1e643 211
lilac0112_1 28:8ac6c3c1e643 212 vx = ir_move_val[data.irNotice][data.irPosition][IR_X];
lilac0112_1 28:8ac6c3c1e643 213 vy = ir_move_val[data.irNotice][data.irPosition][IR_Y];
lilac0112_1 28:8ac6c3c1e643 214 vs = data.OutputPID;
lilac0112_1 28:8ac6c3c1e643 215 if(data.irPosition<8){
lilac0112_1 28:8ac6c3c1e643 216 vx *= data.l_pow;
lilac0112_1 28:8ac6c3c1e643 217 vy *= data.l_pow;
lilac0112_1 28:8ac6c3c1e643 218 }
lilac0112_1 28:8ac6c3c1e643 219 else{
lilac0112_1 28:8ac6c3c1e643 220 vx *= data.s_pow;
lilac0112_1 28:8ac6c3c1e643 221 vy *= data.s_pow;
lilac0112_1 28:8ac6c3c1e643 222 }
lilac0112_1 28:8ac6c3c1e643 223
lilac0112_1 28:8ac6c3c1e643 224
lilac0112_1 28:8ac6c3c1e643 225 move(vx, vy, vs);
lilac0112_1 28:8ac6c3c1e643 226
lilac0112_1 28:8ac6c3c1e643 227 if(data.MotorFlag==1){
lilac0112_1 28:8ac6c3c1e643 228 tx_motor();
lilac0112_1 28:8ac6c3c1e643 229 data.MotorFlag=0;
lilac0112_1 28:8ac6c3c1e643 230 }
lilac0112_1 28:8ac6c3c1e643 231 return;
lilac0112_1 28:8ac6c3c1e643 232 }
lilac0112_1 28:8ac6c3c1e643 233 void modeDebug2(void){//LineRestoringForce
lilac0112_1 28:8ac6c3c1e643 234 int vx,vy,vs;
lilac0112_1 28:8ac6c3c1e643 235 uint8_t LineStop[2];
lilac0112_1 28:8ac6c3c1e643 236 int LineForce[2];
lilac0112_1 28:8ac6c3c1e643 237 if(data.IrFlag==1){
lilac0112_1 28:8ac6c3c1e643 238 ReadIr();
lilac0112_1 28:8ac6c3c1e643 239 data.IrFlag=0;
lilac0112_1 28:8ac6c3c1e643 240 }
lilac0112_1 28:8ac6c3c1e643 241 if(data.PidFlag==1){
lilac0112_1 28:8ac6c3c1e643 242 PidUpdate();
lilac0112_1 28:8ac6c3c1e643 243 data.PidFlag=0;
lilac0112_1 28:8ac6c3c1e643 244 }
lilac0112_1 27:769cb5a7ea37 245 vx = ir_move_val[data.irNotice][data.irPosition][IR_X];
lilac0112_1 27:769cb5a7ea37 246 vy = ir_move_val[data.irNotice][data.irPosition][IR_Y];
lilac0112_1 27:769cb5a7ea37 247 vs = data.OutputPID;
lilac0112_1 28:8ac6c3c1e643 248 if(data.irPosition<8){
lilac0112_1 28:8ac6c3c1e643 249 vx *= data.l_pow;
lilac0112_1 28:8ac6c3c1e643 250 vy *= data.l_pow;
lilac0112_1 28:8ac6c3c1e643 251 }
lilac0112_1 28:8ac6c3c1e643 252 else{
lilac0112_1 28:8ac6c3c1e643 253 vx *= data.s_pow;
lilac0112_1 28:8ac6c3c1e643 254 vy *= data.s_pow;
lilac0112_1 28:8ac6c3c1e643 255 }
lilac0112_1 28:8ac6c3c1e643 256
lilac0112_1 28:8ac6c3c1e643 257
lilac0112_1 28:8ac6c3c1e643 258 LineStop[X_AXIS] = (!((vx>0)&&((data.lnFlag[A_SPOT]==1)&&(1))))*(!((vx<0)&&((data.lnFlag[B_SPOT]==1)&&(1))));
lilac0112_1 28:8ac6c3c1e643 259 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 260
lilac0112_1 28:8ac6c3c1e643 261 LineForce[X_AXIS] = LineForce[Y_AXIS] = 0;
lilac0112_1 28:8ac6c3c1e643 262 if(((vx>0)&&((data.lnFlag[A_SPOT]==1)&&(1)))){
lilac0112_1 28:8ac6c3c1e643 263 LineForce[X_AXIS] = -LINE_RF;
lilac0112_1 28:8ac6c3c1e643 264 }
lilac0112_1 28:8ac6c3c1e643 265 if(((vx<0)&&((data.lnFlag[B_SPOT]==1)&&(1)))){
lilac0112_1 28:8ac6c3c1e643 266 LineForce[X_AXIS] = LINE_RF;
lilac0112_1 28:8ac6c3c1e643 267 }
lilac0112_1 28:8ac6c3c1e643 268 if(((vy>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==1)))){
lilac0112_1 28:8ac6c3c1e643 269 LineForce[Y_AXIS] = -LINE_RF;
lilac0112_1 28:8ac6c3c1e643 270 }
lilac0112_1 28:8ac6c3c1e643 271 if(((vy<0)&&((data.lnFlag[C_SPOT]==1)&&(1)))){
lilac0112_1 28:8ac6c3c1e643 272 LineForce[Y_AXIS] = LINE_RF;
lilac0112_1 28:8ac6c3c1e643 273 }
lilac0112_1 28:8ac6c3c1e643 274
lilac0112_1 28:8ac6c3c1e643 275 move(
lilac0112_1 28:8ac6c3c1e643 276 vx*LineStop[X_AXIS] + LineForce[X_AXIS],
lilac0112_1 28:8ac6c3c1e643 277 vy*LineStop[Y_AXIS] + LineForce[Y_AXIS],
lilac0112_1 28:8ac6c3c1e643 278 vs
lilac0112_1 28:8ac6c3c1e643 279 );
lilac0112_1 28:8ac6c3c1e643 280 if(data.MotorFlag==1){
lilac0112_1 28:8ac6c3c1e643 281 tx_motor();
lilac0112_1 28:8ac6c3c1e643 282 data.MotorFlag=0;
lilac0112_1 28:8ac6c3c1e643 283 }
lilac0112_1 28:8ac6c3c1e643 284 return;
lilac0112_1 28:8ac6c3c1e643 285 }
lilac0112_1 28:8ac6c3c1e643 286 void modeDebug3(void){//movesum
lilac0112_1 28:8ac6c3c1e643 287 int vx,vy,vs;
lilac0112_1 28:8ac6c3c1e643 288 uint8_t LineStop[2];
lilac0112_1 28:8ac6c3c1e643 289 static uint8_t moveLnFlag[4]={1, 1, 1, 1};
lilac0112_1 28:8ac6c3c1e643 290 static int moveLnlog[4];
lilac0112_1 28:8ac6c3c1e643 291 if(data.IrFlag==1){
lilac0112_1 28:8ac6c3c1e643 292 ReadIr();
lilac0112_1 28:8ac6c3c1e643 293 data.IrFlag=0;
lilac0112_1 28:8ac6c3c1e643 294 }
lilac0112_1 28:8ac6c3c1e643 295 if(data.PidFlag==1){
lilac0112_1 28:8ac6c3c1e643 296 PidUpdate();
lilac0112_1 28:8ac6c3c1e643 297 data.PidFlag=0;
lilac0112_1 28:8ac6c3c1e643 298 }
lilac0112_1 28:8ac6c3c1e643 299 vx = ir_move_val[data.irNotice][data.irPosition][IR_X];
lilac0112_1 28:8ac6c3c1e643 300 vy = ir_move_val[data.irNotice][data.irPosition][IR_Y];
lilac0112_1 28:8ac6c3c1e643 301 vs = data.OutputPID;
lilac0112_1 28:8ac6c3c1e643 302 if(data.irPosition<8){
lilac0112_1 28:8ac6c3c1e643 303 vx *= data.l_pow;
lilac0112_1 28:8ac6c3c1e643 304 vy *= data.l_pow;
lilac0112_1 28:8ac6c3c1e643 305 }
lilac0112_1 28:8ac6c3c1e643 306 else{
lilac0112_1 28:8ac6c3c1e643 307 vx *= data.s_pow;
lilac0112_1 28:8ac6c3c1e643 308 vy *= data.s_pow;
lilac0112_1 28:8ac6c3c1e643 309 }
lilac0112_1 27:769cb5a7ea37 310
lilac0112_1 27:769cb5a7ea37 311 data.motorlog[X_AXIS] += vx;
lilac0112_1 27:769cb5a7ea37 312 data.motorlog[Y_AXIS] += vy;
lilac0112_1 27:769cb5a7ea37 313
lilac0112_1 27:769cb5a7ea37 314 if((data.lnFlag[A_SPOT]==1)&&(1)){
lilac0112_1 27:769cb5a7ea37 315 moveLnFlag[A_SPOT]=0;
lilac0112_1 27:769cb5a7ea37 316 moveLnlog[A_SPOT] = data.motorlog[X_AXIS];
lilac0112_1 27:769cb5a7ea37 317 }
lilac0112_1 27:769cb5a7ea37 318 if((data.lnFlag[B_SPOT]==1)&&(1)){
lilac0112_1 27:769cb5a7ea37 319 moveLnFlag[B_SPOT]=0;
lilac0112_1 27:769cb5a7ea37 320 moveLnlog[B_SPOT] = data.motorlog[X_AXIS];
lilac0112_1 27:769cb5a7ea37 321 }
lilac0112_1 27:769cb5a7ea37 322 if((data.lnFlag[C_SPOT]==1)&&(1)){
lilac0112_1 27:769cb5a7ea37 323 moveLnFlag[C_SPOT]=0;
lilac0112_1 27:769cb5a7ea37 324 moveLnlog[C_SPOT] = data.motorlog[Y_AXIS];
lilac0112_1 27:769cb5a7ea37 325 }
lilac0112_1 27:769cb5a7ea37 326 if((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==1)){
lilac0112_1 27:769cb5a7ea37 327 moveLnFlag[AB_SPOT]=0;
lilac0112_1 27:769cb5a7ea37 328 moveLnlog[AB_SPOT] = data.motorlog[Y_AXIS];
lilac0112_1 27:769cb5a7ea37 329 }
lilac0112_1 27:769cb5a7ea37 330
lilac0112_1 27:769cb5a7ea37 331 if(((data.motorlog[X_AXIS]-moveLnlog[A_SPOT])<(-RELEASE_VAL))&&(moveLnFlag[A_SPOT]==0)) moveLnFlag[A_SPOT]=1;
lilac0112_1 27:769cb5a7ea37 332 if(((data.motorlog[X_AXIS]-moveLnlog[B_SPOT])>(RELEASE_VAL))&&(moveLnFlag[B_SPOT]==0)) moveLnFlag[B_SPOT]=1;
lilac0112_1 27:769cb5a7ea37 333 if(((data.motorlog[Y_AXIS]-moveLnlog[C_SPOT])>(RELEASE_VAL))&&(moveLnFlag[C_SPOT]==0)) moveLnFlag[C_SPOT]=1;
lilac0112_1 27:769cb5a7ea37 334 if(((data.motorlog[Y_AXIS]-moveLnlog[AB_SPOT])<(-RELEASE_VAL))&&(moveLnFlag[AB_SPOT]==0)) moveLnFlag[AB_SPOT]=1;
lilac0112_1 27:769cb5a7ea37 335
lilac0112_1 27:769cb5a7ea37 336 LineStop[X_AXIS] = (!((vx>0)&&(moveLnFlag[A_SPOT]==0)))*(!((vx<0)&&(moveLnFlag[B_SPOT]==0)));
lilac0112_1 27:769cb5a7ea37 337 LineStop[Y_AXIS] = (!((vy>0)&&(moveLnFlag[C_SPOT]==0)))*(!((vy<0)&&(moveLnFlag[AB_SPOT]==0)));
lilac0112_1 26:fbb03281fc7d 338
lilac0112_1 27:769cb5a7ea37 339 move(
lilac0112_1 27:769cb5a7ea37 340 vx*LineStop[X_AXIS],
lilac0112_1 27:769cb5a7ea37 341 vy*LineStop[Y_AXIS],
lilac0112_1 27:769cb5a7ea37 342 vs
lilac0112_1 26:fbb03281fc7d 343 );
lilac0112_1 27:769cb5a7ea37 344 if(data.MotorFlag==1){
lilac0112_1 27:769cb5a7ea37 345 tx_motor();
lilac0112_1 27:769cb5a7ea37 346 data.MotorFlag=0;
lilac0112_1 27:769cb5a7ea37 347 }
lilac0112_1 21:d69a8f3c76e1 348 return;
lilac0112_1 21:d69a8f3c76e1 349 }
lilac0112_1 28:8ac6c3c1e643 350 void modeDebug4(void){//solenoid
lilac0112_1 28:8ac6c3c1e643 351 if(data.KickFlag==1){
lilac0112_1 28:8ac6c3c1e643 352 DriveSolenoid();
lilac0112_1 28:8ac6c3c1e643 353 }
lilac0112_1 21:d69a8f3c76e1 354 return;
lilac0112_1 21:d69a8f3c76e1 355 }
lilac0112_1 28:8ac6c3c1e643 356 void modeDebug5(void){//cmpsTest
lilac0112_1 28:8ac6c3c1e643 357 if(data.IrFlag==1){
lilac0112_1 28:8ac6c3c1e643 358 ReadIr();
lilac0112_1 28:8ac6c3c1e643 359 data.IrFlag=0;
lilac0112_1 28:8ac6c3c1e643 360 }
lilac0112_1 28:8ac6c3c1e643 361 if(data.PidFlag==1){
lilac0112_1 28:8ac6c3c1e643 362 PidUpdate();
lilac0112_1 28:8ac6c3c1e643 363 data.PidFlag=0;
lilac0112_1 28:8ac6c3c1e643 364 }
lilac0112_1 28:8ac6c3c1e643 365 move(0,0,data.OutputPID);
lilac0112_1 28:8ac6c3c1e643 366 if(data.MotorFlag==1){
lilac0112_1 28:8ac6c3c1e643 367 tx_motor();
lilac0112_1 28:8ac6c3c1e643 368 data.MotorFlag=0;
lilac0112_1 28:8ac6c3c1e643 369 }
lilac0112_1 28:8ac6c3c1e643 370
lilac0112_1 19:342da3a5a474 371 return;
lilac0112_1 19:342da3a5a474 372 }