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;