nhk2019
Dependencies: mbed mbed-STM32F103C8T6
Diff: main.cpp
- Revision:
- 0:333ed669a96a
- Child:
- 1:bb93f8b91191
diff -r 000000000000 -r 333ed669a96a main.cpp --- /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