Ryu Kaz
/
767zi
t6est
Diff: main.cpp
- Revision:
- 4:9ba47e5db1e2
- Parent:
- 3:8b22783f6bf1
diff -r 8b22783f6bf1 -r 9ba47e5db1e2 main.cpp --- a/main.cpp Thu Sep 26 02:23:27 2019 +0000 +++ b/main.cpp Sat Oct 05 01:41:37 2019 +0000 @@ -43,11 +43,13 @@ const float Min_ang = 0.1f; const float Max_pwm = 0.15f; const float Max_pwm_roll = 0.3f; +const float Min_pwm_roll = 0.015f; const float Min_pwm = 0.02f; -const float Gein = 0.05f;//0.05f +const float Gein = 0.06f;//0.05f const int R = 103;//これ直径やんけ const int Resolution = 1024; const float RoriGein = 0.001f; +const int mae_dis = 300; /////////////////////// int point_count = 0; int array_count = 0; @@ -91,6 +93,8 @@ float x_dis,y_dis; bool flag_finish_move = false; +float mae_pwm = 1; + void setMove(int p0,int p1); void get_rorivol(); @@ -114,16 +118,32 @@ ////////////////////////////////////////////// bool moved_to_wall = false; bool start_wall_time = false; -int from_end = 370; +int from_mid = 0; int wall_C = 0; DigitalIn lim_right(PG_10); DigitalIn lim_left(PG_15); int l_r = 0; int l_l = 0; + + +bool hensu_A = false; ///////////////////////////////////////////// DigitalOut outColor(PG_4); DigitalOut outSorT(PG_7); //////////////////////////////////////////// +DigitalOut gav_out(PD_10); +DigitalIn gav_in(PG_14); +/////////////////////////////////////////// +DigitalOut TdistnaceOut(PC_6); +DigitalOut FdistanceOut(PB_15); +float subdistance = 0; +float only_this_time = 0; +float gav_high = 0; +bool dis_able_to_use_g = false; +bool gav1000 = true; +PwmOut sheetwo_out(PF_6); +bool sheetwo_flag = false; +float last_dis = 0; int main() { ins.mode(PullDown); lim_right.mode(PullDown); @@ -136,6 +156,7 @@ outColor = getColor(); outSorT = getPeriwhich(); + gav_in.mode(PullDown); //if(read_keter())keter=true; setMove(points[point_count][0],points[point_count][1]); serial.printf("%s\n","mpu_setup"); @@ -162,9 +183,9 @@ /////////////傾き修正//////////////////////////// if(!keter){ - pwm = angle_adjust(Gein,all,Min_ang,Min_pwm,Max_pwm_roll); + pwm = angle_adjust(Gein,all,Min_ang,Min_pwm_roll,Max_pwm_roll); if(!activeMove){ - if(pwm <= Min_pwm){ + if(pwm <= Min_pwm_roll){ activeMove = true; } @@ -183,7 +204,7 @@ if(!activeMove)activeMove = true; } /////////ロリコン値取得/////// - if(activeMove){ + if(activeMove && !dis_able_to_use_g){ get_rorivol(); } @@ -196,7 +217,7 @@ float pwm = 0; float distance; if(points[point_count][0] == UP or points[point_count][0] == BACK){ - distance = pointy-x_dis + wall_C; + distance = pointy-x_dis; last_move_vertical = true; last_move_side = false; }else if(points[point_count][0] == LEFT or points[point_count][0] == RIGHT){ @@ -204,14 +225,26 @@ last_move_vertical = false; last_move_side = true; } + subdistance = distance; //else if(points[point_count][0] >= 4 && points[point_count][0] <= 7)ななっめ移動 if(x_flag or y_flag)distance *= -1; pwm = distance*RoriGein; pwm = (pwm < 0)?-pwm:pwm;//abs + if(last_dis - distance > 0){ + TdistnaceOut = 1; + FdistanceOut = 0; + }else if(last_dis - distance < 0){ + TdistnaceOut = 0; + FdistanceOut = 1; + }else{ + TdistnaceOut = 1; + FdistanceOut = 1; + } if(pwm < Min_pwm){ //次の処理へ serial.printf("%d\n",move_tm.read_ms()); if(move_tm.read_ms() >= 600){ + sheetwo_flag = false; serial.printf("%s","waiting"); serial.printf("%d\n",point_count); ////////////////////////////// @@ -219,6 +252,7 @@ float set0_dis; float pwm_while; bool whileactive = true; + only_this_time = rorix; move_tm.reset(); while(move_tm.read_ms() < 600){ if(whileactive){ @@ -227,21 +261,30 @@ set0_dis = roriy-off_roriy; pwm = set0_dis*RoriGein; if(abs(pwm) >= Min_pwm)move_tm.reset(); + if(abs(pwm) > Max_pwm*0.9)pwm = Max_pwm;//ぐわんぐわんするから if(pwm < 0){ Move2(RIGHT,-pwm); }else{ Move2(LEFT,pwm); } }else if(last_move_side){ - serial.printf("%s\n","side"); - set0_dis = rorix-off_rorix; - serial.printf("%f\n",set0_dis); - pwm = set0_dis*RoriGein; + if(points[point_count][2] == 0){ + serial.printf("%s\n","side"); + set0_dis = rorix-off_rorix; + serial.printf("%f\n",set0_dis); + pwm = set0_dis*RoriGein; + }else{ + serial.printf("%s\n","only_this_side"); + set0_dis = abs(rorix - only_this_time); + serial.printf("%f\n",mae_dis-set0_dis); + pwm = (mae_dis-set0_dis)*RoriGein; + } if(abs(pwm) >= Min_pwm)move_tm.reset(); + if(abs(pwm) > Max_pwm*0.9)pwm = Max_pwm;//ぐわんぐわんするから if(pwm < 0){ Move2(UP,-pwm); }else{ - Move2(BACK,pwm); + Move2(BACK,pwm); } } }else{ @@ -281,9 +324,6 @@ bool issheets = (points[point_count][2] == 1)?1:0; if(issheets){ ///////////////////////////////////////////////////////////////////////////////////////// - if(!moved_to_wall && points[point_count][1]-distance > 1200-100 && !start_wall_time){ - moved_to_wall = true; - } ///////////////////////////////////////////////////////////////////////////////////////// if(points[point_count][1]-distance > sheets[array_count] && array_count != 3){ if(array_count == 0)go_up = true; @@ -294,9 +334,6 @@ } }else{ ///////////////////////////////////////////////////////////////////////////////////////// - if(!moved_to_wall && points[point_count][1]-distance > 1200-100 && !start_wall_time){ - moved_to_wall = true; - } ///////////////////////////////////////////////////////////////////////////////////////// if(points[point_count][1]-distance > nomal_towel[array_count] && array_count != 3){ if(array_count == 0)go_up = true; @@ -306,19 +343,43 @@ Move2(0,0); } } +/* + if(points[point_count][1]-distance < 1000){ + if(gav_high == 0)gav_out = 1; + if(gav_in.read()){ + gav_high = points[point_count][1]-distance;//移動距離 + gav_out = 0; + } + } +*//* + if(gav_high == 0){ + gav_out = 1; + if(gav_in.read()){ + gav_high = points[point_count][1]-distance+175; + gav_out = 0; + serial.printf("%f\n",gav_high); + } + } + */ + if(!moved_to_wall && points[point_count][1]-distance > 1030 && !start_wall_time){ + activeMove = false; + moved_to_wall = true; + dis_able_to_use_g = true; + } + if(moved_to_wall){ l_r = lim_right.read(); l_l = lim_left.read(); if(!start_wall_time){ if(l_r && l_l){ start_wall_time = true; - all = 0; + //Move2(10,0); }else if(!l_r && l_l){ - serial.printf("%s\n","LROLL"); - Move2(LROLL,0.1f); + serial.printf("%s\n","LSRIDE"); + Move2(RIGHT,0.1f); }else if(l_r && !l_l){ - Move2(RROLL,0.1f); - serial.printf("%s\n","RROLL"); + Move2(LEFT,0.1f); + serial.printf("%s\n","RSRIDE"); }else{ Move2(UP,0.1f); serial.printf("%s\n","UP"); @@ -326,7 +387,10 @@ }else{ if(!l_r && !l_l){ moved_to_wall = false; - //wall_C = -1750+from_end + points[point_count][1] - pointx + y_dis;//補正 + hensu_A = true; + wait(0.5f); + dis_able_to_use_g = false; + Move2(points[point_count][0],0.1f); }else if(l_r || l_l){ serial.printf("%d",l_l); serial.printf("%s",":"); @@ -335,28 +399,43 @@ } } } - + if(hensu_A){ + gav_out = 1; + while(!gav_in.read()){ + serial.printf("%d\n",gav_in.read()); + } + points[point_count][1] = 1400 + distance; + gav_out = 0; + hensu_A = false; + gav1000 = false; + } if(point_move){ - serial.printf("%s\n","maemae"); + serial.printf("%s","maemae"); if(C_vari == 0){ off_rorik = rorix; C_vari = 1; } if(go_up){ - dis_go_up = abs(rorix-off_rorix); - if(abs(200-dis_go_up)*RoriGein > Min_pwm){ - if(dis_go_up < 200){ - Move2(0,(200-dis_go_up)*RoriGein);//+-20mm + dis_go_up = abs(rorix-off_rorik);//進んだ距離 + serial.printf("%f",mae_dis-dis_go_up);//残り + if(mae_pwm > Min_pwm){ + if(dis_go_up < mae_dis){ + mae_pwm = (mae_dis-dis_go_up)*RoriGein; + if(mae_pwm > Max_pwm)mae_pwm = 0.1f; + Move2(0,mae_pwm);//+-20mm go_up_tm.reset(); }else{ - Move2(1,(dis_go_up-200)*RoriGein); + mae_pwm = (dis_go_up-mae_dis)*RoriGein; + if(mae_pwm > Max_pwm)mae_pwm = 0.1f; + Move2(1,mae_pwm); go_up_tm.reset(); } } if(go_up_tm.read_ms() > 300){ go_up = false; - Move2(0,0); + sheetwo_flag = true; + Move2(10,0); } }else{ serial.printf("%s\n","owari"); @@ -368,12 +447,18 @@ } } - + if(sheetwo_flag){ + sheetwo_out = 0.3f; + } 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; - Move2(sewomukeru[points[point_count][0]],pwm); + if(points[point_count][2] != 0 && gav1000){ + Move2(points[point_count][0],0.1f); + }else{ + if(distance > 0)Move2(points[point_count][0],pwm); + else{ + pwm = (-distance*RoriGein > Max_pwm)?Max_pwm:-distance*RoriGein; + Move2(sewomukeru[points[point_count][0]],pwm); + } } } serial.printf("%f",pwm); @@ -389,8 +474,10 @@ if(points[1][0] == RIGHT)serial.printf("%s","orange"); if(points[1][0] == LEFT)serial.printf("%s","blue"); serial.printf("%s",":"); - if(points[0][1] > 5100)serial.printf("%s\n","sheets"); - else serial.printf("%s\n","towel"); + if(points[0][1] > 5100)serial.printf("%s","sheets"); + else serial.printf("%s","towel"); + serial.printf("%s",":"); + serial.printf("%f\n",points[point_count][1]-distance); } move_tm.reset(); } @@ -407,7 +494,7 @@ } } - + last_dis = subdistance; } }