Diff: main.cpp

Revision:

0:333ed669a96a

Child:

1:bb93f8b91191

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun Feb 24 13:33:47 2019 +0000
@@ -0,0 +1,239 @@
+#include "mbed.h"
+#include "stm32f103c8t6.h"
+const int ID = 0;
+
+CANMessage msg;
+CAN Reseive (PA_11, PA_12);
+InterruptIn A(PB_0);//A0
+DigitalIn B(PB_1);//A1
+Ticker pid;
+
+/*-----モーター---*/
+//z軸
+PwmOut pwmZ(PB_3);//D3
+DigitalOut z1(PB_4);//D4
+DigitalOut z2(PA_15);//D5
+//y軸
+PwmOut pwmY(PA_8);//D3
+DigitalOut y1(PB_14);//D4
+DigitalOut y2(PB_15);//D5
+
+
+DigitalOut led(PC_13);
+
+DigitalIn switch1(PA_1);//A2
+DigitalIn switch2(PA_2);//A2
+DigitalIn switch3(PA_3);//A2
+
+/*-----PID変数----*/
+const float Kp = 0.0016;
+const float Ki = 0.0;
+const float Kd = 0.0;
+float p,i,d;
+float prep = 0.0;
+float goal = 0.0;//目標回転数
+
+
+long Count = 0;
+float val = 0.0;
+const float interval = 0.002;
+const float basisPowY = 0.2;
+const float basisPowZ = 0.2;
+
+int ymode = 0;
+
+bool finishFlagY = false;
+bool finishFlagZ = false;
+
+/*----受信割り込み---*/
+void inter(){
+    Reseive.read(msg);
+    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;
+    }else if(msg.id == (ID+4)){//フラッグチェック用
+        char p;
+        if(finishFlagY == 1 && finishFlagZ == 1){
+            p = 1;
+        }else{
+            p = 0;
+        }
+        Reseive.write( CANMessage( ID+4, &p, 1) );
+    }      
+}
+
+void upZ(){
+    if(ID == 0 || ID == 3){
+        z1 = 0;
+        z2 = 1;
+    }else{
+        z1 = 1;
+        z2 = 0;
+    }
+}
+
+void downZ(){
+    if(ID == 0 || ID == 3){
+        z1 = 1;
+        z2 = 0;
+    }else{
+        z1 = 0;
+        z2 = 1;
+    }
+}
+
+void forwardY(){
+    if(ID == 0 || ID == 2){
+        y1 = 1;
+        y2 = 0;
+    }else{
+        y1 = 0;
+        y2 = 1;
+    }
+}
+void backY(){
+    if(ID == 0 || ID == 2){
+        y1 = 0;
+        y2 = 1;
+    }else{
+        y1 = 1;
+        y2 = 0;
+    }
+}
+void stopZ(){
+    z1 = 1;
+    z2 = 1;
+}
+void stopY(){
+    y1 = 1;
+    y2 = 1;
+}
+void Arise(){
+    if(int(B) == 0){
+        Count++;
+    }else{
+        Count--;
+    }
+        
+}
+
+void Afall(){
+    if(int(B) == 0){
+        Count--;
+    }else{
+        Count++;
+    }
+        
+}
+
+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;
+        }
+        
+        //z1 = 0;
+        //z2 = 1;
+        
+    }else if(output < 0){
+        upZ();
+        
+        //downZ();
+    }else{
+        stopZ();
+        output = 0;
+        finishFlagZ = true;
+    }
+    if(output > (1 - basisPowZ)){
+        output = 1 - basisPowZ;
+    }else if(output < (basisPowZ - 1)){
+        output = basisPowZ -1;
+    }
+    if(p <= 3 && p >= -3){
+        stopZ();
+        output = 0;
+        finishFlagZ = true;
+    }
+    output = abs(output);
+    pwmZ = basisPowZ + output;
+}
+
+int main(){
+    led = 1;
+    Reseive.frequency(100000);
+    Reseive.attach( &inter);
+    switch1.mode(PullDown);
+    switch2.mode(PullDown);
+    switch3.mode(PullDown);
+    pwmZ.period_us(50);
+    pwmY.period_us(50);
+    
+    //初期位置
+    pwmZ = basisPowZ;
+    downZ();
+    pwmY = basisPowY;
+    stopY();
+    while(1){
+        if(switch1.read() == 0){
+            stopZ();
+            break;
+        }
+    }
+    pid.attach(&Update,interval);
+    A.mode(PullUp);
+    B.mode(PullUp);
+    A.rise(&Arise);
+    A.fall(&Afall);
+    //led = 0;
+    while (true) {
+        if(finishFlagY == false){
+            if(ymode == 1){
+                if(switch2.read() == 0){
+                    stopY();
+                    finishFlagY = true;
+                }else{//前へ
+                    forwardY();
+                }
+            }else{
+                if(switch3.read() == 0){
+                    stopY();
+                    finishFlagY = true;
+                }else{//後ろへ
+                    backY();
+                }
+            }
+        }else{
+            stopY();
+        }           
+    }
+}
\ No newline at end of file