CatPot 2015-2016
ジャパンオープン用のメインプログラム
Dependencies: mbed AQM1602 HMC6352 PID
Diff: main_processing/strategy/strategy.cpp
- Revision:
- 18:3a42a931c95a
- Parent:
- 17:cc862ecf9812
- Child:
- 19:967207de919d
--- a/main_processing/strategy/strategy.cpp Mon Mar 14 08:56:04 2016 +0000
+++ b/main_processing/strategy/strategy.cpp Tue Mar 15 12:03:51 2016 +0000
@@ -2,732 +2,7 @@
#include "extern.h"
//Atk
-void modeAttack0(void){
- double ir_x, ir_y;
- int vx,vy,vs;
- uint8_t LineDir[4];
- uint8_t LineStop[2];
- //uint8_t IrRange[4];
- //uint8_t LineBind[4];
- if(sys.IrFlag==1){
- ReadIr();
- sys.IrFlag=0;
- }
- if(sys.PidFlag==1){
- Line_ticker.detach();
- PidUpdate();
- Line_ticker.attach(&ReadLine, 0.005);
- sys.PidFlag=0;
- }
- ir_x = ir_move_val_old[data.irNotice][data.irPosition][IR_X];
- ir_y = ir_move_val_old[data.irNotice][data.irPosition][IR_Y];
- if(data.irPosition<8){
- ir_x *= (double)(sys.l_pow);
- ir_y *= (double)(sys.l_pow);
- }
- else{
- ir_x *= (double)(sys.s_pow);
- ir_y *= (double)(sys.s_pow);
- }
- //Lineを考慮していないIrのみの値
- vx = ir_x;
- vy = ir_y;
- //Line検出方向を調べる
- LineDir[A_SPOT] = (!((vx>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==0))));
- LineDir[B_SPOT] = (!((vx<0)&&((data.lnFlag[A_SPOT]==0)&&(data.lnFlag[B_SPOT]==1))));
- LineDir[C_SPOT] = (!((vy<0)&&((data.lnFlag[C_SPOT]==1)&&(1))));
- LineDir[AB_SPOT] = (!((vy>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==1))));
-
- LineStop[X_AXIS] = LineDir[A_SPOT]*LineDir[B_SPOT];
- LineStop[Y_AXIS] = LineDir[C_SPOT]*LineDir[AB_SPOT];
-
- //Ir
- move(
- vx*LineStop[X_AXIS] + (LINE_RF)*(vy!=0)*((-1)*(LineDir[A_SPOT]==0) + (LineDir[B_SPOT]==0)),
- vy*LineStop[Y_AXIS] + (LINE_RF)*(vx!=0)*((LineDir[C_SPOT]==0) + (-1)*(LineDir[AB_SPOT]==0)),
- vs
- );
- if(sys.MotorFlag==1){
- tx_motor();
- sys.MotorFlag=0;
- }
- return;
-}
-uint8_t HmcResetFlag;
-void HmcReset(void){
- hmc_reset=HMC_RST;
- wait_us(100);
- hmc_reset=HMC_RUN;
-}
-uint8_t LineReverseFlag;
-void LineReverse(void){
- LineReverseFlag=0;
-}
-uint8_t LineSign[3];
-uint8_t LineFirst[2];
-uint8_t LinePriority[2];//0を後に，1を優先
-void LineClear_A(void){LineSign[A_SPOT]=0;data.lnFlag[A_SPOT]=0;}
-void LineClear_B(void){LineSign[B_SPOT]=0;data.lnFlag[B_SPOT]=0;}
-void LineClear_C(void){LineSign[C_SPOT]=0;data.lnFlag[C_SPOT]=0;}
-void LineCall_A(void){
- //
- LineSign[A_SPOT] = 1;
- if(LineSign[B_SPOT]==0){
- LineFirst[X_AXIS] = A_SPOT;
- }
- if((LineSign[A_SPOT]==1)&&(LineSign[B_SPOT]==1)){
- if(LineSign[C_SPOT]==0){
- LineFirst[Y_AXIS] = AB_SPOT;
- }
- }
- //
- if((Line[A_SPOT].read()==1)||(1)) data.lnFlag[A_SPOT]=1;
- //
- Line_timeout[A_SPOT].attach(&LineClear_A, LINE_DELAY);
-}
-void LineCall_B(void){
- //
- LineSign[B_SPOT] = 1;
- if(LineSign[A_SPOT]==0){
- LineFirst[X_AXIS] = B_SPOT;
- }
- if((LineSign[A_SPOT]==1)&&(LineSign[B_SPOT]==1)){
- if(LineSign[C_SPOT]==0){
- LineFirst[Y_AXIS] = AB_SPOT;
- }
- }
- //
- if((Line[B_SPOT].read()==1)||(1)) data.lnFlag[B_SPOT]=1;
- //
- Line_timeout[B_SPOT].attach(&LineClear_B, LINE_DELAY);
-}
-void LineCall_C(void){
- //
- LineSign[C_SPOT] = 1;
- if(!((LineSign[A_SPOT]==1)&&(LineSign[B_SPOT]==1))){
- LineFirst[Y_AXIS] = C_SPOT;
- }
- //
- if((Line[C_SPOT].read()==1)||(1)) data.lnFlag[C_SPOT]=1;
- //
- Line_timeout[C_SPOT].attach(&LineClear_C, LINE_DELAY);
-}
-void modeAttack1(void){
- double ir_x, ir_y;
- int vx,vy,vs;
- uint8_t LineDir[4];
- uint8_t LineStop[2];
- uint8_t IrRange[4];
- uint8_t LinePulse[4];
- uint8_t static LineBind[4];
- if(sys.KickOffFlag==1){
- LineBind[0]=0;
- LineBind[1]=0;
- LineBind[2]=0;
- LineBind[3]=0;
- LineReverseFlag=0;
-
- LineSign[A_SPOT]=0;
- LineSign[B_SPOT]=0;
- LineSign[C_SPOT]=0;
-
- data.lnFlag[A_SPOT]=0;
- data.lnFlag[B_SPOT]=0;
- data.lnFlag[C_SPOT]=0;
-
- sys.KickOffFlag=0;
- }
- if(sys.IrFlag==1){
- ReadIr();
- sys.IrFlag=0;
- }
- if(sys.PidFlag==1){
- //Line_ticker.detach();
- PidUpdate();
- //Line_ticker.attach(&ReadLine, 0.005);
- sys.PidFlag=0;
- }
- ir_x = ir_move_val_old[data.irNotice][data.irPosition][IR_X];
- ir_y = ir_move_val_old[data.irNotice][data.irPosition][IR_Y];
- if(data.irPosition<8){
- ir_x *= sys.l_pow;
- ir_y *= sys.l_pow;
- }
- else{
- ir_x *= sys.s_pow;
- ir_y *= sys.s_pow;
- }
-
- //Lineを考慮していないIrのみの値
- vx = ir_x;
- vy = ir_y;
- //Line検出方向を調べる
- LineDir[A_SPOT] = (!((vx>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==0))&&(1)));
- LineDir[B_SPOT] = (!((vx<0)&&((data.lnFlag[A_SPOT]==0)&&(data.lnFlag[B_SPOT]==1))&&(1)));
- LineDir[C_SPOT] = (!((vy<0)&&((data.lnFlag[C_SPOT]==1)&&(1 ))&&(1)));
- LineDir[AB_SPOT] = (!((vy>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==1))&&(1)));
-
- LineStop[X_AXIS] = LineDir[A_SPOT]*LineDir[B_SPOT];
- LineStop[Y_AXIS] = LineDir[C_SPOT]*LineDir[AB_SPOT];
-
- //Ir
- //strict
- /*
- IrRange[A_SPOT] = ((18<=data.irPosition)&&(data.irPosition<=19))||((8<=data.irPosition)&&(data.irPosition<=10))
- ||((6<=data.irPosition)&&(data.irPosition<=7))||(( 1)&&(data.irPosition<=1));
- IrRange[B_SPOT] = ((12<=data.irPosition)&&(data.irPosition<=16))||((2<=data.irPosition)&&(data.irPosition<=5));
- IrRange[C_SPOT] = ((15<=data.irPosition)&&(data.irPosition<=19))||((4<=data.irPosition)&&(data.irPosition<=7));
- IrRange[AB_SPOT] = ((9<=data.irPosition)&&(data.irPosition<=13))||(( 1)&&(data.irPosition<=3));
- */
- //sweet
- IrRange[A_SPOT] = ((19<=data.irPosition)&&(data.irPosition<=19))||((8<=data.irPosition)&&(data.irPosition<=9))
- ||((6<=data.irPosition)&&(data.irPosition<=7))||(( 1)&&(data.irPosition<=1));
- IrRange[B_SPOT] = ((13<=data.irPosition)&&(data.irPosition<=15))||((2<=data.irPosition)&&(data.irPosition<=5));
- IrRange[C_SPOT] = ((16<=data.irPosition)&&(data.irPosition<=18))||((4<=data.irPosition)&&(data.irPosition<=7));
- IrRange[AB_SPOT] = ((10<=data.irPosition)&&(data.irPosition<=12))||(( 1)&&(data.irPosition<=3));
-
-
- LinePulse[A_SPOT] = ((IrRange[A_SPOT]==1)&&(LineDir[A_SPOT]==0));
- LinePulse[B_SPOT] = ((IrRange[B_SPOT]==1)&&(LineDir[B_SPOT]==0));
- LinePulse[C_SPOT] = ((IrRange[C_SPOT]==1)&&(LineDir[C_SPOT]==0));
- LinePulse[AB_SPOT] = ((IrRange[AB_SPOT]==1)&&(LineDir[AB_SPOT]==0));
-
- LineBind[A_SPOT] = ((LinePulse[A_SPOT])||((IrRange[A_SPOT]==1)&&(LineBind[A_SPOT]==1)))&&(LineBind[B_SPOT]==0);
- LineBind[B_SPOT] = ((LinePulse[B_SPOT])||((IrRange[B_SPOT]==1)&&(LineBind[B_SPOT]==1)))&&(LineBind[A_SPOT]==0);
- LineBind[C_SPOT] = ((LinePulse[C_SPOT])||((IrRange[C_SPOT]==1)&&(LineBind[C_SPOT]==1)))&&(LineBind[AB_SPOT]==0);
- LineBind[AB_SPOT] = ((LinePulse[AB_SPOT])||((IrRange[AB_SPOT]==1)&&(LineBind[AB_SPOT]==1)))&&(LineBind[C_SPOT]==0);
- /*
- LineBind[A_SPOT] = ((IrRange[A_SPOT]==1)&&((LineDir[A_SPOT]==0)||(LineBind[A_SPOT]==1)));
- LineBind[B_SPOT] = ((IrRange[B_SPOT]==1)&&((LineDir[B_SPOT]==0)||(LineBind[B_SPOT]==1)));
- LineBind[C_SPOT] = ((IrRange[C_SPOT]==1)&&((LineDir[C_SPOT]==0)||(LineBind[C_SPOT]==1)));
- LineBind[AB_SPOT] = ((IrRange[AB_SPOT]==1)&&((LineDir[AB_SPOT]==0)||(LineBind[AB_SPOT]==1)));
- */
- vx = vx*LineStop[X_AXIS] + (LINE_RF)*(vy!=0)*((-1)*(LineDir[A_SPOT]==0) + (LineDir[B_SPOT]==0));
- vy = vy*LineStop[Y_AXIS] + (LINE_RF)*(vx!=0)*((LineDir[C_SPOT]==0) + (-1)*(LineDir[AB_SPOT]==0));
- vs = cmps_set.OutputPID;
- if((LineBind[A_SPOT]==1)||(LineBind[B_SPOT]==1)||(LineBind[C_SPOT]==1)||(LineBind[AB_SPOT]==1)){
- if(LineRaw>0){
- vx=(LINE_RF*2)*((-1)*IrRange[A_SPOT] + IrRange[B_SPOT]);
- vy=(LINE_RF*2)*( IrRange[C_SPOT] + (-1)*IrRange[AB_SPOT]);
- /*vx=(LINE_RF*2)*((-1)*(LineFirst[X_AXIS] == A_SPOT) + (LineFirst[X_AXIS] == B_SPOT));
- vy=(LINE_RF*2)*( (LineFirst[Y_AXIS] == C_SPOT) + (-1)*(LineFirst[Y_AXIS] == AB_SPOT));
- Line_timeout[A_SPOT].attach(&LineClear_A, LINE_DELAY);
- Line_timeout[B_SPOT].attach(&LineClear_B, LINE_DELAY);
- Line_timeout[C_SPOT].attach(&LineClear_C, LINE_DELAY);*/
- }
- else{
- vx=0;
- vy=0;
- }
- }
- if(LineRaw>0){
- Line_timeout[A_SPOT].attach(&LineClear_A, LINE_DELAY);
- Line_timeout[B_SPOT].attach(&LineClear_B, LINE_DELAY);
- Line_timeout[C_SPOT].attach(&LineClear_C, LINE_DELAY);
- }
- move(
- vx,
- vy,
- vs
- );
- if(sys.MotorFlag==1){
- tx_motor();
- sys.MotorFlag=0;
- }
- return;
-}
-void modeAttack2(void){
- double ir_x, ir_y;
- int vx,vy,vs;
- /*int LineForce[2];
- uint8_t LineDir[4];
- uint8_t LineOn[4];
- uint8_t LineReturn[4];
- uint8_t LineStop[2];
- uint8_t IrRange[4];
- uint8_t static LineBind[4];*/
- //buint8_t static spi_count;
- if(sys.KickOffFlag==1){
-
- sys.MotorFlag=0;
- sys.IrFlag=0;
- /*LineBind[0]=0;
- LineBind[1]=0;
- LineBind[2]=0;
- LineBind[3]=0;
- LineReverseFlag=0;
-
- LineSign[A_SPOT]=0;
- LineSign[B_SPOT]=0;
- LineSign[C_SPOT]=0;
-
- data.lnFlag[A_SPOT]=0;
- data.lnFlag[B_SPOT]=0;
- data.lnFlag[C_SPOT]=0;
-
- HmcResetFlag = 0;
- sys.UswFlag = 0;
- //spi_count=0;
- */
- hmc_reset=HMC_RUN;
- sys.KickFlag = 0;
-
- sys.KickOffFlag=0;
- //while((Sw[2].read()==1)&&(Sw[3].read()==1));//押して離すとスタート
- }
- if(sys.IrFlag==1){
- //LED[0] = 0;
- //LED[1] = 1;
- /*spi_count++;
- if(spi_count%10 == 0){
- ReadPing();
- }
- else{
- ReadIr();
- }
- if(spi_count==20) spi_count=0;
- */
- ReadIr();
- sys.IrFlag=0;
- }
- if(sys.PidFlag==1){
- PidUpdate();
- sys.PidFlag=0;
- }
- /*if(sys.UswFlag==1){
- ReadPing();
- sys.UswFlag=0;
- }*/
- /*
- if(HmcResetFlag==1){
- HmcReset();
- HmcResetFlag=0;
- }
- */
- //ir_x = ir_move_val_old[data.irNotice][data.irPosition][IR_X];
- //ir_y = ir_move_val_old[data.irNotice][data.irPosition][IR_Y];
- ir_x = ir_move_val[0][data.irNotice][data.irPosition][IR_X_DIR]+ir_move_val[0][data.irNotice][data.irPosition][IR_X_TURN];
- ir_y = ir_move_val[0][data.irNotice][data.irPosition][IR_Y_DIR]+ir_move_val[0][data.irNotice][data.irPosition][IR_Y_TURN];
-
- if(data.irPosition<8){
- ir_x *= sys.l_pow;
- ir_y *= sys.l_pow;
- }
- else{
- ir_x *= sys.s_pow;
- ir_y *= sys.s_pow;
- }
-
- //Lineを考慮していないIrのみの値
- vx = ir_x;
- vy = ir_y;
- /*
- if((data.irPosition==10)&&(vy>0)){
- vy += 0;//前進加速
- }
- if((data.irPosition==11)&&(vy>0)){
- vy += 0;//前進加速
- if(sys.KickFlag==1){
- DriveSolenoid();
- }
- }
- if((data.irPosition==12)&&(vy>0)){
- vy += 0;//前進加速
- }
- if((data.irPosition==1)&&(vy>0)){
- vy += 0;//前進加速
- }
- if((data.irPosition==2)&&(vy>0)){
- vy += 0;//前進加速
- }
-
- if((data.irPosition==17)&&(data.ping[L_PING]>data.ping[R_PING])){
- vx *= -1.0;//背後回り込みの左右判断
- }*/
- /*
- if((cmps_set.InputPID<(REFERENCE-30))||(cmps_set.InputPID>(REFERENCE+30))){
- vx = vx*(0.75);
- vy = vy*(0.75);
- }
- */
- //Lineを踏み始めた方向を調べる
- /*LineDir[A_SPOT] = (vx>0)&&((data.lnFlag[A_SPOT]==1)&&(1 ))&&(LineFirst[X_AXIS] == A_SPOT);
- LineDir[B_SPOT] = (vx<0)&&((data.lnFlag[B_SPOT]==1)&&(1 ))&&(LineFirst[X_AXIS] == B_SPOT);
- LineDir[C_SPOT] = (vy<0)&&((data.lnFlag[C_SPOT]==1)&&(1 ))&&(LineFirst[Y_AXIS] == C_SPOT);
- LineDir[AB_SPOT]= (vy>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==1))&&(LineFirst[Y_AXIS] == AB_SPOT);*/
- /*
- LineDir[A_SPOT] = (vx>0)&&((LineSign[A_SPOT]==1)&&(1 ))&&(LineFirst[X_AXIS] == A_SPOT);
- LineDir[B_SPOT] = (vx<0)&&((LineSign[B_SPOT]==1)&&(1 ))&&(LineFirst[X_AXIS] == B_SPOT);
- LineDir[C_SPOT] = (vy<0)&&((LineSign[C_SPOT]==1)&&(1 ))&&(LineFirst[Y_AXIS] == C_SPOT);
- LineDir[AB_SPOT]= (vy>0)&&((LineSign[A_SPOT]==1)&&(LineSign[B_SPOT]==1))&&(LineFirst[Y_AXIS] == AB_SPOT);
-
- //Irボールの方向
- //strict
-
- IrRange[A_SPOT] = ((18<=data.irPosition)&&(data.irPosition<=19))||((8<=data.irPosition)&&(data.irPosition<=10))
- ||((6<=data.irPosition)&&(data.irPosition<=7))||(( 1)&&(data.irPosition<=1));
- IrRange[B_SPOT] = ((12<=data.irPosition)&&(data.irPosition<=16))||((2<=data.irPosition)&&(data.irPosition<=5));
- IrRange[C_SPOT] = ((15<=data.irPosition)&&(data.irPosition<=19))||((4<=data.irPosition)&&(data.irPosition<=7));
- IrRange[AB_SPOT] = ((9<=data.irPosition)&&(data.irPosition<=13))||(( 1)&&(data.irPosition<=3));
- */
- //sweet
- /*IrRange[A_SPOT] = ((19<=data.irPosition)&&(data.irPosition<=19))||((8<=data.irPosition)&&(data.irPosition<=9))
- ||((6<=data.irPosition)&&(data.irPosition<=7))||(( 1)&&(data.irPosition<=1));
- IrRange[B_SPOT] = ((13<=data.irPosition)&&(data.irPosition<=15))||((2<=data.irPosition)&&(data.irPosition<=5));
- IrRange[C_SPOT] = ((16<=data.irPosition)&&(data.irPosition<=18))||((4<=data.irPosition)&&(data.irPosition<=7));
- IrRange[AB_SPOT] = ((10<=data.irPosition)&&(data.irPosition<=12))||(( 1)&&(data.irPosition<=3));*/
- /*
- //none
- if(data.irNotice==IR_NONE){
- IrRange[A_SPOT] = 0;
- IrRange[B_SPOT] = 0;
- IrRange[C_SPOT] = 0;
- IrRange[AB_SPOT] = 0;
- }
- //白線を踏み始めた方向とボールの方向が一致．(SelfHold)
- LineBind[A_SPOT] = (IrRange[A_SPOT]==1)&&((LineDir[A_SPOT]==1)||(LineBind[A_SPOT]==1));
- LineBind[B_SPOT] = (IrRange[B_SPOT]==1)&&((LineDir[B_SPOT]==1)||(LineBind[B_SPOT]==1));
- LineBind[C_SPOT] = (IrRange[C_SPOT]==1)&&((LineDir[C_SPOT]==1)||(LineBind[C_SPOT]==1));
- LineBind[AB_SPOT] = (IrRange[AB_SPOT]==1)&&((LineDir[AB_SPOT]==1)||(LineBind[AB_SPOT]==1));
-
- LineStop[X_AXIS] = (LineBind[A_SPOT]==0)*(LineBind[B_SPOT]==0);
- LineStop[Y_AXIS] = (LineBind[C_SPOT]==0)*(LineBind[AB_SPOT]==0);
-
- //白線踏んでる
- if(LineRaw>0){
- LineOn[A_SPOT] = (LineSign[A_SPOT]==1) &&(LineFirst[X_AXIS]==A_SPOT);
- LineOn[B_SPOT] = (LineSign[B_SPOT]==1) &&(LineFirst[X_AXIS]==B_SPOT);
- LineOn[C_SPOT] = (LineSign[C_SPOT]==1) &&(LineFirst[Y_AXIS]==C_SPOT);
- LineOn[AB_SPOT] = ((LineSign[A_SPOT]==1)&&(LineSign[B_SPOT]==1))&&(LineFirst[Y_AXIS]==AB_SPOT);
- //外側に向かう力を消す．
- //x
- if(((LineOn[A_SPOT]==1)&&(vx>0))||((LineOn[B_SPOT]==1)&&(vx<0))){
- vx=0;
- //yの力を加える．
- if(vy>0){vy += 10;}
- if(vy<0){vy -= 10;}
- }
- //y
- if(((LineOn[C_SPOT]==1)&&(vy<0))||((LineOn[AB_SPOT]==1)&&(vy>0))){
- vy=0;
- }
- //内側に向かう力を加える．
- LineReturn[A_SPOT] = (LineOn[A_SPOT]==1);
- if((LineReturn[A_SPOT]==1)&&(LineOn[AB_SPOT]==1)){
- if(LineOn[C_SPOT]==0){
- LineReturn[A_SPOT]=0;
- }
- else{
- LineReturn[A_SPOT]=1;
- }
- }
-
- LineReturn[B_SPOT] = (LineOn[B_SPOT]==1);
- if((LineReturn[B_SPOT]==1)&&(LineOn[AB_SPOT]==1)){
- if(LineOn[C_SPOT]==0){
- LineReturn[B_SPOT]=0;
- }
- else{
- LineReturn[B_SPOT]=1;
- }
- }
- LineReturn[C_SPOT] = (LineOn[C_SPOT]==1);
- if(LineReturn[C_SPOT]==1){
- LineReturn[A_SPOT]=0;
- LineReturn[B_SPOT]=0;
- }
- LineReturn[AB_SPOT] = (LineOn[AB_SPOT]==1);
-
- LineForce[X_AXIS] = (LINE_RF*2)*((-1)*(LineReturn[A_SPOT]==1) + ( 1)*(LineReturn[B_SPOT]==1)) +
- (LINE_RF*2)*(LineReturn[AB_SPOT]==1)*(LineReturn[A_SPOT]==0)*(LineReturn[B_SPOT]==0)
- *(( 1)*(data.ping[L_PING]<40) + (-1)*(data.ping[R_PING]<40)) +
- (LINE_RF*2)*(LineReturn[C_SPOT]==1)*(LineReturn[A_SPOT]==0)*(LineReturn[B_SPOT]==0)
- *(( 1)*(data.ping[L_PING]<40) + (-1)*(data.ping[R_PING]<40));
- LineForce[Y_AXIS] = (LINE_RF*2)*(( 1)*(LineReturn[C_SPOT]==1) + (-1)*(LineReturn[AB_SPOT]==1));
-
- Line_timeout[A_SPOT].attach(&LineClear_A, LINE_DELAY);
- Line_timeout[B_SPOT].attach(&LineClear_B, LINE_DELAY);
- Line_timeout[C_SPOT].attach(&LineClear_C, LINE_DELAY);
- }
- else{
- LineForce[X_AXIS] = 0;
- LineForce[Y_AXIS] = 0;
- }
- */
- //vx = vx*LineStop[X_AXIS] + LineForce[X_AXIS];
- //vy = vy*LineStop[Y_AXIS] + LineForce[Y_AXIS];
- vs = cmps_set.OutputPID;
- move(
- vx,
- vy,
- vs
- );
- /*move(
- 0,
- 0,
- 10
- );*/
- if(sys.MotorFlag==1){
- tx_motor();
- sys.MotorFlag=0;
- }
- return;
-}
-void modeAttack3(void){
- double ir_x, ir_y;
- int vx,vy,vs, LineForce[2];
- uint8_t LineDir[4];
- uint8_t LineOn[4];
- uint8_t LineReturn[4];
- uint8_t LineStop[2];
- uint8_t IrRange[4];
- uint8_t static LineBind[4];
- //buint8_t static spi_count;
- if(sys.KickOffFlag==1){
- LineBind[0]=0;
- LineBind[1]=0;
- LineBind[2]=0;
- LineBind[3]=0;
- LineReverseFlag=0;
-
- LineSign[A_SPOT]=0;
- LineSign[B_SPOT]=0;
- LineSign[C_SPOT]=0;
-
- data.lnFlag[A_SPOT]=0;
- data.lnFlag[B_SPOT]=0;
- data.lnFlag[C_SPOT]=0;
-
- LinePriority[X_AXIS]=0;
- LinePriority[Y_AXIS]=0;
-
- HmcResetFlag = 0;
- sys.UswFlag = 0;
- //spi_count=0;
-
- hmc_reset=HMC_RUN;
- sys.KickFlag = 0;
-
- sys.KickOffFlag=0;
- //while((Sw[2].read()==1)&&(Sw[3].read()==1));//押して離すとスタート
- }
- if(sys.IrFlag==1){
- /*spi_count++;
- if(spi_count%10 == 0){
- ReadPing();
- }
- else{
- ReadIr();
- }
- if(spi_count==20) spi_count=0;
- */
- ReadIr();
- sys.IrFlag=0;
- }
- if(sys.PidFlag==1){
- PidUpdate();
- sys.PidFlag=0;
- }
- if(sys.UswFlag==1){
- ReadPing();
- sys.UswFlag=0;
- }
- /*
- if(HmcResetFlag==1){
- HmcReset();
- HmcResetFlag=0;
- }
- */
- ir_x = ir_move_val_old[data.irNotice][data.irPosition][IR_X];
- ir_y = ir_move_val_old[data.irNotice][data.irPosition][IR_Y];
- if(data.irPosition<8){
- ir_x *= sys.l_pow;
- ir_y *= sys.l_pow;
- }
- else{
- ir_x *= sys.s_pow;
- ir_y *= sys.s_pow;
- }
-
- //Lineを考慮していないIrのみの値
- vx = ir_x;
- vy = ir_y;
-
- if((data.irPosition==10)&&(vy>0)){
- vy += 15;//前進加速
- }
- if((data.irPosition==11)&&(vy>0)){
- vy += 15;//前進加速
- if(sys.KickFlag==1){
- DriveSolenoid();
- }
- }
- if((data.irPosition==12)&&(vy>0)){
- vy += 15;//前進加速
- }
- if((data.irPosition==1)&&(vy>0)){
- vy += 25;//前進加速
- }
- if((data.irPosition==2)&&(vy>0)){
- vy += 25;//前進加速
- }
-
-
- if((data.irPosition==17)&&(data.ping[L_PING]>data.ping[R_PING])){
- vx *= -1.0;//背後回り込みの左右判断
- }
- /*
- if((cmps_set.InputPID<(REFERENCE-30))||(cmps_set.InputPID>(REFERENCE+30))){
- vx = vx*(0.75);
- vy = vy*(0.75);
- }
- */
- //Lineを踏み始めた方向を調べる
- /*LineDir[A_SPOT] = (vx>0)&&((data.lnFlag[A_SPOT]==1)&&(1 ))&&(LineFirst[X_AXIS] == A_SPOT);
- LineDir[B_SPOT] = (vx<0)&&((data.lnFlag[B_SPOT]==1)&&(1 ))&&(LineFirst[X_AXIS] == B_SPOT);
- LineDir[C_SPOT] = (vy<0)&&((data.lnFlag[C_SPOT]==1)&&(1 ))&&(LineFirst[Y_AXIS] == C_SPOT);
- LineDir[AB_SPOT]= (vy>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==1))&&(LineFirst[Y_AXIS] == AB_SPOT);*/
-
- LineDir[A_SPOT] = (vx>0)&&((LineSign[A_SPOT]==1)&&(1 ))&&(LineFirst[X_AXIS] == A_SPOT);
- LineDir[B_SPOT] = (vx<0)&&((LineSign[B_SPOT]==1)&&(1 ))&&(LineFirst[X_AXIS] == B_SPOT);
- LineDir[C_SPOT] = (vy<0)&&((LineSign[C_SPOT]==1)&&(1 ))&&(LineFirst[Y_AXIS] == C_SPOT);
- LineDir[AB_SPOT]= (vy>0)&&((LineSign[A_SPOT]==1)&&(LineSign[B_SPOT]==1))&&(LineFirst[Y_AXIS] == AB_SPOT);
-
- //Irボールの方向
- //strict
-
- IrRange[A_SPOT] = ((18<=data.irPosition)&&(data.irPosition<=19))||((8<=data.irPosition)&&(data.irPosition<=10))
- ||((6<=data.irPosition)&&(data.irPosition<=7))||(( 1)&&(data.irPosition<=1));
- IrRange[B_SPOT] = ((12<=data.irPosition)&&(data.irPosition<=16))||((2<=data.irPosition)&&(data.irPosition<=5));
- IrRange[C_SPOT] = ((15<=data.irPosition)&&(data.irPosition<=19))||((4<=data.irPosition)&&(data.irPosition<=7));
- IrRange[AB_SPOT] = ((9<=data.irPosition)&&(data.irPosition<=13))||(( 1)&&(data.irPosition<=3));
-
- //sweet
- /*IrRange[A_SPOT] = ((19<=data.irPosition)&&(data.irPosition<=19))||((8<=data.irPosition)&&(data.irPosition<=9))
- ||((6<=data.irPosition)&&(data.irPosition<=7))||(( 1)&&(data.irPosition<=1));
- IrRange[B_SPOT] = ((13<=data.irPosition)&&(data.irPosition<=15))||((2<=data.irPosition)&&(data.irPosition<=5));
- IrRange[C_SPOT] = ((16<=data.irPosition)&&(data.irPosition<=18))||((4<=data.irPosition)&&(data.irPosition<=7));
- IrRange[AB_SPOT] = ((10<=data.irPosition)&&(data.irPosition<=12))||(( 1)&&(data.irPosition<=3));*/
- //none
- if(data.irNotice==IR_NONE){
- IrRange[A_SPOT] = 0;
- IrRange[B_SPOT] = 0;
- IrRange[C_SPOT] = 0;
- IrRange[AB_SPOT] = 0;
- }
- //白線を踏み始めた方向とボールの方向が一致．(SelfHold)
- LineBind[A_SPOT] = (IrRange[A_SPOT]==1)&&((LineDir[A_SPOT]==1)||(LineBind[A_SPOT]==1));
- LineBind[B_SPOT] = (IrRange[B_SPOT]==1)&&((LineDir[B_SPOT]==1)||(LineBind[B_SPOT]==1));
- LineBind[C_SPOT] = (IrRange[C_SPOT]==1)&&((LineDir[C_SPOT]==1)||(LineBind[C_SPOT]==1));
- LineBind[AB_SPOT] = (IrRange[AB_SPOT]==1)&&((LineDir[AB_SPOT]==1)||(LineBind[AB_SPOT]==1));
-
- LineStop[X_AXIS] = 1;//(LineBind[A_SPOT]==0)*(LineBind[B_SPOT]==0);
- LineStop[Y_AXIS] = 1;//(LineBind[C_SPOT]==0)*(LineBind[AB_SPOT]==0);
-
- //白線踏んでる
- if(LineRaw>0){
- LineOn[A_SPOT] = (LineSign[A_SPOT]==1) &&(LineFirst[X_AXIS]==A_SPOT);
- LineOn[B_SPOT] = (LineSign[B_SPOT]==1) &&(LineFirst[X_AXIS]==B_SPOT);
- LineOn[C_SPOT] = (LineSign[C_SPOT]==1) &&(LineFirst[Y_AXIS]==C_SPOT);
- LineOn[AB_SPOT] = ((LineSign[A_SPOT]==1)&&(LineSign[B_SPOT]==1))&&(LineFirst[Y_AXIS]==AB_SPOT);
- //外側に向かう力を消す．
- //x
- if(((LineOn[A_SPOT]==1)&&(vx>0))||((LineOn[B_SPOT]==1)&&(vx<0))){
-
- if(LinePriority[Y_AXIS]==0){
- LinePriority[X_AXIS]=1;
- LinePriority[Y_AXIS]=0;
- }
- vx=0;
- //yの力を加える．
- if(vy>0){vy += 10;}
- if(vy<0){vy -= 10;}
- }
- //y
- if(((LineOn[C_SPOT]==1)&&(vy<0))||((LineOn[AB_SPOT]==1)&&(vy>0))){
- if(LinePriority[X_AXIS]==0){
- LinePriority[X_AXIS]=0;
- LinePriority[Y_AXIS]=1;
- }
- if((LinePriority[X_AXIS]==1)&&(LineOn[AB_SPOT]==1)){
- LinePriority[X_AXIS]=0;
- LinePriority[Y_AXIS]=1;
- }
- vy=0;
- }
- //内側に向かう力を加える．
- LineReturn[A_SPOT] = (LineOn[A_SPOT]==1);
- /*if((LineReturn[A_SPOT]==1)&&(LineOn[AB_SPOT]==1)){
- if(LineOn[C_SPOT]==0){
- LineReturn[A_SPOT]=0;
- }
- else{
- LineReturn[A_SPOT]=1;
- }
- }*/
-
- LineReturn[B_SPOT] = (LineOn[B_SPOT]==1);
- /*if((LineReturn[B_SPOT]==1)&&(LineOn[AB_SPOT]==1)){
- if(LineOn[C_SPOT]==0){
- LineReturn[B_SPOT]=0;
- }
- else{
- LineReturn[B_SPOT]=1;
- }
- }*/
- LineReturn[C_SPOT] = (LineOn[C_SPOT]==1);
- /*if(LineReturn[C_SPOT]==1){
- LineReturn[A_SPOT]=0;
- LineReturn[B_SPOT]=0;
- }*/
- LineReturn[AB_SPOT] = (LineOn[AB_SPOT]==1);
-
- LineForce[X_AXIS] = (LINE_RF*2)*(LinePriority[X_AXIS])*((-1)*(LineReturn[A_SPOT]==1) + ( 1)*(LineReturn[B_SPOT]==1)) +
- (LINE_RF*2)*(LineReturn[AB_SPOT]==1)*(( 1)*(data.ping[L_PING]<40) + (-1)*(data.ping[R_PING]<40)) +
- (LINE_RF*2)*(LineReturn[C_SPOT]==1)*(( 1)*(data.ping[L_PING]<40) + (-1)*(data.ping[R_PING]<40));
- LineForce[Y_AXIS] = (LINE_RF*2)*(LinePriority[Y_AXIS])*(( 1)*(LineReturn[C_SPOT]==1) + (-1)*(LineReturn[AB_SPOT]==1));
- /*
- LineForce[X_AXIS] = (LINE_RF*2)*((-1)*(LineReturn[A_SPOT]==1) + ( 1)*(LineReturn[B_SPOT]==1)) +
- (LINE_RF*2)*(LineReturn[AB_SPOT]==1)*(LineReturn[A_SPOT]==0)*(LineReturn[B_SPOT]==0)
- *(( 1)*(data.ping[L_PING]<30) + (-1)*(data.ping[R_PING]<30)) +
- (LINE_RF*2)*(LineReturn[C_SPOT]==1)*(LineReturn[A_SPOT]==0)*(LineReturn[B_SPOT]==0)
- *(( 1)*(data.ping[L_PING]<30) + (-1)*(data.ping[R_PING]<30));
- LineForce[Y_AXIS] = (LINE_RF*2)*(( 1)*(LineReturn[C_SPOT]==1) + (-1)*(LineReturn[AB_SPOT]==1));
- */
- Line_timeout[A_SPOT].attach(&LineClear_A, LINE_DELAY);
- Line_timeout[B_SPOT].attach(&LineClear_B, LINE_DELAY);
- Line_timeout[C_SPOT].attach(&LineClear_C, LINE_DELAY);
- }
- else{
- LineForce[X_AXIS] = 0;
- LineForce[Y_AXIS] = 0;
-
- LinePriority[X_AXIS]=0;
- LinePriority[Y_AXIS]=0;
- }
-
- vx = vx*LineStop[X_AXIS] + LineForce[X_AXIS];
- vy = vy*LineStop[Y_AXIS] + LineForce[Y_AXIS];
- vs = cmps_set.OutputPID;
- move(
- vx,
- vy,
- vs
- );
- /*move(
- 10,
- 0,
- 0
- );*/
- if(sys.MotorFlag==1){
- tx_motor();
- sys.MotorFlag=0;
- }
- if(sys.stopflag==1){
- //停止処理
- }
- return;
-}
+
void LineJudgeSlow(double pow_x, double pow_y, double *x, double *y){
uint8_t LineState;
uint8_t LineSum;
@@ -826,27 +101,31 @@
data.ReturnDir[X_LINE]=L_LINE;
- if(pow_x<0){
+ /*if(pow_x<0){
*x = 0;
data.lnStop[X_LINE]=1;
}
else{
*x = -LineReturn[4];
data.lnStop[X_LINE]=0;
- }
+ }*/
+ *x = -LineReturn[4];
+ data.lnStop[X_LINE]=0;
}
if((LinePingState[L_PING]==1)&&(LinePingState[R_PING]==0)){
data.ReturnDir[X_LINE]=R_LINE;
- if(pow_x>0){
+ /*if(pow_x>0){
*x = 0;
data.lnStop[X_LINE]=1;
}
else{
*x = LineReturn[4];
data.lnStop[X_LINE]=0;
- }
+ }*/
+ *x = LineReturn[4];
+ data.lnStop[X_LINE]=0;
}
if((LinePingState[L_PING]==1)&&(LinePingState[R_PING]==1)){
*x = 0;
@@ -934,9 +213,13 @@
data.lnStop[Y_LINE]=1;
}
}
-void LineJudgeReset(void){
- if((data.lnRaw==0)&&(data.lnHold==7)){
+void LineJudgeReset(double pow_x, double pow_y, double *x, double *y){
+ //static uint8_t NewLineCorner[4]={LINE_EMPTY, LINE_EMPTY, LINE_EMPTY, LINE_EMPTY};
+ //static uint8_t LastLineCorner[4]={LINE_EMPTY, LINE_EMPTY, LINE_EMPTY, LINE_EMPTY};
+
+ if((/*data.lnRaw==0*/1)&&(data.lnHold==7)){
if(data.FieldSpot==LINE_INSIDE){
+
data.lnCorner[L_LINE] = (data.ping[L_PING]<OutToWall[X_PING]);
data.lnCorner[R_LINE] = (data.ping[R_PING]<OutToWall[X_PING]);
data.lnCorner[F_LINE] = (data.ping[F_PING]<OutToWall[Y_PING]);
@@ -947,19 +230,47 @@
(data.lnCorner[F_LINE])||
(data.lnCorner[B_LINE])
){
+
data.NonWall[L_LINE] = (data.ping[L_PING]>WhiteToWall[X_PING]);
data.NonWall[R_LINE] = (data.ping[R_PING]>WhiteToWall[X_PING]);
data.NonWall[F_LINE] = (data.ping[F_PING]>WhiteToWall[Y_PING]);
data.NonWall[B_LINE] = (data.ping[B_PING]>WhiteToWall[Y_PING]);
+
+
+
+ /*LastLineCorner[L_LINE]=NewLineCorner[L_LINE];
+ LastLineCorner[R_LINE]=NewLineCorner[R_LINE];
+ LastLineCorner[F_LINE]=NewLineCorner[F_LINE];
+ LastLineCorner[B_LINE]=NewLineCorner[B_LINE];
+
+ NewLineCorner[L_LINE]=data.lnCorner[L_LINE];
+ NewLineCorner[R_LINE]=data.lnCorner[R_LINE];
+ NewLineCorner[F_LINE]=data.lnCorner[F_LINE];
+ NewLineCorner[B_LINE]=data.lnCorner[B_LINE];
+
+ if(
+ (LastLineCorner[L_LINE]==NewLineCorner[L_LINE])&&
+ (LastLineCorner[R_LINE]==NewLineCorner[R_LINE])&&
+ (LastLineCorner[F_LINE]==NewLineCorner[F_LINE])&&
+ (LastLineCorner[B_LINE]==NewLineCorner[B_LINE])
+ ){
+ data.lnRepeat++;
+ }
+ else{
+ data.lnRepeat=0;
+ }*/
+
data.FieldSpot = LINE_OUTSIDE;
LineLiberate();
}
}
else if(data.FieldSpot==LINE_OUTSIDE){
- data.FieldSpot = LINE_INSIDE;
- data.NonWall[L_LINE] = data.NonWall[R_LINE] = data.NonWall[F_LINE] = data.NonWall[B_LINE] = 0;
- LineLiberate();
- LineRankClear();
+ if(data.lnRaw==0){
+ data.FieldSpot = LINE_INSIDE;
+ data.NonWall[L_LINE] = data.NonWall[R_LINE] = data.NonWall[F_LINE] = data.NonWall[B_LINE] = 0;
+ LineLiberate();
+ LineRankClear();
+ }
}
}
if(data.FieldSpot == LINE_OUTSIDE){
@@ -993,16 +304,6 @@
LineLiberate();
LineRankClear();
}
- /*if(
- (data.ping[L_PING]>=WhiteToWall[X_PING])&&
- (data.ping[R_PING]>=WhiteToWall[X_PING])&&
- (data.ping[F_PING]>=WhiteToWall[Y_PING])&&
- (data.ping[B_PING]>=WhiteToWall[Y_PING])
- ){
- data.FieldSpot = LINE_INSIDE;
- LineLiberate();
- LineRankClear();
- }*/
}
if((data.FieldSpot == LINE_INSIDE)&&(0<data.lnHold)&&(data.lnHold<7)&&(data.lnRaw==0)){
if(
@@ -1025,11 +326,57 @@
((data.lnOrder[0]==A_SPOT)&&(data.lnOrder[1]==B_SPOT))||
((data.lnOrder[0]==B_SPOT)&&(data.lnOrder[1]==A_SPOT))
)
+ /*||
+ (
+ (data.ping[L_PING]>=GoalEdgeToWall[X_PING])&&
+ (data.ping[R_PING]>=GoalEdgeToWall[X_PING])
+ )*/
){
data.NonWall[L_LINE] = data.NonWall[R_LINE] = data.NonWall[F_LINE] = data.NonWall[B_LINE] = 0;
LineLiberate();
}
}
+ /*
+ if(
+ (data.irNotice==IR_NONE)||
+ (data.irNotice==IR_FAR)||
+ (
+ (data.ping[L_PING]>=GoalEdgeToWall[X_PING])&&
+ (data.ping[R_PING]>=GoalEdgeToWall[X_PING])
+ )
+ ){
+ data.lnRepeat = 0;
+
+ data.lnCorner[L_LINE]=LINE_EMPTY;
+ data.lnCorner[R_LINE]=LINE_EMPTY;
+ data.lnCorner[F_LINE]=LINE_EMPTY;
+ data.lnCorner[B_LINE]=LINE_EMPTY;
+ }
+ data.lnRepeat=0;
+ if(data.lnRepeat>0){
+ if(
+ ((pow_x>=0)&&(data.lnCorner[R_LINE]))||
+ ((pow_x<0)&&(data.lnCorner[L_LINE]))
+ ){
+ data.lnStay[X_LINE]=0;
+ }
+ else{
+ data.lnStay[X_LINE]=1;
+ }
+ if(
+ ((pow_y>=0)&&(data.lnCorner[F_LINE]))||
+ ((pow_y<0)&&(data.lnCorner[B_LINE]))
+ ){
+ data.lnStay[Y_LINE]=0;
+ }
+ else{
+ data.lnStay[Y_LINE]=1;
+ }
+ }
+ else{
+ data.lnStay[X_LINE]=data.lnStay[Y_LINE]=1;
+ }*/
+
}
void modeAttack4(void){
double ir_x_dir, ir_y_dir;
@@ -1047,10 +394,18 @@
if(sys.KickOffFlag==1){
- sys.IrBlind=1;
+ sys.IrBlind=0;
sys.LineBlind=0;
sys.PingBlind=0;
+ sys.ir_pow_table = 0;
+
+ data.lnCorner[L_LINE]=data.lnCorner[R_LINE]=data.lnCorner[F_LINE]=data.lnCorner[B_LINE]=LINE_EMPTY;
+ data.lnRepeat = 0;
+
+ //data.lnStay[X_LINE]=data.lnStay[Y_LINE]=1;
+ data.lnStop[X_LINE]=data.lnStop[Y_LINE]=1;
+
data.FieldSpot = LINE_INSIDE;
LineLiberate();
LineRankClear();
@@ -1066,10 +421,34 @@
data.lnRaw = LineRaw;
data.lnHold = LineHold;
+ if(data.ping[B_PING]<=30){
+ sys.ir_pow_table=1;
+ ir_x_dir = ir_move_val[1][data.irNotice][data.irPosition][IR_X_DIR];
+ ir_y_dir = ir_move_val[1][data.irNotice][data.irPosition][IR_Y_DIR];
+ ir_x_turn = ir_move_val[1][data.irNotice][data.irPosition][IR_X_TURN];
+ ir_y_turn = ir_move_val[1][data.irNotice][data.irPosition][IR_Y_TURN];
+ }
+ else{
+ sys.ir_pow_table=0;
+ ir_x_dir = ir_move_val[0][data.irNotice][data.irPosition][IR_X_DIR];
+ ir_y_dir = ir_move_val[0][data.irNotice][data.irPosition][IR_Y_DIR];
+ ir_x_turn = ir_move_val[0][data.irNotice][data.irPosition][IR_X_TURN];
+ ir_y_turn = ir_move_val[0][data.irNotice][data.irPosition][IR_Y_TURN];
+ }
+ /*
+ ir_x_dir = ir_move_val[sys.ir_pow_table][data.irNotice][data.irPosition][IR_X_DIR];
+ ir_y_dir = ir_move_val[sys.ir_pow_table][data.irNotice][data.irPosition][IR_Y_DIR];
+ ir_x_turn = ir_move_val[sys.ir_pow_table][data.irNotice][data.irPosition][IR_X_TURN];
+ ir_y_turn = ir_move_val[sys.ir_pow_table][data.irNotice][data.irPosition][IR_Y_TURN];
+ */
+ /*
ir_x_dir = ir_move_val[0][data.irNotice][data.irPosition][IR_X_DIR];
ir_y_dir = ir_move_val[0][data.irNotice][data.irPosition][IR_Y_DIR];
ir_x_turn = ir_move_val[0][data.irNotice][data.irPosition][IR_X_TURN];
ir_y_turn = ir_move_val[0][data.irNotice][data.irPosition][IR_Y_TURN];
+ */
+
+ if(sys.IrBlind==1) data.irNotice=IR_NONE;
if(data.irNotice==IR_CLOSER){
ir_pow = sys.s_pow;
}
@@ -1082,7 +461,12 @@
else{//data.irNotice==IR_NONE
ir_pow = 0;
}
- if(sys.IrBlind==1) data.irNotice=IR_NONE;
+
+
+ if((data.ping[R_PING]<data.ping[L_PING])&&(data.irPosition==17)){
+ ir_x_turn = -ir_x_turn;
+ ir_y_turn = -ir_y_turn;
+ }
ir_x = (ir_x_dir + ir_x_turn);
ir_y = (ir_y_dir + ir_y_turn);
@@ -1101,14 +485,14 @@
data.FieldSpot = LINE_INSIDE;
}
else{
- LineJudgeReset();
+ LineJudgeReset(ir_x, ir_y, &LineSlowPower[X_LINE], &LineSlowPower[Y_LINE]);
- //LineJudgeSlow(ir_x, ir_y, &LineSlowPower[X_LINE], &LineSlowPower[Y_LINE]);
- LineJudgeReturn(ir_x, ir_y, &LineReturnPower[X_LINE], &LineReturnPower[Y_LINE]);
-
+ LineJudgeSlow(ir_x, ir_y, &LineSlowPower[X_LINE], &LineSlowPower[Y_LINE]);
+ LineJudgeReturn(ir_x*LineSlowPower[X_LINE], ir_y*LineSlowPower[Y_LINE], &LineReturnPower[X_LINE], &LineReturnPower[Y_LINE]);
+ /*
LineSlowPower[X_LINE] = 1.0;
LineSlowPower[Y_LINE] = 1.0;
- /*
+
LineReturnPower[X_LINE] = 0.0;
LineReturnPower[Y_LINE] = 0.0;
@@ -1125,8 +509,8 @@
//else LED = 0xA;
//LED = LineHold;
- vx = (ir_pow*ir_x)*data.lnStop[X_LINE]*LineSlowPower[X_LINE] + LineReturnPower[X_LINE];
- vy = (ir_pow*ir_y)*data.lnStop[Y_LINE]*LineSlowPower[Y_LINE] + LineReturnPower[Y_LINE];
+ vx = (ir_pow*ir_x)*data.lnStop[X_LINE]*(/*data.lnStay[X_LINE]*/1)*LineSlowPower[X_LINE] + LineReturnPower[X_LINE];
+ vy = (ir_pow*ir_y)*data.lnStop[Y_LINE]*(/*data.lnStay[X_LINE]*/1)*LineSlowPower[Y_LINE] + LineReturnPower[Y_LINE];
vs = cmps_set.OutputPID;
move(
vx,