File content as of revision 1:be4238d819bb:

#include "mbed.h"

Serial pc( USBTX, USBRX);
CAN can( PA_11, PA_12);
CANMessage msg;
DigitalOut led (PA_5);
DigitalIn start(PB_13);


typedef union{
    float val;
    char binary[4];
}transFloat;

//P,I,D,YPOWER,ZPOWERの順
const float parameter[4][5] = {
    {0.0006, 0.001, 0.0, 0.25, 0.25},
    {0.0006, 0.001, 0.0, 0.30, 0.25},
    {0.0006, 0.001, 0.0, 0.25, 0.25},
    {0.0006, 0.001, 0.0, 0.25, 0.25}};
    

bool finishFlags[4] = {false, false, false, false};

void checkFlags(int id){
    char data = 10;
    if(!finishFlags[id]){
        can.write( CANMessage((id + 4),&data,1) );
        while(1){
            if(can.read(msg)){
                break;
            }
        }
        if(msg.id == id+14){
            if(msg.data[0] == 0){
                finishFlags[msg.id - 14] = false;
            }else{
                finishFlags[msg.id - 14] = true;
            }
        }
    }
}


void waitReach(int id){
    while(1){
        checkFlags(id);
        pc.printf("%d \n",finishFlags[id]);
        if(finishFlags[id] == true){
            break;
        }
    }
}

void allWaitReach(){
    for(int i=0;i<4;i++){
        waitReach(i);
    }
}

void step(int id, int y, bool z){
    char data[2] = {y,z};
    can.write(CANMessage(id,data,2) );
    finishFlags[id] = false;
    wait_ms(2);
}

int main() {
    wait(1);
    start.mode(PullUp);
    can.frequency(100000);
    led.write(0);
    
    
    
    int count = 0;
    while(1){
        if(start.read() == 0){
            count++;
        }
        if(count>= 100){
            break;
        }
    }
    
    step(0,10,1);
    step(1,10,0);
    step(2,10,0);
    step(3,10,1);
    allWaitReach();
    
    wait(2.0);
    while (true) {
        /*
        //led.write(1);
        step(0,20,1);
        step(1,200,1);
        step(2,20,0);
        step(3,200,0);
        waitReach(0);
        waitReach(1);
        waitReach(2);
        waitReach(3);
        //pc.printf("ok \n");
        //wait(3.0);
        step(0,200,0);
        step(1,20,0);
        step(2,200,1);
        step(3,20,1);
        
        waitReach(0);
        waitReach(1);
        waitReach(2);
        waitReach(3);
        */
        //pc.printf("ok \n");
        
        
        step(1,200,0);
        step(2,200,0);
        waitReach(1);
        waitReach(2);
        
        step(0,10,0);
        step(1,200,1);
        step(2,200,1);
        step(3,10,0);
        waitReach(0);
        waitReach(1);
        waitReach(2);
        waitReach(3);
        
        step(1,10,1);
        step(2,10,1);
        waitReach(1);
        waitReach(2);
        
        step(0,200,0);
        step(3,200,0);
        waitReach(0);
        waitReach(3);
        
        step(0,200,1);
        step(1,10,0);
        step(2,10,0);
        step(3,200,1);
        waitReach(0);
        waitReach(1);
        waitReach(2);
        waitReach(3);
        
        step(0,10,1);
        step(3,10,1);
        waitReach(0);
        waitReach(3);
        
        
        //wait(3.0);
        //pc.printf("%d \t",val);
        
    }
}