Revision 31:ce079259f559, committed 2021-10-31

Comitter:

THtakahiro702286

Date:

Sun Oct 31 13:45:53 2021 +0000

Parent:

30:00a513858a44

Commit message:

10/31 last parameter

Changed in this revision

--- a/control_parameter.h	Sat Oct 30 07:56:14 2021 +0000
+++ b/control_parameter.h	Sun Oct 31 13:45:53 2021 +0000
@@ -13,14 +13,14 @@
 //振り子振る間隔
 #define FIRST_WAIT          0.15
 #define SECOND_WAIT         0.25
-#define THIRD_WAIT          0.3
+#define THIRD_WAIT          0.35
 
-#define FURIKO_STOP_LINE    80     //ふり幅
+#define FURIKO_STOP_LINE    73     //ふり幅
 #define SET_WAIT            0.15   //ロリコン依存で振り切った後の待ち時間
 #define SECOND_LINE         1      //firstからsecondに移行するための時間
 #define THIRD_LINE          2      //secondからthirdに移行するための時間
-#define JUMP_TIME           12     //飛ぶために必要そうな時間
-#define JUMP_LINE           -40    //飛ぶために必要そうなエンコーダーのタイミング
+#define JUMP_TIME           14.5     //飛ぶために必要そうな時間
+#define JUMP_LINE           -45    //飛ぶために必要そうなエンコーダーのタイミング
 #define RELEASE_LINE        70     //離す瞬間のZ軸の加速度、重力に逆らうので98より小さくなるはず
 //ロープ検知の閾値
 #define PH_LINE1            0.1

--- a/main.cpp	Sat Oct 30 07:56:14 2021 +0000
+++ b/main.cpp	Sun Oct 31 13:45:53 2021 +0000
@@ -26,7 +26,7 @@
 lpf mSp(MOT_C_TIME, MOT_A_TIME);
 Servo servo[3] = {SERVO1, SERVO2, SERVO3};
 IR2302 ir2302(MD_A,MD_B);//梅沢追加　IR2302
-Timer setWait,nextWait,jumpWait;
+Timer setWait,nextWait,jumpWait,buzzerTime;
 AnalogIn ph[3] = {PHOTO1,PHOTO2,PHOTO3};
 DigitalOut light[3] = {ROPE1,ROPE2,ROPE3};
 DigitalIn ub(USER_BUTTON);
@@ -43,9 +43,12 @@
     jy.calibrateAll(50);
  
     int nowPalse;
+    int zeroPalse=0;
     
     float phI[3] = {0};//フォトインタラプタ
     bool rope = 0;
+    
+    float freq = 3000;//ブザーのやつ
 
     bool setFlag   = 1;//1
     bool firstFlag = 0;//0
@@ -56,6 +59,7 @@
     bool catchFlag = 0;//0
     int8_t way     = 1;//1or-1
     bool resetFlag = 0;//0
+    bool buzzerFlag= 0;//0
     float fiveYAcc[5]={0};
     
     float servoAngle[3];
@@ -96,13 +100,15 @@
         j++;
 */
     //ロータリーエンコーダ 向き変ったので-1掛け
-        nowPalse = encoder.getPulses() * -1;
+        nowPalse = (encoder.getPulses() + zeroPalse) * -1;
         
     //紐センサ
         for(i=0; i<3; i++) phI[i]=ph[i];
         
 //処理部
 
+//        buzzerFlag = 0;
+
     //セッティング
         if(setFlag)
         {
@@ -111,11 +117,15 @@
             else if(!ab && ub) motor=0;
             else if(!ub && ab)
             {
+                motor = 0;
                 setFlag = 0;
                 firstFlag = 1;
-                buzzer.beep(3000,1.0);
+                buzzer.beep(freq,1.0);
+//                buzzerFlag=1;
+                zeroPalse = nowPalse;
                 wait(5);
                 buzzer.nobeep();
+//                buzzerFlag = 0;
             }
 
         }
@@ -130,6 +140,7 @@
                 way *= -1;
                 nextWait.start();
                 setWait.start();
+//                buzzerFlag=1;
                 swingFlag = 0;
                 if(nextWait > SECOND_LINE)                        //一定時間経過したらsecondに移行
                 {
@@ -158,6 +169,7 @@
                 way *= -1;
                 setWait.start();
                 swingFlag = 0;
+//                buzzerFlag = 1;
                 if(nextWait > THIRD_LINE)                        //一定時間経過したらthirdに移行
                 {
                     secondFlag   = 0;
@@ -186,6 +198,7 @@
                 nextWait.start();
                 setWait.start();
                 swingFlag = 0;
+//                buzzerFlag = 1;
             }
             if(setWait > THIRD_WAIT)
             {
@@ -193,6 +206,13 @@
                 setWait.reset();
                 swingFlag = 1;
             }
+/*
+            if(jumpWait > JUMP_TIME)
+            {
+                buzzerFlag = 1;
+                freq = 5000;
+            }
+*/
             if((jumpWait > JUMP_TIME) && (nowPalse < JUMP_LINE) && (nowPalse > JUMP_LINE-10) && (way == -1))             //一定時間たったら前に飛ぶ
             {
                 flyFlag = 1;
@@ -234,7 +254,7 @@
     //掴んだ後
         if(resetFlag)
         {
-            if(nowPalse < NEXT_PALSE * l)
+            if(nowPalse < NEXT_PALSE)
             {
                 motor=0.35;
             }
@@ -286,18 +306,28 @@
     
     //モーター
         ir2302.SetMotorSpeed(motor);
-      
+/*        
+    //ブザー
+        if(buzzerFlag)
+        {
+            buzzer.beep(freq,0.5);
+        }
+        else
+        {
+            buzzer.nobeep();
+        }
+*/      
 //表示系
 //        pc.printf("%.2f,%.2f,%.2f",angle[0],angle[1],angle[2]);
-        pc.printf("%.2f,%.2f,%.2f,",servoAngle[0],servoAngle[1],servoAngle[2]);
-        pc.printf("%.4f,",motor);
+//        pc.printf("%.2f,%.2f,%.2f,",servoAngle[0],servoAngle[1],servoAngle[2]);
+//        pc.printf("%.4f,",motor);
 //        pc.printf("%.2f,%.2f,",abac[1],abac[2]);
 //        pc.printf("%.2f,%.2f,",lpfY,lpfZ);
-//        pc.printf("%d,",nowPalse);
-        pc.printf("%d,",setFlag);
+        pc.printf("%d,",nowPalse);
+//        pc.printf("%d,",setFlag);
 //        pc.printf("%d,",resetFlag);
 //        pc.printf("%.2f,",lpfM);
-//        pc.printf("%.3f,%.3f,%.3f,",phI[0],phI[1],phI[2]);
+        pc.printf("%.3f,%.3f,%.3f,",phI[0],phI[1],phI[2]);
 //        pc.printf("%d,",k);
         pc.printf("\r\n");