nhk2019
Dependencies: mbed mbed-STM32F103C8T6
main.cpp
- Committer:
- kenken0721
- Date:
- 2019-02-24
- Revision:
- 0:333ed669a96a
- Child:
- 1:bb93f8b91191
File content as of revision 0:333ed669a96a:
#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(); } } }