Revision 4:9ba47e5db1e2, committed 2019-10-05

Comitter:

kazuryu

Date:

Sat Oct 05 01:41:37 2019 +0000

Parent:

3:8b22783f6bf1

Commit message:

B

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Pulse.lib	Sat Oct 05 01:41:37 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/NickRyder/code/Pulse/#fb79a4637a64

--- a/backup.md	Thu Sep 26 02:23:27 2019 +0000
+++ b/backup.md	Sat Oct 05 01:41:37 2019 +0000
@@ -41,13 +41,14 @@
 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
+const float Gein = 0.04f;//0.05f
 const int R = 103;//これ直径やんけ
 const int Resolution = 1024;
 const float RoriGein = 0.001f;
+const int mae_dis = 250;
 ///////////////////////
 int point_count = 0;
 int array_count = 0;
@@ -91,6 +92,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 +117,22 @@
 //////////////////////////////////////////////
 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);
+///////////////////////////////////////////
+float only_this_time = 0;
 int main() {
     ins.mode(PullDown);
     lim_right.mode(PullDown);
@@ -136,6 +145,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");
@@ -196,7 +206,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){
@@ -219,6 +229,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,17 +238,26 @@
                                     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{
@@ -280,40 +300,10 @@
                         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){
+                                /////////////////////////////////////////////////////////////////////////////////////////
+                                if(!moved_to_wall && points[point_count][1]-distance > 1000 && !start_wall_time){
                                     moved_to_wall = 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;
-                                        }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(points[point_count][1]-distance > sheets[array_count] && array_count != 3){
                                     if(array_count == 0)go_up = true;
@@ -323,40 +313,11 @@
                                     Move2(0,0);
                                     }
                             }else{
-                            
-                                if(!moved_to_wall && points[point_count][1]-distance > 1200 && !start_wall_time){
+                                /////////////////////////////////////////////////////////////////////////////////////////
+                                if(!moved_to_wall && points[point_count][1]-distance > 1000 && !start_wall_time){
                                     moved_to_wall = 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;
-                                        }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){
-                                            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){
-                                            serial.printf("%d",l_l);
-                                            serial.printf("%s",":");
-                                            serial.printf("%d\n",l_r);
-                                            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++;
@@ -364,28 +325,73 @@
                                     activeMove = false;
                                     Move2(0,0);
                                     }
+                                }
+                            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;
+                                        Move2(10,0);
+                                    }else if(!l_r && l_l){
+                                        serial.printf("%s\n","LSRIDE");
+                                        Move2(RIGHT,0.1f);
+                                    }else if(l_r && !l_l){
+                                        Move2(LEFT,0.1f);
+                                        serial.printf("%s\n","RSRIDE");
+                                    }else{
+                                        Move2(UP,0.1f);
+                                        serial.printf("%s\n","UP");
+                                    }
+                                }else{
+                                    if(!l_r && !l_l){
+                                        moved_to_wall = false;
+                                        hensu_A = true;
+                                        wait(0.5f);
+                                    }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(hensu_A){
+                                gav_out = 1;
+                                while(!gav_in.read()){
+                                    serial.printf("%d\n",gav_in.read());
+                                    }
+                                //wall_C = -1400-from_mid+points[point_count][1]-pointx+y_dis; 
+                                points[point_count][1] = 1400 + distance;
+                                gav_out = 0;
+                                hensu_A = 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(150-dis_go_up)*RoriGein > Min_pwm){
-                                    if(dis_go_up < 150){
-                                        Move2(0,(150-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-150)*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);
+                                    Move2(10,0);
                                 }
                             }else{
                                 serial.printf("%s\n","owari");
@@ -418,8 +424,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();
                 }

--- a/foot.cpp	Thu Sep 26 02:23:27 2019 +0000
+++ b/foot.cpp	Sat Oct 05 01:41:37 2019 +0000
@@ -20,7 +20,7 @@
 PwmOut pwms[8] = {RFA,RFB,LFA,LFB,RBA,RBB,LBA,LBB};
 PwmOut lockR(PB_10);
 PwmOut lockL(PB_11);
-PwmOut lockpwms[2] = {lockR,lockL};
+
 //new
 //PB_6 -> PD_12
 //PC_7 -> PB_5
@@ -53,8 +53,9 @@
   {255, -255, 255, -255},//9左旋回
   {500,500,500,500}
 };
-float pwm_g[] = {1.1052f,1,1.0421f,1.0526f};
-
+//1.1052f,1,1.0421f,1.0726f
+float pwm_g[] = {1,0.904f,0.982f,0.970f};
+    //1,0.904,0.942,0.970
 void Move(int way,float pwm){
     int num;
     for(int i =0;i<4;i++){
@@ -84,12 +85,12 @@
             pwms[i*2] = 0;
             pwms[i*2+1] = pwm*pwm_g[i];
         }else if(num == 500){
-            pwms[i*2] = 1;
-            pwms[i*2+1] = 1;
+            pwms[i*2] = 0;
+            pwms[i*2+1] = 0;
             }
         }
     }
-    
+/*
 void Move_lock(int way,float pwm){
     int num;
     if(count_Move_lock == 0){
@@ -109,7 +110,7 @@
         }
         }
     }
-
+*/
 void trans(int a,int dis,float *x,float *y,int *sign){
     if(a == 0){
         *y=dis;

--- a/functions.cpp	Thu Sep 26 02:23:27 2019 +0000
+++ b/functions.cpp	Sat Oct 05 01:41:37 2019 +0000
@@ -2,10 +2,10 @@
 #include "functions.h"
 
 const int addr = 0x68 << 1;
-const int Sampling = 4000;
+const int Sampling = 3000;
 
 I2C i2c(PB_11 , PB_10);//PB_11 -> PB_9
-                       //PB_10 -> PB_8
+                       // -> PB_8
                          //PF_0,PF_1
 Timer timer;
 Serial ss(USBTX,USBRX);
@@ -33,6 +33,7 @@
     }
 
 void get_offset(){
+    offset_gz = 0;
     for(int i =0;i<Sampling;i++){
         
         double yaw;

--- 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;
    }
    
 }

--- a/peripheral.cpp	Thu Sep 26 02:23:27 2019 +0000
+++ b/peripheral.cpp	Sat Oct 05 01:41:37 2019 +0000
@@ -19,6 +19,8 @@
 DigitalIn DD5(D5);
 DigitalIn Dketer(PE_7);
 
+DigitalOut trig(PF_5);
+
 Serial peri_serial(USBTX,USBRX);
 
 int a= 150;