ロボステ6期 / Mbed 2 deprecated NHK2020-arm-sub4-3-4

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed KondoServoLibrary ros_lib_kinetic

main.cpp

Committer:
yuki0701
Date:
2019-12-24
Revision:
4:f1f638a6b6c5
Parent:
3:9cef6e58a462
Child:
5:f9dd9346bfd4

File content as of revision 4:f1f638a6b6c5:

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

//#define NHK2020_TEST_MODE
//#define NHK2020_MAIN_MODE
#define NHK2020_MAIN_MODE2
CAN can1(p30,p29);

Ticker can_ticker;  //can用ticker

Ec4multi EC_backdrop(p15,p16,RESOLUTION);
SpeedControl backdrop(p22,p21,50,EC_backdrop);
Serial pc(USBTX,USBRX);
DigitalOut snatch(p8);
DigitalOut pass1(p27);
DigitalOut pass2(p28);
DigitalOut pakapaka1(p14);
//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();





#ifdef NHK2020_TEST_MODE
    while(1) {
        printf("T2 = %d\n\r",T2);
        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");
    //printf("T2 = %d\n\r",T2);
    if(T2 == 0) {
        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");
                T2=1;
                break;

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

    if(T2 == 2) {  //ボール掴んで投げる
        //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 == 3) {  //スタートゾーンに戻る（アーム待機）
        while(1) {
            printf("a-T = 2  T2 = %d  t2_r = %d\n\r",T2,t2_r);
            wait(0.1);
            if(T2 == 4) {
                break;
            }
        }
    }
    printf("end\n\r");

#endif
#ifdef NHK2020_MAIN_MODE2
    can1.frequency(1000000);
    can_ticker.attach(&can_read,0.01);
//printf("T2 = %d\n\r",T2);
    if(T2 == 0) {
        //T2=1;
        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");
                T2=1;
                break;

            }
        }
    }
    if(T2 == 1) {  //スタート位置からボール前まで移動（アーム待機）
        //T2=2;
        pass2=0;
        while(1) {
            if(T2 == 2) {
                break;
            }
        }
    }

    if(T2 == 2) {  //ボール掴んで投げる
        //pakapaka=1;
        //T2=3;

        while(1) {
            if(T2 == 3) {
                break;
            }
        }
    }
    if(T2 == 3) {  //スタートゾーンに戻る（アーム待機）
        //T2=4;
        while(1) {
            if(T2 == 4) {
                break;
            }
        }
    }
    if(T2 == 4) {  //kikk
        //T2=5;
        T2++;
        while(1) {
            if(T2 == 5) {
                break;
            }
        }
    }
    

    if(T2 == 5) {  //ボール掴んで投げる
        while(1) {
            if(T2 == 6) {
                break;
            }
        }
    }
    if(T2 == 6) {  //スタートゾーンに戻る（アーム待機）
        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);

        pass1=1;
        pass2=1;
        //wait(5);
        T2++;
        //T2=7;
        while(1) {
            if(T2 == 7) {
                break;
            }
        }
    }
    if(T2 == 7) {  //スタート位置からボール前まで移動（アーム待機）
        //T2=8;

        pass2=0;
        while(1) {
            if(T2 == 8) {
                break;
            }
        }
    }

    if(T2 == 8) {  //ボール掴んで投げる
        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);

        pass1=1;
        pass2=1;

        T2++;
        //T2=9;
        while(1) {
            if(T2 == 9) {
                break;
            }
        }
    }
    if(T2 == 9) {  //スタートゾーンに戻る（アーム待機）
        pass2=0;
        //T2=10;
        while(1) {
            if(T2 == 10) {
                break;
            }
        }
    }
    if(T2 == 10) {  //ボール掴んで投げる
        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++;
        //T2=9;
        while(1) {
            if(T2 == 11) {
                break;
            }
        }
    }
    if(T2 == 11) {  //スタートゾーンに戻る（アーム待機）
        //T2=10;
        while(1) {
            if(T2 == 12) {
                break;
            }
        }
    }
#endif
#ifdef NHK2020_MAIN_MODE3
    can1.frequency(1000000);
    can_ticker.attach(&can_read,0.01);
//printf("T2 = %d\n\r",T2);
    if(T2 == 0) {
        //T2=1;
        while(1) {
            if(T2 == 1) {
                break;
            }
        }
    }
    if(T2 == 1) {  //スタート位置からボール前まで移動（アーム待機）
        //T2=2;
        while(1) {
            if(T2 == 2) {
                break;
            }
        }
    }

    if(T2 == 2) {  //ボール掴んで投げる
        pakapaka=1;
        //T2=3;
        while(1) {
            if(T2 == 3) {
                break;
            }
        }
    }
    if(T2 == 3) {  //スタートゾーンに戻る（アーム待機）
        //T2=4;
        while(1) {
            if(T2 == 4) {
                break;
            }
        }
    }
    if(T2 == 4) {  //スタート位置からボール前まで移動（アーム待機）
        pakapaka=0;
        //T2=5;
        while(1) {
            if(T2 == 5) {
                break;
            }
        }
    }


#endif




}