File content as of revision 1:bc34fdc4e16b:

#include "mbed.h"
#include "EC.h" //Encoderライブラリをインクルード
#include "SpeedController.h"
#define RESOLUTION 2048//分解能

//#define NHK2020_TEST_MODE
#define NHK2020_MAIN_MODE

CAN can1(p30,p29);

Ticker can_ticker;  //can用ticker

Ec4multi EC_backdrop(p15,p16,RESOLUTION);
SpeedControl backdrop(p21,p22,50,EC_backdrop);
Serial pc(USBTX,USBRX);
DigitalOut snatch(p8);
DigitalOut pass1(p27);
DigitalOut pass2(p28);
//Ticker motor_tick;  //角速度計算用ticker

char t2[1]= {0}; //動作番号（送信する値（char型））
int t2_r=0, T2=0; //動作番号（受け取った値、送信する値（int型））

double a=0;//now
double b=0;//target
double turn=0;
int k = 0;
void tsukami()
{
    b=1.3;
}
void put()
{
    b=-0.7;
}
void top()
{
    b=0;
}
void move_motor()
{
    while(1) {
        double old_turn=turn;
        a=EC_backdrop.getRad();
        if(a-b>=0.1) {
            turn=0.1;
            pc.printf("F");
        } else if (a-b>=0.05) {
            turn=10*(a-b)*(a-b);
            pc.printf("f");
        } else if (b-a>=0.1) {
            turn=-0.1;
            pc.printf("B");
        } else if (b-a>=0.05) {
            turn=-10*(a-b)*(a-b);
            pc.printf("b");
        } else {
            backdrop.stop();
            backdrop.turn(0);
            turn=0;
            pc.printf("s");
        }
        if(turn*old_turn<0)turn=0;
        backdrop.turn(turn);
        pc.printf("%lf",EC_backdrop.getRad());
        if(b-a<0.05&&a-b<0.05)break;
    }
}

int q = 0;

void can_read()
{


    CANMessage msg;

    if(can1.read(msg)) {

        if(msg.id == 1) {
            t2_r = msg.data[0];
            //printf("arm T2 = %d  t2_r = %d\n\r",T2,t2_r);
        } else {
            if(q > 100) {
                //printf("id1 fale\n\r");
                q = 0;
            }
            q++;
        }

    } else {
        if(k > 100) {
            //printf("arm fale\n\r");
            k = 0;
        }
        k++;
    }

    t2[0] = T2;

    if(can1.write(CANMessage(2,t2,1))) {
    }

    if(t2_r > T2) {
        T2 = t2_r;
    }


}


int main()
{
    pc.printf("setting please");
    move_motor();
    while(1) {




#ifdef NHK2020_TEST_MODE

        pc.printf("%lf",EC_backdrop.getRad());
        if(pc.readable()) {
            char sel=pc.getc();
            if(sel=='z') {
                pc.printf("z\r\n");
                tsukami();
            } else if(sel=='x') {
                pc.printf("x\r\n");
                put();
            } else if(sel=='c') {
                pc.printf("c\r\n");
                top();
            }
            /*            if(sel=='q') {
                            printf("\r\n");
                            if(denjiben==0)denjiben=1;
                            else denjiben=0;
                        }*/
            if(sel=='1') {
                snatch=0;
                printf("snatch_off\r\n");
            }
            if(sel=='2') {
                snatch=1;
                printf("snatch_on\r\n");
            }
            if(sel=='3') {
                pass1=0;
                printf("pass1_off\r\n");
            }
            if(sel=='4') {
                pass1=1;
                printf("pass1_on\r\n");
            }
            if(sel=='5') {
                pass2=0;
                printf("pass2_off\r\n");
            }
            if(sel=='6') {
                pass2=1;
                printf("pass2_on\r\n");
            }
            //     if(sel=='a') {
            //         //pc.printf("x\r\n");
            //         //put();
            //     }
        }
    }
#endif
#ifdef NHK2020_MAIN_MODE
    can1.frequency(1000000);
    can_ticker.attach(&can_read,0.01);
    printf("wait_mode\r\n");
    while(1) {
        char sel=pc.getc();
        if(sel=='1') {
            snatch=0;
            printf("snatch_off\r\n");
        }
        if(sel=='2') {
            snatch=1;
            printf("snatch_on\r\n");
        }
        if(sel=='3') {
            pass1=0;
            printf("pass1_off\r\n");
        }
        if(sel=='4') {
            pass1=1;
            printf("pass1_on\r\n");
        }
        if(sel=='5') {
            pass2=0;
            printf("pass2_off\r\n");
        }
        if(sel=='6') {
            pass2=1;
            printf("pass2_on\r\n");
        }
        if(sel=='q') {
            printf("end_wait_mode\r\n");
            break;

        }
    }

    if(T2 == 0) {  //スタート位置からボール前まで移動（アーム待機）
        T2=1;
        while(1) {
            //printf("a-T = 0  T2 = %d  t2_r = %d\n\r",T2,t2_r);
            wait(0.1);
            if(T2 == 1) {
                break;
            }
        }
    }

    if(T2 == 1) {  //ボール掴んで投げる
        //printf("a-T = 1  T2 = %d  t2_r = %d\n\r",T2,t2_r);
        snatch=1;
        tsukami();
        printf("tsukami");
        move_motor();
        wait(3);
        snatch=0;
        wait(1);
        put();
        printf("put");
        move_motor();
        wait(3);
        snatch=1;
        wait(1);
        top();
        printf("top");
        move_motor();
        wait(5);
        pass1=0;
        wait(5);
        //wait(5);

        T2++;
    }
    if(T2 == 2) {  //スタートゾーンに戻る（アーム待機）
        while(1) {
            //printf("a-T = 2  T2 = %d  t2_r = %d\n\r",T2,t2_r);
            wait(0.1);
            if(T2 == 1) {
                break;
            }
        }
    }
    printf("end\n\r");
}
#endif
}