CatPot 2015-2016
/
CatPot_2v10_T_Main
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: mbed AQM1602 HMC6352 PID
Diff: main_processing/strategy/LineProcess.cpp
- Revision:
- 30:5998ba42237e
- Parent:
- 29:e8bafe48aa90
- Child:
- 33:aa115c30892e
diff -r e8bafe48aa90 -r 5998ba42237e main_processing/strategy/LineProcess.cpp
--- a/main_processing/strategy/LineProcess.cpp Tue Mar 22 07:01:10 2016 +0000
+++ b/main_processing/strategy/LineProcess.cpp Wed Mar 23 11:25:22 2016 +0000
@@ -78,12 +78,48 @@
}
}
else if(LineSum==0){
+ ////超音波による減速
if(pow_x>=0){
if((data.ping[R_PING]<GoalEdgeToWall[X_PING])&&((*px)>LineDecrease[LineState])) (*px)=LineDecrease[LineState];
}
else{
if((data.ping[L_PING]<GoalEdgeToWall[X_PING])&&((*px)>LineDecrease[LineState])) (*px)=LineDecrease[LineState];
}
+
+ if(pow_y>=0){
+ //if((data.ping[F_PING]<15)&&((*py)>LineDecrease[LineState])) (*py)=LineDecrease[LineState];
+ }
+ else{
+ if((data.ping[B_PING]<15)&&((*py)>LineDecrease[LineState])) (*py)=LineDecrease[LineState];
+ }
+ }
+
+ if((data.lnStayNow[X_LINE]==1)&&((*px)>15)){
+ (*px)=15;
+ }
+ if((data.lnStayNow[Y_LINE]==1)&&((*py)>15)){
+ (*py)=15;
+ }
+
+ ////超音波による減速
+ if(pow_x>=0){
+ if((data.ping[R_PING]<GoalEdgeToWall[X_PING])&&((*px)>20)){
+ (*px)=20;
+ if((*py)>30) (*py)=30;
+ }
+ }
+ else{
+ if((data.ping[L_PING]<GoalEdgeToWall[X_PING])&&((*px)>20)){
+ (*px)=20;
+ if((*py)>30) (*py)=30;
+ }
+ }
+
+ if(pow_y>=0){
+ if((data.ping[F_PING]<WhiteToWallPlus[Y_PING])&&((*py)>25)) (*py)=25;
+ }
+ else{
+ if((data.ping[B_PING]<WhiteToWallPlus[Y_PING])&&((*py)>25)) (*py)=25;
}
*x=1;
@@ -599,26 +635,35 @@
}
void LineJudgeReset2(double pow_x, double pow_y, double *x, double *y){
- if(
- (data.lnRawOrderLog2[0]==data.lnRawOrderLog1[0])&&
- (data.lnRawOrderLog2[1]==data.lnRawOrderLog1[1])&&
- //(data.lnRawOrderLog2[2]==data.lnRawOrderLog1[2])
- (data.lnRawOrderLog2[0]!=LINE_EMPTY)&&
- (data.lnRawOrderLog2[1]!=LINE_EMPTY)
- ){
+
+ if(data.lnRepeat==0){
+ data.lnStayNow[X_LINE]=0;
+ data.lnStayNow[Y_LINE]=0;
+ }
+ data.lnStay[X_LINE]=1;
+ data.lnStay[Y_LINE]=1;
+
+ if((data.irNotice==IR_FAR)&&(data.lnRepeat>1)){
data.lnRepeat=1;
}
- else{
- data.lnRepeat=0;
- }
if(
(data.irNotice==IR_NONE)||
- (data.irNotice==IR_FAR)||
- (data.irValPhase[IR_SHORT]>=DIS_5)||
+ //(data.irNotice==IR_FAR)||
+ //(data.irLastPosition<=7)||
(
- (data.ping[L_PING]>=GoalEdgeToWall[X_PING])&&
- (data.ping[R_PING]>=GoalEdgeToWall[X_PING])
+ (!(
+ (data.irPosition==data.irLastPosition)||
+ (data.irPosition==(ir_posi_s[(data.irLastPosition-8+24+1)%12]))||
+ (data.irPosition==(ir_posi_s[(data.irLastPosition-8+24-1)%12]))||
+ (data.irPosition==(ir_posi_s[(data.irLastPosition-8+24+2)%12]))||
+ (data.irPosition==(ir_posi_s[(data.irLastPosition-8+24-2)%12]))||
+ (data.irPosition==(ir_posi_s[(data.irLastPosition-8+24+3)%12]))||
+ (data.irPosition==(ir_posi_s[(data.irLastPosition-8+24-3)%12]))
+ ))&&(
+ (data.irLastNotice==IR_CLOSE)||
+ (data.irLastNotice==IR_CLOSER)
+ )
)
){
data.lnRepeat = 0;
@@ -627,28 +672,189 @@
}
//data.lnRepeat=0;
//if((data.lnRepeat==1)&&((data.lnRawOrder[0]==LINE_EMPTY)&&(data.lnRawOrder[1]==LINE_EMPTY)&&(data.lnRawOrder[2]==LINE_EMPTY))){
- if((data.lnRepeat==1)&&((data.lnRawOrder[0]==LINE_EMPTY))){
+ if(
+ (data.lnRepeat>=1)&&
+ (data.lnRawOrder[0]==LINE_EMPTY)/*&&
+ (
+ (
+ (data.irPosition==data.irLastPosition)||
+ (data.irPosition==(data.irLastPosition+1))||
+ (data.irPosition==(data.irLastPosition-1))||
+ (data.irPosition==(data.irLastPosition+11))||
+ (data.irPosition==(data.irLastPosition-11))
+ )&&
+ (
+ (data.irNotice==IR_CLOSE)||
+ (data.irNotice==IR_CLOSER)
+ )
+ )*/
+ ){
//y
if(
((pow_y>0)&&((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]==B_SPOT)&&(1)))||
- ((pow_y>0)&&((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]==A_SPOT)&&(1)))||
+ ((pow_y>0)&&((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]==A_SPOT)&&(1)))
+ ){
+ if(data.ping[F_PING]<WhiteToWallPlus[Y_PING]){
+ data.lnStay[Y_LINE]=0;
+ }
+ else{
+ data.lnStay[Y_LINE]=1;
+ }
+ data.lnStay[Y_LINE]=0;
+ data.lnStayNow[Y_LINE]=1;
+ }
+ else{
+ data.lnStay[Y_LINE]=1;
+ }
+ if(
((pow_y<0)&&((data.lnRawOrderLog1[0]==C_SPOT)&&(1)&&(1)))
){
+ if(data.ping[B_PING]<WhiteToWallPlus[Y_PING]){
+ data.lnStay[Y_LINE]=0;
+ }
+ else{
+ data.lnStay[Y_LINE]=1;
+ }
data.lnStay[Y_LINE]=0;
+ data.lnStayNow[Y_LINE]=1;
}
else{
data.lnStay[Y_LINE]=1;
}
//x
if(
- ((pow_x>0)&&((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]==C_SPOT)&&(1)))||
+ ((pow_x>0)&&((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]==C_SPOT)&&(1)))
+ ){
+ if(data.ping[R_PING]<WhiteToWallPlus[X_PING]){
+ data.lnStay[X_LINE]=0;
+ }
+ else{
+ data.lnStay[X_LINE]=1;
+ }
+ data.lnStay[X_LINE]=0;
+ data.lnStayNow[X_LINE]=1;
+ }
+ else{
+ data.lnStay[X_LINE]=1;
+ }
+ if(
((pow_x<0)&&((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]==C_SPOT)&&(1)))
){
+ if(data.ping[L_PING]<WhiteToWallPlus[X_PING]){
+ data.lnStay[X_LINE]=0;
+ }
+ else{
+ data.lnStay[X_LINE]=1;
+ }
data.lnStay[X_LINE]=0;
+ data.lnStayNow[X_LINE]=1;
}
else{
data.lnStay[X_LINE]=1;
}
+ if((data.irNotice==IR_CLOSE)||(data.irNotice==IR_CLOSER)){
+ //y2
+ if((data.lnStay[X_LINE]==1)&&(data.lnStay[Y_LINE]==0)){
+ if(
+ ((pow_x>0)&&((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]==B_SPOT))&&((data.irPosition== 9)||(data.irPosition==10)))||
+ ((pow_x>0)&&((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]==A_SPOT))&&((data.irPosition== 9)||(data.irPosition==10)))||
+ ((pow_x>0)&&((data.lnRawOrderLog1[0]==C_SPOT)&&(1 ))&&((data.irPosition==18)||(data.irPosition==19)))
+ ){
+ if(data.ping[R_PING]<WhiteToWallPlus[X_PING]){
+ data.lnStay[X_LINE]=0;
+ }
+ else{
+ data.lnStay[X_LINE]=1;
+ }
+ if(data.lnRepeat>=LINE_REPEAT){
+ data.lnStay[X_LINE]=0;
+ }
+ data.lnStayNow[X_LINE]=1;
+ }
+ else{
+ data.lnStay[X_LINE]=1;
+ }
+ }
+ if((data.lnStay[X_LINE]==1)&&(data.lnStay[Y_LINE]==0)){
+ if(
+ ((pow_x<0)&&((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]==B_SPOT))&&((data.irPosition==12)||(data.irPosition==13)))||
+ ((pow_x<0)&&((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]==A_SPOT))&&((data.irPosition==12)||(data.irPosition==13)))||
+ ((pow_x<0)&&((data.lnRawOrderLog1[0]==C_SPOT)&&(1 ))&&((data.irPosition==15)||(data.irPosition==16)))
+ ){
+ if(data.ping[L_PING]<WhiteToWallPlus[X_PING]){
+ data.lnStay[X_LINE]=0;
+ }
+ else{
+ data.lnStay[X_LINE]=1;
+ }
+ if(data.lnRepeat>=LINE_REPEAT){
+ data.lnStay[X_LINE]=0;
+ }
+ data.lnStayNow[X_LINE]=1;
+ }
+ else{
+ data.lnStay[X_LINE]=1;
+ }
+ }
+ //x2
+ if((data.lnStay[X_LINE]==0)&&(data.lnStay[Y_LINE]==1)){
+ if(
+ ((pow_y>0)&&((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]==C_SPOT))&&((data.irPosition== 9)||(data.irPosition==10)))||
+ ((pow_y>0)&&((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]==C_SPOT))&&((data.irPosition==12)||(data.irPosition==13)))
+ ){
+ if(data.ping[F_PING]<WhiteToWallPlus[Y_PING]){
+ data.lnStay[Y_LINE]=0;
+ }
+ else{
+ data.lnStay[Y_LINE]=1;
+ }
+ if(data.lnRepeat>=LINE_REPEAT){
+ data.lnStay[Y_LINE]=0;
+ }
+ data.lnStayNow[Y_LINE]=1;
+ }
+ else{
+ data.lnStay[Y_LINE]=1;
+ }
+
+ if(
+ ((pow_y<0)&&((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]==C_SPOT))&&((data.irPosition==18)||(data.irPosition==19)))||
+ ((pow_y<0)&&((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]==C_SPOT))&&((data.irPosition==15)||(data.irPosition==16)))
+ ){
+ if(data.ping[B_PING]<WhiteToWallPlus[Y_PING]){
+ data.lnStay[Y_LINE]=0;
+ }
+ else{
+ data.lnStay[Y_LINE]=1;
+ }
+ if(data.lnRepeat>=LINE_REPEAT){
+ data.lnStay[Y_LINE]=0;
+ }
+ data.lnStayNow[Y_LINE]=1;
+ }
+ else{
+ data.lnStay[Y_LINE]=1;
+ }
+ }
+ if(
+ (data.lnRepeat>=2)
+ ){
+ if(
+ ((pow_y>0)&&((data.ping[F_PING]<40)&&(1)))||
+ ((pow_y<0)&&((data.ping[B_PING]<40)&&(1)))
+ ){
+ data.lnStay[Y_LINE]=0;
+ }
+ if(
+ ((pow_x>0)&&((data.ping[R_PING]<40)&&(1)))||
+ ((pow_x<0)&&((data.ping[L_PING]<40)&&(1)))
+ ){
+ data.lnStay[X_LINE]=0;
+ }
+ }
+ }
+
+
}
else{
data.lnStay[X_LINE]=data.lnStay[Y_LINE]=1;