nhk2019
Dependencies: mbed mbed-STM32F103C8T6
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