CatPot 2015-2016 / Mbed 2 deprecated CatPot_Main_T_2v00

Main Program

Dependencies:   mbed AQM1602 HMC6352 PID

main_processing/strategy/strategy.cpp@38:a7eb15b1f813, 2016-02-03 (annotated)

Committer:
lilac0112_1
Date:
Wed Feb 03 11:31:03 2016 +0000
Revision:
38:a7eb15b1f813
Parent:
37:ae5661306900
Child:
40:f68474b1d5e7
Good line Good turn Good power

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