File content as of revision 1:bb93f8b91191:

#include "mbed.h"
#include "stm32f103c8t6.h"
const int ID = 3;

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.0006;
const float Ki = 0.001;
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.25;
const float basisPowZ = 0.25;

int ymode = 0;

bool finishFlagY = false;
bool finishFlagZ = false;

/*----受信割り込み---*/
void inter(){
    Reseive.read(msg);
    if(msg.id == ID){
        finishFlagY = false;
        finishFlagZ = false;
        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)){
            p = 1;
        }else{
            p = 0;
        }
        Reseive.write( CANMessage( ID+14, &p, 1) );
        //wait_ms(5);
    }      
}

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(){
    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){
        downZ();
        if(switch1.read() == 0){
            stopZ();
            output = 0;
            //Count = 0;
        }
    }else if(output < 0){
        upZ();
        led = 0;
    }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;
    }else{
        //finishFlagZ = false;
    }
    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();
                    //finishFlagY = false;
                }
            }else{
                if(switch3.read() == 0){
                    stopY();
                    finishFlagY = true;
                }else{//後ろへ
                    backY();
                    //finishFlagY = false;
                }
            }
        }else{
            stopY();
        }
        if(switch1.read() == 0){
            Count = 0;
        }
    }
}