File content as of revision 0:ee7e9405e1c7:

#include "mbed.h"
#include "math.h"
#include "mechanam.h"
#include "Reset.h"

Serial Twelite(D5,D4,115200);

Mechanam gx_d_l(D7,D12);//1 #1/3N
Mechanam fx_d_l(A3,D10);//2 #3/2
Mechanam gx_d_r(D9,D3);//3 #1/1
Mechanam fx_d_r(D2,D8);//4 #16/1

Mbed_MD elev(A2,D11);

DigitalOut fire(D6);

char buffer[12];
double x;
double y;
double x_t;
double y_t;
int8_t SEL;
int8_t SEL_T;
bool error_fa=false;
bool check=false;
bool Yadama=false;
bool Yadama_check=false;
Timer error_time;

Reset_Mbed Reset_t;

void receive()
{
    for (int i=0; i<=11; i++) {
        buffer[i]=Twelite.getc();
        if (error_fa==true and buffer[0]!='[')return;
    }
    if (buffer[0]=='[' and buffer[11]=='\0' and int16_t(buffer[9])<=1 and int16_t(buffer[10])<=1) {
        int16_t x_get=(buffer[1]<<8)+buffer[2];
        int16_t y_get=(buffer[3]<<8)+buffer[4];
        int16_t turnx_get=(buffer[5]<<8)+buffer[6];
        int16_t turny_get=(buffer[7]<<8)+buffer[8];
        SEL=buffer[9];
        SEL_T=buffer[10];
        x=(double)(x_get)/1000.0;
        y=(double)(y_get)/1000.0;
        x_t=(double)(turnx_get)/1000.0;
        y_t=(double)(turny_get)/1000.0;
        check=true;
        error_fa=false;
        error_time.reset();
    } else {
        check=false;
        error_fa=true;
    }
}

int main()
{
    Twelite.attach(receive,Serial::RxIrq);

    while(1) {
        // ----------------ここから矢玉の処理----------------
        if ( (SEL_T==1) and Yadama==false) Yadama=true;
        if ( (SEL_T==0) and Yadama==true) Yadama_check=true;
        while (Yadama_check==true) {
            fx_d_r.free();
            fx_d_l.free();
            gx_d_r.free();
            gx_d_l.free();

            fire=(SEL==1 ? 1:0);

            if (-0.10<y_t and y_t<0.10)y_t=0;
            elev.drive(-y_t);

            printf("mode:Y SEL=%d,Y=%3.2f\n\r",SEL,-y_t);
            
            if (error_fa==true and error_time.read()==0)error_time.start();
            if (error_time.read()>=0.5f)Twelite.putc('e');
            Reset_t.Reset(error_time.read(),1.0);
            
            if ( (SEL_T==1) and Yadama==true)Yadama=false;
            if ( (SEL_T==0) and Yadama==false)Yadama_check=false;
        }
        // ----------------ここまで矢玉の処理----------------

        if(error_fa==true and error_time.read()==0)error_time.start();
        
        // --------------ここからメカナムの処理---------------

        if (check==true) {
            elev.free();
            fx_d_l.drive(x,y,x_t,2.8,1.5,1.0,1.0,1.0,'f');
            fx_d_r.drive(x,y,x_t,2.8,1.5,1.0,1.0,1.0,'f');
            gx_d_l.drive(x,y,x_t,2.8,1.5,1.0,1.0,1.0,'g');
            gx_d_r.drive(x,y,x_t,2.8,1.5,1.0,1.0,1.0,'g');
            printf("mode:M x=%3.2f y=%3.2f x_t=%3.2f\n\r",x,y,x_t);
            check=false;
            error_fa=true;
        } else {
            error_fa=true;
        }

        if (error_time.read()>=0.5f)Twelite.putc('e');
        Reset_t.Reset(error_time.read(),1.0);
        // --------------ここまでメカナムの処理---------------
    }
}