4/2にちゃんと動いたやつ

Dependencies:   mbed SBDBT arrc_mbed

main.cpp

Committer:
gennnisi
Date:
2022-04-02
Revision:
12:e96b0757577d
Parent:
11:9ae7fe40a807

File content as of revision 12:e96b0757577d:

#include "mbed.h"
#include "scrp_slave.hpp"
#include "sbdbt.hpp"
#include "rotary_inc.hpp"

int main()
{
//---------------初期設定---------------
    //scrp_slave
    ScrpSlave scrp(PC_12,PD_2,PH_1,0x0807f800);

    //sbdbt
    sbdbt sb(PA_0,PA_1);

    //scrp_slaveのid設定
    const int wheel_num = 1;
    const int BeanbagGet_num = 2;
    const int BeanbagShot_num = 3;
    const int TakoGetPut_num = 4;
    const int TapeLed_num = 5;
    const int send_all = 255;

    //コマンド番号設定
    //足回り用
    const int r_x_component_cmd = 1;
    const int r_y_component_cmd = 2;
    const int l2_cmd = 3;
    const int r2_cmd = 4;
    const int l1_cmd = 5;
    const int r1_cmd = 8;
    const int circle_cmd = 6;
    const int switch_cmd = 7;
    const int l_x_component_cmd = 9;
    const int l_y_component_cmd = 10;

    //ビーンバッグ回収用
    const int move = 1;

    //ビーンバッグ発射用
    const int shot = 2;

    //農作物回収・設置用
    const int up = 1;
    const int down = 2;
    /*const int limit1 = 3;
    const int limit2 = 4;
    const int limit3 = 5;
    const int limit4 = 6;*/

    //テープLED用
    const int voice = 1;
    const int start = 2;

    //遠隔非常停止受信用

    //全体送信用
    const int select = 50;//手動・自動切り替え
    const int stop_all = 51;//全プログラム停止

    //変数
    bool auto_mode = false;
    bool status_start = 0;
    bool auto_move = 0;
    bool field = 0;
    int r_x_component;
    int r_y_component;
    int l_x_component;
    int l_y_component;
    int l2_num;
    int r2_num;

    DigitalIn status_stop(PB_14,PullUp);
    DigitalIn sw1(PB_6);
    sw1.mode(PullUp);
    
    DigitalOut led(PA_10);
    
    //RotaryInc rot1(PC_0,PC_1,M_PI*2,40*M_PI,4);
    //RotaryInc rot2(PC_3,PC_2,M_PI*2,40*M_PI,4);

    //count
    int count_up;
    int count_down;
    int count_shot;
    int count_auto;
    int count_start;
    int count_stop;
    int count_circle;
//-------------------------------------

    while(true) {
        printf("%d\n",sw1.read());
        sb.button_state();
        r_x_component = sb.rsx();
        r_y_component = sb.rsy();
        l_x_component = sb.lsx();
        l_y_component = sb.lsy();
        l2_num = sb.l2An();
        r2_num = sb.r2An();
        if(sb.select() == 1) {
            count_auto++;
            printf("%d\n",count_auto);
            if(count_auto == 1) {
                auto_mode =! auto_mode;
            }
        } else {
            count_auto = 0;
        }
        if(auto_mode == 1) {
                led = 1;
                scrp.send1(TapeLed_num,voice,4);
                //printf("auto_mode\n");
            }else{
                led = 0;
                //printf("manual_mode\n");
        }

        //足回り
        scrp.send1(wheel_num,r_x_component_cmd,r_x_component);
        wait_ms(2);
        scrp.send1(wheel_num,r_y_component_cmd,r_y_component);
        wait_ms(2);
        scrp.send1(wheel_num,l2_cmd,l2_num);
        wait_ms(2);
        scrp.send1(wheel_num,r2_cmd,r2_num);
        wait_ms(2);
        scrp.send1(wheel_num,l1_cmd,sb.l1());
        wait_ms(2);
        scrp.send1(wheel_num,r1_cmd,sb.r1());
        wait_ms(2);
        scrp.send1(wheel_num,l_x_component_cmd,l_x_component);
        wait_ms(2);
        scrp.send1(wheel_num,l_y_component_cmd,l_y_component);
        wait_ms(2);
        if(auto_mode == true){
            if(sb.circle() == 1) {
                count_circle++;
            if(count_circle == 1) {
                auto_move =! auto_move;
                scrp.send1(BeanbagShot_num,circle_cmd,auto_move);
                wait_ms(2);
            }
        } else {
            count_circle = 0;
        }
            scrp.send1(wheel_num,circle_cmd,auto_move);
            wait_ms(2);
        }
        
        if(sw1.read() == 1){
            field = 0;
        }else{
            field = 1;
        }
        scrp.send1(wheel_num,switch_cmd,field);
        wait_ms(2);

        //ビーンバッグ回収
        if(auto_mode == false) {
            scrp.send1(TapeLed_num,1,sb.square());
            wait_ms(2);
        }

        //ビーンバッグ発射
        if(auto_mode == true) {
            scrp.send1(BeanbagShot_num,shot,sb.square());
            wait_ms(2);
            if(sb.square() == 1) {
                count_shot++;
                if(count_shot == 1) {
                    scrp.send1(TapeLed_num,voice,5);
                }
            } else {
                count_shot = 0;
            }
        }
        
        //装填
        if(auto_mode == false) {
            scrp.send1(BeanbagShot_num,10,sb.cross());
            wait_ms(2);
        }
        
        if(auto_mode == true){
            if(sb.cross() == 1){
                scrp.send1(TapeLed_num,1,10);
                wait_ms(2);      
            } 
        }

        //農作物回収・設置
        if(sb.up() == 1) {
            count_up++;
            if(count_up == 1) {
                scrp.send1(TapeLed_num,voice,3);
            }
        } else {
            count_up = 0;
        }
        if(sb.down() == 1) {
            count_down++;
            if(count_down == 1) {
                scrp.send1(TapeLed_num,voice,2);
            }
        } else {
            count_down = 0;
        }
        scrp.send1(TakoGetPut_num,up,sb.up());
        wait_ms(2);
        scrp.send1(TakoGetPut_num,down,sb.down());
        wait_ms(2);

        //テープLED
        if(sb.start() == 1) {
            count_auto++;
            if(count_start == 1) {
                status_start =! status_start;
            }
        } else {
            count_start = 0;
        }
        scrp.send1(TapeLed_num,start,status_start);
        wait_ms(2);

        //全体送信
        scrp.send1(send_all,select,auto_mode);//自動・手動モード切り替え
        wait_ms(2);
        if(status_stop.read() == 1) {
            count_stop++;
            if(count_stop <= 100000){
            scrp.send1(send_all,stop_all,1);
            }
        } else {
            scrp.send1(send_all,stop_all,0);
            count_stop = 0;
        }
        scrp.send1(send_all,stop_all,0);
        wait_ms(2);
    }
}