Diff: main.cpp

Revision:

49:6337717484fe

Parent:

48:d9f3ce701aca

Child:

50:3e73f68c3b37

Child:

54:f54d26e8d8ac

--- a/main.cpp	Sat May 18 09:12:33 2019 +0000
+++ b/main.cpp	Mon May 20 00:54:03 2019 +0000
@@ -116,79 +116,101 @@
     switch(i) {
         case 0://最初の直線
             printf("go straight!!!!!!!!!!\r\n");
-            motor_lo.setDutyLimit(0.6);
-            motor_li.setDutyLimit(0.6);
-            for(int i = 0; i < 30; i++) {
-                straightAndInfinity(0, 7);
+            motor_lo.setDutyLimit(params[0].duty);
+            motor_li.setDutyLimit(params[0].duty);
+            for(int i = 0; i < (int)params[0].condition; i++) {
+                straightAndInfinity(params[0].argument[0], params[0].argument[1]);
             }
             printf("get distance mode");
+            
+            motor_lo.setDutyLimit(params[1].duty);
+            motor_li.setDutyLimit(params[1].duty);
             for(int i=0; i<1; ++i) {
-                while(get_dist_back() < 320) {//300
-                    straightAndInfinity(0, 7);
+                while(get_dist_back() < params[1].condition) {//300
+                    straightAndInfinity(params[1].argument[0], params[1].argument[1]);
                 }
             }
             //段差前旋回
-            motor_lo.setDutyLimit(0.6);//0.5
-            motor_li.setDutyLimit(0.6);
-
-            if(RightOrLeft == 0) turn(35.0);
-            else turn(-35.0);
+            motor_lo.setDutyLimit(params[2].duty);
+            motor_li.setDutyLimit(params[2].duty);
+            if(RightOrLeft == 0) turn(params[2].argument[0]);
+            else turn(-params[2].argument[0]);
         case 1://段差前
             printf("climb!!!!!!!!!!\r\n");
+            
+            motor_lo.setDutyLimit(params[3].duty);
+            motor_li.setDutyLimit(params[3].duty);
             //段差乗り越え
-            while(get_dist_forward() < 60) {
-                if(RightOrLeft == 0) straightAndInfinity(45, 5);
-                else straightAndInfinity(-45, 5);
+            while(get_dist_forward() < params[3].condition) {
+                if(RightOrLeft == 0) straightAndInfinity(params[3].argument[0], params[3].argument[1]);
+                else straightAndInfinity(-params[3].argument[0], params[3].argument[1]);
             }
-            if(RightOrLeft == 0) phasing(45.0);
-            else phasing(-45.0);
+            
+            motor_lo.setDutyLimit(params[4].duty);
+            motor_li.setDutyLimit(params[4].duty);
+            if(RightOrLeft == 0) phasing(params[4].argument[0]);
+            else phasing(-params[4].argument[0]);
 
-            motor_lo.setDutyLimit(0.2);//0.5
-            motor_li.setDutyLimit(0.2);
-
-            for(int i= 0; i<14; ++i) straight();
-            while(get_dist_back() > 80) straight();
+            motor_lo.setDutyLimit(params[5].duty);
+            motor_li.setDutyLimit(params[5].duty);
+            for(int i= 0; i<(int)params[5].condition; ++i) straight();
+            
+            motor_lo.setDutyLimit(params[6].duty);
+            motor_li.setDutyLimit(params[6].duty);
+            while(get_dist_back() > params[6].condition) straight();
+            
             //段差後旋回
-            motor_lo.setDutyLimit(0.6);//0.5
-            motor_li.setDutyLimit(0.6);
-
-            if(RightOrLeft == 0) turn(85.0);
-            else turn(-85.0);
+            motor_lo.setDutyLimit(params[7].duty);
+            motor_li.setDutyLimit(params[7].duty);
+            if(RightOrLeft == 0) turn(params[7].argument[0]);
+            else turn(-params[7].argument[0]);
 
         case 2://段差直後
             printf("go lope!!!!!!!!!!\r\n");
             //前進
-            motor_lo.setDutyLimit(0.6);
-            motor_li.setDutyLimit(0.6);
-            for(int i=0; i<13; ++i) {
-                if(RightOrLeft == 0) straightAndInfinity(90.0, 7.0);
-                else straightAndInfinity(-90.0, 7.0);
+            motor_lo.setDutyLimit(params[8].duty);
+            motor_li.setDutyLimit(params[8].duty);
+            for(int i=0; i<(int)params[8].condition; ++i) {
+                if(RightOrLeft == 0) straightAndInfinity(params[8].argument[0], params[8].argument[1]);
+                else straightAndInfinity(-params[8].argument[0], params[8].argument[1]);
             }
+            
+            motor_lo.setDutyLimit(params[9].duty);
+            motor_li.setDutyLimit(params[9].duty);
             for(int i=0; i<3; ++i) {
-                while(get_dist_forward() > 65) {
-                    if(RightOrLeft == 0) straightAndInfinity(90.0, 7.0);
-                    else straightAndInfinity(-90.0, 7.0);
+                while(get_dist_forward() > params[9].condition) {
+                    if(RightOrLeft == 0) straightAndInfinity(params[9].argument[0], params[9].argument[1]);
+                    else straightAndInfinity(-params[9].argument[0], params[9].argument[1]);
                 }
             }
             //ロープ前旋回
             printf("before roop deg:%.3f\r\n",degree0);
-
-            if(RightOrLeft == 0) turn(5.0);
-            else turn(-5.0);
+            motor_lo.setDutyLimit(params[10].duty);
+            motor_li.setDutyLimit(params[10].duty);
+            if(RightOrLeft == 0) turn(params[10].argument[0]);
+            else turn(-params[10].argument[0]);
 
             //ロープ位置ジャストまで前進
+            motor_lo.setDutyLimit(params[11].duty);
+            motor_li.setDutyLimit(params[11].duty);
             for(int i=0; i<3; ++i) {
-                straightAndInfinity(0.0, 7.0);
+                straightAndInfinity(params[11].argument[0], params[11].argument[1]);
             }
             for(int i=0; i<2; ++i) {
-                while(get_dist_back() < 100) {
-                    straightAndInfinity(0.0, 7.0);
+                while(get_dist_back() < params[11].condition) {
+                    straightAndInfinity(params[11].argument[0], params[11].argument[1]);
                 }
             }
-            phasing(0.0);
+            
+            motor_lo.setDutyLimit(params[12].duty);
+            motor_li.setDutyLimit(params[12].duty);
+            phasing(params[12].argument[0]);
+            
             //ロープ
+            motor_lo.setDutyLimit(params[13].duty);
+            motor_li.setDutyLimit(params[13].duty);
             while(switch_modes[2].read() == 0) {
-                //straightAndInfinity(0, 7);
+                //straightAndInfinity(params[13].argument[0], params[13].argument[1]);
                 straight();
             }
             printf("uhai stand by ok!!!!!!!!!!\r\n");
@@ -198,25 +220,26 @@
 
             if(RightOrLeft == 0) theta0 = -degree/100.0+90;
             else theta0 = -degree/100.0-90;
-            motor_lo.setDutyLimit(0.4);//0.3
-            motor_li.setDutyLimit(0.4);
-            for(int i=0; i<20; ++i) {
-                if(RightOrLeft == 0) straightAndInfinity(-90, 15);//straight();//climbAndInfinity(-90,5);
-                else straightAndInfinity(90, 15);
+            motor_lo.setDutyLimit(params[14].duty);
+            motor_li.setDutyLimit(params[14].duty);
+            for(int i=0; i<(int)params[14].condition; ++i) {
+                if(RightOrLeft == 0) straightAndInfinity(params[14].argument[0], params[14].argument[1]);//straight();//climbAndInfinity(-90,5);
+                else straightAndInfinity(-params[14].argument[0], params[14].argument[1]);
             }
             printf("wall standby");
-            motor_lo.setDutyLimit(0.3);//0.3
-            motor_li.setDutyLimit(0.3);
-            while(get_dist_back() < 250) {
+            motor_lo.setDutyLimit(params[15].duty);
+            motor_li.setDutyLimit(params[15].duty);
+            while(get_dist_back() < params[15].condition) {
 //                straight();
-                if(RightOrLeft == 0) climbAndInfinity(-90,15);
-                else climbAndInfinity(90,15);
+                if(RightOrLeft == 0) climbAndInfinity(params[15].argument[0], params[15].argument[1]);
+                else climbAndInfinity(-params[15].argument[0], params[15].argument[1]);
             }
+            
             for(int i=0; i<1; ++i) {
-                while(get_dist_forward() > 60) {
+                while(get_dist_forward() > params[16].condition) {
                     //straight();
-                    if(RightOrLeft == 0) climbAndInfinity(-90,15);
-                    else climbAndInfinity(90,15);
+                    if(RightOrLeft == 0) climbAndInfinity(params[16].argument[0], params[16].argument[1]);
+                    else climbAndInfinity(-params[16].argument[0], params[16].argument[1]);
                 }
             }
             hand_mode = GOAL;
@@ -282,8 +305,8 @@
     //以下ファイル操作の中身
     int i;
     while (fscanf(fp, "%f,%f,%f,%f", &params[i].argument[0], &params[i].argument[1], &params[i].duty,&params[i].condition) != EOF) {
+        printf("%f,%f,%f,%f\r\n",params[i].argument[0],params[i].argument[1],params[i].duty,params[i].condition);
         i++;
-        printf("%f,%f,%f,%f\r\n",params[i].argument[0],params[i].argument[1],params[i].duty,params[i].condition);
     }
     fclose(fp);//ファイル操作終わり
 }
\ No newline at end of file