Diff: main.cpp

Revision:

1:bb93f8b91191

Parent:

0:333ed669a96a

--- a/main.cpp	Sun Feb 24 13:33:47 2019 +0000
+++ b/main.cpp	Sat Mar 02 01:10:45 2019 +0000
@@ -1,6 +1,6 @@
 #include "mbed.h"
 #include "stm32f103c8t6.h"
-const int ID = 0;
+const int ID = 3;
 
 CANMessage msg;
 CAN Reseive (PA_11, PA_12);
@@ -26,8 +26,8 @@
 DigitalIn switch3(PA_3);//A2
 
 /*-----PID変数----*/
-const float Kp = 0.0016;
-const float Ki = 0.0;
+const float Kp = 0.0006;
+const float Ki = 0.001;
 const float Kd = 0.0;
 float p,i,d;
 float prep = 0.0;
@@ -37,8 +37,8 @@
 long Count = 0;
 float val = 0.0;
 const float interval = 0.002;
-const float basisPowY = 0.2;
-const float basisPowZ = 0.2;
+const float basisPowY = 0.25;
+const float basisPowZ = 0.25;
 
 int ymode = 0;
 
@@ -51,25 +51,19 @@
     if(msg.id == ID){
         finishFlagY = false;
         finishFlagZ = false;
-        int y = int(msg.data[0]);
-        int z = int(msg.data[1]);
-        goal = float(-1*y);
-        /*
-        if(ID == 0 || ID == 3){//右前足、左後足
-            goal = float(-1*y);
-        }else{
-            goal = float(y);
-        }
-        */
-        ymode = z;
+        int z = int(msg.data[0]);
+        int y = int(msg.data[1]);
+        goal = float(-1*z);
+        ymode = y;
     }else if(msg.id == (ID+4)){//フラッグチェック用
         char p;
-        if(finishFlagY == 1 && finishFlagZ == 1){
+        if((finishFlagY == 1) && (finishFlagZ == 1)){
             p = 1;
         }else{
             p = 0;
         }
-        Reseive.write( CANMessage( ID+4, &p, 1) );
+        Reseive.write( CANMessage( ID+14, &p, 1) );
+        //wait_ms(5);
     }      
 }
 
@@ -138,41 +132,30 @@
 }
 
 void Update(){
-    /*
-    if(ID == 1 || ID == 2){
-        Count = Count * -1;
-    }
-    */
     long now = Count;
     if(ID == 1 || ID == 2){
         now = now * -1;
     }
-    
     p = goal - float(now);//比例
     i += p * interval;//積分
     d = (p - prep)/interval;//微分
     prep = p;
     float output = (Kp*p + Ki*i + Kd*d);
     if(output > 0){
-        led = 0;
-        //upZ();
         downZ();
         if(switch1.read() == 0){
             stopZ();
             output = 0;
+            //Count = 0;
         }
-        
-        //z1 = 0;
-        //z2 = 1;
-        
     }else if(output < 0){
         upZ();
-        
-        //downZ();
+        led = 0;
     }else{
         stopZ();
         output = 0;
         finishFlagZ = true;
+        
     }
     if(output > (1 - basisPowZ)){
         output = 1 - basisPowZ;
@@ -183,6 +166,8 @@
         stopZ();
         output = 0;
         finishFlagZ = true;
+    }else{
+        //finishFlagZ = false;
     }
     output = abs(output);
     pwmZ = basisPowZ + output;
@@ -223,6 +208,7 @@
                     finishFlagY = true;
                 }else{//前へ
                     forwardY();
+                    //finishFlagY = false;
                 }
             }else{
                 if(switch3.read() == 0){
@@ -230,10 +216,14 @@
                     finishFlagY = true;
                 }else{//後ろへ
                     backY();
+                    //finishFlagY = false;
                 }
             }
         }else{
             stopY();
-        }           
+        }
+        if(switch1.read() == 0){
+            Count = 0;
+        }
     }
 }
\ No newline at end of file