Diff: main.cpp

Revision:

48:d9f3ce701aca

Parent:

38:89d2a9e6c96f

Child:

49:6337717484fe

--- a/main.cpp	Wed May 15 09:57:11 2019 +0000
+++ b/main.cpp	Sat May 18 09:12:33 2019 +0000
@@ -3,16 +3,31 @@
 #include "microinfinity.h"
 #include "sensors.h"
 #include "moves.h"
+#include <stdlib.h>
+
+
+#define BADRULEMODE
 
 void StateFlow(int i);
 void SetMode();
 void Start();
 void NoHandSignal();
+void LoadParameter();
 
 int start_state=0;
+LocalFileSystem SeqFile("Local");
+
+struct Param {
+    float argument[2];
+    float duty;
+    float condition;
+};
+struct Param params[17]= {}; //とりあえずいっぱい作った
 
 int main()
 {
+    LoadParameter();
+
     //setup関連
     can1.frequency(1000000);
     SetupMoves();
@@ -23,6 +38,7 @@
         printf("switch 1:%d  2:%d  3:%d  LR:%d \r\n", switch_modes[0].read(),switch_modes[1].read(),switch_modes[2].read(),switch_LR.read());
         wait(0.1);
     }*/
+
     printf("hand read:%d\r\n",hand.read());
 
     printf("reset standby\r\n");
@@ -42,7 +58,7 @@
 //    mode選択
     Start();
     //SetMode();
-
+#ifndef BADRULEMODE
     if(RightOrLeft == 0) {
         if(start_state == 0) theta0 = -degree/100.0;
         else if(start_state == 1) theta0 = -degree/100.0 - 45;
@@ -56,6 +72,39 @@
         else if(start_state == 3) theta0 = -degree/100.0 - 90;
         else printf("state_error");
     }
+#endif
+#ifdef BADRULEMODE
+    if(RightOrLeft == 0) {
+        if(start_state == 0) theta0 = -degree/100.0;
+        else if(start_state == 1) {
+            theta0 = -degree/100.0 + 90;
+            motor_lo.setDutyLimit(0.6);//0.5
+            motor_li.setDutyLimit(0.6);
+            turn(35.0);
+        } else if(start_state == 2) {
+            theta0 = -degree/100.0 - 135;
+            motor_lo.setDutyLimit(0.6);//0.5
+            motor_li.setDutyLimit(0.6);
+            turn(100.0);
+        } else if(start_state == 3) theta0 = -degree/100.0 + 90;
+        else printf("state_error");
+    } else {
+        if(start_state == 0) theta0 = -degree/100.0;
+        else if(start_state == 1) {
+            theta0 = -degree/100.0 - 45;
+            //段差前旋回
+            motor_lo.setDutyLimit(0.6);//0.5
+            motor_li.setDutyLimit(0.6);
+            turn(-35.0);
+        } else if(start_state == 2) {
+            theta0 = -degree/100.0 + 135;
+            motor_lo.setDutyLimit(0.6);//0.5
+            motor_li.setDutyLimit(0.6);
+            turn(-100.0);
+        } else if(start_state == 3) theta0 = -degree/100.0 - 90;
+        else printf("state_error");
+    }
+#endif
     FileOpen();
     printf("LR:%d StateFlow:%d\r\n", RightOrLeft,start_state);
     StateFlow(start_state);
@@ -74,13 +123,13 @@
             }
             printf("get distance mode");
             for(int i=0; i<1; ++i) {
-                while(Hcsr04BackWithEc() < 320) {//300
+                while(get_dist_back() < 320) {//300
                     straightAndInfinity(0, 7);
                 }
             }
             //段差前旋回
-            motor_lo.setDutyLimit(0.5);//0.5
-            motor_li.setDutyLimit(0.5);
+            motor_lo.setDutyLimit(0.6);//0.5
+            motor_li.setDutyLimit(0.6);
 
             if(RightOrLeft == 0) turn(35.0);
             else turn(-35.0);
@@ -100,8 +149,8 @@
             for(int i= 0; i<14; ++i) straight();
             while(get_dist_back() > 80) straight();
             //段差後旋回
-            motor_lo.setDutyLimit(0.5);//0.5
-            motor_li.setDutyLimit(0.5);
+            motor_lo.setDutyLimit(0.6);//0.5
+            motor_li.setDutyLimit(0.6);
 
             if(RightOrLeft == 0) turn(85.0);
             else turn(-85.0);
@@ -116,14 +165,17 @@
                 else straightAndInfinity(-90.0, 7.0);
             }
             for(int i=0; i<3; ++i) {
-                while(get_dist_forward() > 60) {
+                while(get_dist_forward() > 65) {
                     if(RightOrLeft == 0) straightAndInfinity(90.0, 7.0);
                     else straightAndInfinity(-90.0, 7.0);
                 }
             }
             //ロープ前旋回
             printf("before roop deg:%.3f\r\n",degree0);
-            turn(0);
+
+            if(RightOrLeft == 0) turn(5.0);
+            else turn(-5.0);
+
             //ロープ位置ジャストまで前進
             for(int i=0; i<3; ++i) {
                 straightAndInfinity(0.0, 7.0);
@@ -136,8 +188,8 @@
             phasing(0.0);
             //ロープ
             while(switch_modes[2].read() == 0) {
-                straightAndInfinity(0, 7);
-                //straight();
+                //straightAndInfinity(0, 7);
+                straight();
             }
             printf("uhai stand by ok!!!!!!!!!!\r\n");
             NoHandSignal();
@@ -146,8 +198,8 @@
 
             if(RightOrLeft == 0) theta0 = -degree/100.0+90;
             else theta0 = -degree/100.0-90;
-            motor_lo.setDutyLimit(0.2);//0.3
-            motor_li.setDutyLimit(0.2);
+            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);
@@ -183,6 +235,7 @@
         SetMode();
         hand_mode = G_OPEN;
         stop();//ここで開くことをcanでslaveに送る
+        wait(1);
         wait_gerege();
         hand_mode = G_CLOSE;
         HandMove();
@@ -218,4 +271,19 @@
     while(get_dist_back() < 40.0) {
         wait(0.01);
     }
+}
+void LoadParameter()
+{
+    FILE *fp;
+    if ((fp = fopen("/Local/params.csv", "r")) == NULL) {//ファイル操作はじめかつエラー処理
+        printf("file open error!!\n");
+        exit(EXIT_FAILURE);
+    }
+    //以下ファイル操作の中身
+    int i;
+    while (fscanf(fp, "%f,%f,%f,%f", &params[i].argument[0], &params[i].argument[1], &params[i].duty,&params[i].condition) != EOF) {
+        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