File content as of revision 0:6e2abd0956f1:

#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()
{
    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;
        //break;
        //pc.printf("s");
    }
    if(turn*old_turn<0)turn=0;
    backdrop.turn(turn);
}

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()
{
    move();

    pc.printf("setting please");
    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);
        tsukami();
        move();
        wait(3);
        snatch=0;
        wait(1);
        put();
        wait(3);
        snatch=1;
        wait(1);
        top();
        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
}