CatPot 2015-2016 / Mbed 2 deprecated CatPot_Main_T_2v00

Main Program

Dependencies:   mbed AQM1602 HMC6352 PID

main_processing/strategy/strategy.cpp@37:ae5661306900, 2016-02-03 (annotated)

Committer:
lilac0112_1
Date:
Wed Feb 03 09:14:49 2016 +0000
Revision:
37:ae5661306900
Parent:
36:50092d488f78
Child:
38:a7eb15b1f813
GoodLine BadTurn

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