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:
- 34:1c86c1299ea4
- Parent:
- 33:aa115c30892e
- Child:
- 38:67bc78f3c0ab
--- a/main_processing/strategy/LineProcess.cpp Thu Mar 24 11:50:01 2016 +0000
+++ b/main_processing/strategy/LineProcess.cpp Thu Mar 24 15:33:54 2016 +0000
@@ -862,199 +862,230 @@
}
void LineJudgeReset3(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};
- 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>0)){
- 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]);
- data.lnCorner[B_LINE] = (data.ping[B_PING]<OutToWall[Y_PING]);
- if(
- (data.lnCorner[L_LINE])||
- (data.lnCorner[R_LINE])||
- (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;
- //Line_home.detach();
- LineLiberate();
- }
- else{
- data.FieldSpot = LINE_INSIDE;
- data.NonWall[L_LINE] = data.NonWall[R_LINE] = data.NonWall[F_LINE] = data.NonWall[B_LINE] = 0;
- //Line_home.detach();
- LineLiberate();
- LineRankClear();
- }
- }
- /*else if(data.FieldSpot==LINE_OUTSIDE){
- 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.lnRepeat==0){
+ data.lnStayNow[X_LINE]=0;
+ data.lnStayNow[Y_LINE]=0;
}
- if(data.FieldSpot == LINE_OUTSIDE){
- //Line_home.attach(&JudgeInSide, 2.0);
- if(
- (
- data.lnRaw==0
- )&&
- (
- (data.ping[L_PING]>=WhiteToWall[X_PING])||
- (data.NonWall[L_LINE]==1)||
- ((data.lnOrder[0]==A_SPOT)&&(data.lnOrder[1]==C_SPOT))||
- ((data.lnOrder[0]==A_SPOT)&&(data.NonWall[F_LINE]==1)&&(data.NonWall[B_LINE]==1))||
- ((data.lnOrder[0]==A_SPOT)&&(data.ping[F_PING]>WhiteToWall[Y_PING])&&(data.ping[B_PING]>WhiteToWall[Y_PING]))
- )&&
- (
- (data.ping[R_PING]>=WhiteToWall[X_PING])||
- (data.NonWall[R_LINE]==1)||
- ((data.lnOrder[0]==B_SPOT)&&(data.lnOrder[1]==C_SPOT))||
- ((data.lnOrder[0]==B_SPOT)&&(data.NonWall[F_LINE]==1)&&(data.NonWall[B_LINE]==1))||
- ((data.lnOrder[0]==B_SPOT)&&(data.ping[F_PING]>WhiteToWall[Y_PING])&&(data.ping[B_PING]>WhiteToWall[Y_PING]))
- )&&
- (
- (data.ping[F_PING]>=WhiteToWall[Y_PING])||
- (data.NonWall[F_LINE]==1)||
- ((data.lnOrder[0]==C_SPOT)&&(data.lnOrder[1]==A_SPOT))||
- ((data.lnOrder[0]==C_SPOT)&&(data.lnOrder[1]==B_SPOT))||
- ((data.lnOrder[0]==C_SPOT)&&(data.lnOrder[1]==LINE_EMPTY))
- )&&
- ((data.ping[B_PING]>=WhiteToWall[Y_PING])||
- (data.NonWall[B_LINE]==1)||
- ((data.lnOrder[0]==A_SPOT)&&(data.lnOrder[1]==B_SPOT))||
- ((data.lnOrder[0]==B_SPOT)&&(data.lnOrder[1]==A_SPOT))
- )
- ){
- data.FieldSpot = LINE_INSIDE;
- data.NonWall[L_LINE] = data.NonWall[R_LINE] = data.NonWall[F_LINE] = data.NonWall[B_LINE] = 0;
- //Line_home.detach();
- LineLiberate();
- LineRankClear();
- }
+ data.lnStay[X_LINE]=1;
+ data.lnStay[Y_LINE]=1;
+
+ if((data.irNotice==IR_FAR)&&(data.lnRepeat>1)){
+ data.lnRepeat=1;
}
- /*if((data.FieldSpot == LINE_INSIDE)&&(0<data.lnHold)&&(data.lnHold<7)&&(data.lnRaw==0)){
- if(
- (
- (
- (data.ping[L_PING]>=WhiteToWall[X_PING])||
- ((data.lnOrder[0]==A_SPOT)&&(data.lnOrder[1]==C_SPOT))
- //(data.lnOrder[0]==A_SPOT)
- )&&
- (
- (data.ping[R_PING]>=WhiteToWall[X_PING])||
- ((data.lnOrder[0]==B_SPOT)&&(data.lnOrder[1]==C_SPOT))
- //(data.lnOrder[0]==B_SPOT)
- )&&
- (
- (data.ping[F_PING]>=WhiteToWall[Y_PING])||
- ((data.lnOrder[0]==C_SPOT)&&(data.lnOrder[1]==A_SPOT))||
- ((data.lnOrder[0]==C_SPOT)&&(data.lnOrder[1]==B_SPOT))
- )&&
- (
- (data.ping[B_PING]>=WhiteToWall[Y_PING])||
- ((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.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]))&&(cmps_set.GoalDeg==0))||
+ //((data.irPosition==(ir_posi_s[(data.irLastPosition-8+24-3)%12]))&&(cmps_set.GoalDeg==0))
+ ))&&(
+ (data.irLastNotice==IR_CLOSE)||
+ (data.irLastNotice==IR_CLOSER)
+ )
)
){
data.lnRepeat = 0;
- NewLineCorner[L_LINE]=LINE_EMPTY;
- NewLineCorner[R_LINE]=LINE_EMPTY;
- NewLineCorner[F_LINE]=LINE_EMPTY;
- NewLineCorner[B_LINE]=LINE_EMPTY;
-
- LastLineCorner[L_LINE]=LINE_EMPTY;
- LastLineCorner[R_LINE]=LINE_EMPTY;
- LastLineCorner[F_LINE]=LINE_EMPTY;
- LastLineCorner[B_LINE]=LINE_EMPTY;
+ LineRawLogReset();
}
//data.lnRepeat=0;
- if((data.lnRepeat>0)&&(data.FieldSpot == LINE_INSIDE)){
+ //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)/*&&
+ (
+ (
+ (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)))
+ ){
+ 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)&&(NewLineCorner[R_LINE]))||
- ((pow_x<0)&&(NewLineCorner[L_LINE]))
+ ((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;
}
- //y
- if(
- ((pow_y>=0)&&(NewLineCorner[F_LINE]))||
- ((pow_y<0)&&(NewLineCorner[B_LINE]))
- ){
- data.lnStay[Y_LINE]=0;
+ 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[Y_LINE]=1;
- }
+
+
}
else{
data.lnStay[X_LINE]=data.lnStay[Y_LINE]=1;
}
- //*/
}
void JudgeInSide(void){