Ryu Kaz
/
767zi
t6est
Diff: main.cpp
- Revision:
- 2:a3eebc8cd220
- Parent:
- 1:bbf776e6c792
- Child:
- 3:8b22783f6bf1
--- a/main.cpp Wed Sep 25 03:57:31 2019 +0000 +++ b/main.cpp Wed Sep 25 07:47:37 2019 +0000 @@ -69,7 +69,7 @@ }; */ int sheets[] = { - 900,1700,3500//下から:1750,2775,3575 + 1975,2775,3575//下から:1750,2775,3575 }; //当たるのは1200mm @@ -114,10 +114,10 @@ ////////////////////////////////////////////// bool moved_to_wall = false; bool start_wall_time = false; -int from_end = 200; +int from_end = 370; int wall_C = 0; -DigitalIn lim_right(D0); -DigitalIn lim_left(D1); +DigitalIn lim_right(PG_10); +DigitalIn lim_left(PG_15); int l_r = 0; int l_l = 0; ///////////////////////////////////////////// @@ -297,9 +297,9 @@ bool issheets = (points[point_count][2] == 1)?1:0; if(issheets){ /////////////////////////////////////////////////////////////////////////////////////////// - /* + if(!moved_to_wall && points[point_count][1]-distance > 1200 && !start_wall_time){ - start_wall_time = true; + //start_wall_time = true; moved_to_wall = true; } if(moved_to_wall){ @@ -327,7 +327,7 @@ } } } - */ + ///////////////////////////////////////////////////////////////////////////////////////// if(points[point_count][1]-distance > sheets[array_count] && array_count != 3){ if(array_count == 0)go_up = true; @@ -338,6 +338,38 @@ //goto START; } }else{ + + if(!moved_to_wall && points[point_count][1]-distance > 1200 && !start_wall_time){ + //start_wall_time = true; + moved_to_wall = true; + } + if(moved_to_wall){ + if(!start_wall_time){ + l_r = lim_right.read(); + l_l = lim_left.read(); + if(l_r && l_l){ + start_wall_time = true; + all = 0; + }else if(!l_r && l_l){ + serial.printf("%s\n","LROLL"); + Move2(LROLL,0.1f); + }else if(l_r && !l_l){ + Move2(RROLL,0.1f); + serial.printf("%s\n","RROLL"); + }else{ + Move2(UP,0.1f); + serial.printf("%s\n","UP"); + } + }else{ + if(!l_r && !l_l && start_wall_time){ + serial.printf("%s\n","C"); + moved_to_wall = false; + wall_C = -1750+from_end + points[point_count][1] - pointx + y_dis;//補正 + }else if(l_r || l_l){ + Move2(points[point_count][0],0.1f); + } + } + } if(points[point_count][1]-distance > nomal_towel[array_count] && array_count != 3){ if(array_count == 0)go_up = true; array_count++; @@ -380,7 +412,7 @@ } } - if(!point_move && !moved_to_wall){ + if(!point_move && !moved_to_wall && !go_up){ if(distance > 0)Move2(points[point_count][0],pwm); else{ pwm = (-distance*RoriGein > Max_pwm)?Max_pwm:-distance*RoriGein;