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(){