Ryu Kaz
/
767zi
t6est
Diff: main.cpp
- Revision:
- 3:8b22783f6bf1
- Parent:
- 2:a3eebc8cd220
- Child:
- 4:9ba47e5db1e2
diff -r a3eebc8cd220 -r 8b22783f6bf1 main.cpp --- a/main.cpp Wed Sep 25 07:47:37 2019 +0000 +++ b/main.cpp Thu Sep 26 02:23:27 2019 +0000 @@ -41,7 +41,7 @@ Timer go_up_tm; ///////////定数たち////////////// const float Min_ang = 0.1f; -const float Max_pwm = 0.2f; +const float Max_pwm = 0.15f; const float Max_pwm_roll = 0.3f; const float Min_pwm = 0.02f; const float Gein = 0.05f;//0.05f @@ -121,6 +121,9 @@ int l_r = 0; int l_l = 0; ///////////////////////////////////////////// +DigitalOut outColor(PG_4); +DigitalOut outSorT(PG_7); +//////////////////////////////////////////// int main() { ins.mode(PullDown); lim_right.mode(PullDown); @@ -129,38 +132,31 @@ int whichnum = read_peri(); int points[whichnum][3]; changeColor(&points[0][0]); - + + outColor = getColor(); + outSorT = getPeriwhich(); + //if(read_keter())keter=true; - //bt.putc('a'); setMove(points[point_count][0],points[point_count][1]); serial.printf("%s\n","mpu_setup"); - //bt.putc('b'); if(!keter){ mpu_setup(); serial.printf("%s\n","get_off"); - //bt.putc('c'); get_offset(); serial.printf("%d\n",rez()); serial.printf("%s\n","move2"); - //bt.putc('d'); } Move2(0,0); change_rm.start(); move_tm.start(); go_up_tm.start(); - //bt.putc('e'); - /* - */ while(1) { serial.printf("%f",all); serial.printf("%s",":"); if(!keter){ readGyz(&gz); offset_adjust(&all,&gz); - //bt.putc('f'); - //bt.puts(puc(all)); - //bt.putc('F'); to_signed(&all); } /////////////傾き修正//////////////////////////// @@ -169,7 +165,6 @@ pwm = angle_adjust(Gein,all,Min_ang,Min_pwm,Max_pwm_roll); if(!activeMove){ if(pwm <= Min_pwm){ - //bt.putc('k'); activeMove = true; } @@ -196,14 +191,6 @@ x_dis = rorix-off_rorix; y_dis = roriy-off_roriy; - /* - bt.putc('g'); - bt.puts(puc(x_dis)); - bt.putc('G'); - bt.putc('h'); - bt.puts(puc(y_dis)); - bt.putc('H'); - */ /////移動処理///////// if(!flag_finish_move){ float pwm = 0; @@ -227,7 +214,6 @@ if(move_tm.read_ms() >= 600){ serial.printf("%s","waiting"); serial.printf("%d\n",point_count); - //bt.putc('i'); ////////////////////////////// pwm = 1; float set0_dis; @@ -259,7 +245,6 @@ } } }else{ - //serial.printf("%s\n","whileroll"); pwm_while = angle_adjust(Gein,all,Min_ang,Min_pwm,Max_pwm_roll); if(pwm_while <= Min_pwm){ whileactive = true; @@ -290,44 +275,15 @@ } } }else{ - //serial.printf("%f",pwm); if(pwm > Max_pwm)pwm = Max_pwm; if(activeMove){ 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; + ///////////////////////////////////////////////////////////////////////////////////////// + if(!moved_to_wall && points[point_count][1]-distance > 1200-100 && !start_wall_time){ 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; @@ -335,50 +291,51 @@ point_move = true; activeMove = false; Move2(0,0); - //goto START; } }else{ - - if(!moved_to_wall && points[point_count][1]-distance > 1200 && !start_wall_time){ - //start_wall_time = true; + ///////////////////////////////////////////////////////////////////////////////////////// + if(!moved_to_wall && points[point_count][1]-distance > 1200-100 && !start_wall_time){ 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++; point_move = true; activeMove = false; Move2(0,0); - //goto START; } + } + 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; + }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){ + moved_to_wall = false; + //wall_C = -1750+from_end + points[point_count][1] - pointx + y_dis;//補正 + }else if(l_r || l_l){ + serial.printf("%d",l_l); + serial.printf("%s",":"); + serial.printf("%d\n",l_r); + Move2(points[point_count][0],0.1f); + } + } } + if(point_move){ serial.printf("%s\n","maemae"); if(C_vari == 0){ @@ -387,12 +344,12 @@ } if(go_up){ dis_go_up = abs(rorix-off_rorix); - if(abs(150-dis_go_up)*RoriGein > Min_pwm){ - if(dis_go_up < 150){ - Move2(0,(150-dis_go_up)*RoriGein);//+-20mm + if(abs(200-dis_go_up)*RoriGein > Min_pwm){ + if(dis_go_up < 200){ + Move2(0,(200-dis_go_up)*RoriGein);//+-20mm go_up_tm.reset(); }else{ - Move2(1,(dis_go_up-150)*RoriGein); + Move2(1,(dis_go_up-200)*RoriGein); go_up_tm.reset(); } } @@ -446,7 +403,6 @@ if(change_rm.read_ms() > 1500){ activeMove = false; change_rm.reset(); - //bt.putc('j'); serial.printf("%s\n","change_rm"); } }