Ryu Kaz
/
767zi
t6est
Diff: main.cpp
- Revision:
- 1:bbf776e6c792
- Parent:
- 0:3dc012104243
- Child:
- 2:a3eebc8cd220
--- a/main.cpp Tue Sep 24 13:35:31 2019 +0000 +++ b/main.cpp Wed Sep 25 03:57:31 2019 +0000 @@ -62,16 +62,19 @@ int Robox = getRobox(); //int C = (1750-Robox); int C = 1550; + /* int sheets[] = { C+100,C+1300,C+2500 }; */ int sheets[] = { - 900,1700,3500 + 900,1700,3500//下から:1750,2775,3575 }; + +//当たるのは1200mm int nomal_towel[]{ - 1750,2775,3575//2130,2930,3730 + 1975,2775,3575//2130,2930,3730 }; int final_towel[]{ C+410,C+1070,C+1730,C+2390 @@ -109,9 +112,19 @@ bool go_up = false; float dis_go_up = 0; ////////////////////////////////////////////// +bool moved_to_wall = false; +bool start_wall_time = false; +int from_end = 200; +int wall_C = 0; +DigitalIn lim_right(D0); +DigitalIn lim_left(D1); +int l_r = 0; +int l_l = 0; +///////////////////////////////////////////// int main() { ins.mode(PullDown); - + lim_right.mode(PullDown); + lim_left.mode(PullDown); setup_peri(); int whichnum = read_peri(); int points[whichnum][3]; @@ -196,11 +209,11 @@ float pwm = 0; float distance; if(points[point_count][0] == UP or points[point_count][0] == BACK){ - distance = pointy-x_dis; + distance = pointy-x_dis + wall_C; last_move_vertical = true; last_move_side = false; }else if(points[point_count][0] == LEFT or points[point_count][0] == RIGHT){ - distance = pointx-y_dis; + distance = pointx-y_dis + wall_C; last_move_vertical = false; last_move_side = true; } @@ -265,6 +278,10 @@ } //////////////////////// point_count++; + ////////////////////////////////////////////////////////////////// + start_wall_time = false; + moved_to_wall = false; + ///////////////////////////////////////////////////////////////// Move2(1,0); if(point_count >= sizeof(points)/sizeof(*points)){ flag_finish_move = true; @@ -279,6 +296,39 @@ if(points[point_count][2] != 0){ 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; + 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;//補正 + } + } + } + */ + ///////////////////////////////////////////////////////////////////////////////////////// if(points[point_count][1]-distance > sheets[array_count] && array_count != 3){ if(array_count == 0)go_up = true; array_count++; @@ -330,7 +380,7 @@ } } - if(!point_move){ + if(!point_move && !moved_to_wall){ if(distance > 0)Move2(points[point_count][0],pwm); else{ pwm = (-distance*RoriGein > Max_pwm)?Max_pwm:-distance*RoriGein; @@ -387,6 +437,7 @@ else x_flag = false; if((sign >> 1) == 1)y_flag = true;//y < 0 else y_flag = false; + } void get_rorivol(){