File content as of revision 6:dd35cc3b5ca0:

//2021/12/20更新
//aoki
#include "yuka.h"
Timer t_obj;
double auto_time = 0;
int main()
{
    pc.attach(SerialRX);
    //pc.baud(115200);
    //CAN
    canPort.frequency(1000000); //Bit Rate:1MHz
    canPort.attach(CANdataRX,CAN::RxIrq);
    //CAN node Setting
    int node1 = 1;
    int node2 = 2;
    int node3 = 3;
    int node4 = 4;

    //エンコーダ関係
    int ActPos1 = 0;
    int ActPos2 = 0;
    
    int Init_Pos = 0;
    //超音波センサ関係パラメータ
    int dist1;
    int dist2;
    int dist3;

    pc.printf("YUKA PROGRAM START\r\n");
    wait(0.1);
    //-------------起動時に必ず送信---------------
    //オペレーティングモードを送信
    sendOPModeT(node1);
    sendOPModeT(node2);
    sendOPModeT(node3);
    sendOPModeT(node4);
    //Shutdown,Enableコマンド送信｜リセット
    sendCtrlSD(node1);
    sendCtrlSD(node2);
    sendCtrlSD(node3);
    sendCtrlSD(node4);

    sendCtrlEN(node1);
    sendCtrlEN(node2);
    sendCtrlEN(node3);
    sendCtrlEN(node4);

    //初期加減速度
    int Acc = 2000;
    int Dec = 2000;
    sendProAcc(1,Acc);
    sendProAcc(2,Acc);
    sendProAcc(3,Acc);
    sendProAcc(4,Acc);

    sendProDec(1,Dec);
    sendProDec(2,Dec);
    sendProDec(3,Dec);
    sendProDec(4,Dec);
    //トルク設定
    int trq = 100;   //torque Setting[mA]

    sendTgtTrq(1,trq);
    sendTgtTrq(2,trq);
    sendTgtTrq(3,trq);
    sendTgtTrq(4,trq);

    int state_1 = 0;
    int state_2 = 0;
    int ride_count = 0;

    int X_POS_TMP = 0;
    int dist1_ori = 0;
    int dist2_ori = 0;

    readActPos(1);
    ActPos=canmsgRx.data[4]+canmsgRx.data[5]*256+canmsgRx.data[6]*65536;
    if(ActPos > 8388608) {
        ActPos -= 0x1000000;//16進数でのマイナス処理 16^7 = 16,777,216 = 0x1000000
    }
    Init_Pos = ActPos;//起動時の角度を保存
    t_obj.reset();
    t_obj.start();
    //set_MODE_T();
    printf("\nstart\r\n");
    while(1) {
        auto_time = t_obj.read();
        pc.printf("state_1:%d  state_2:%d\r\n",state_1,state_2);
        LED.printf("%d",state_1);
        readActPos(1);
        ActPos=canmsgRx.data[4]+canmsgRx.data[5]*256+canmsgRx.data[6]*65536;
        if(ActPos > 8388608) {
            ActPos -= 0x1000000;//16進数でのマイナス処理 16^7 = 16,777,216 = 0x1000000
            //printf("check\r\n");
        }
        dist1 = get_cm_n(u1, 3);
        dist2 = get_cm_n(u2, 3);
        dist3 = get_cm_n(u3, 3);
        /*--------------------------*/
        if(state_1 == 0) { //入力判断フェーズ
            state_2 = 0;
            if(ride_count >= 2 && auto_time > Standby_Time) {
                state_1 = 20;
            } else {
                if(ActPos < (Init_Pos - KICK)) { //前入力検出
                    ride_count++;
                    state_1 = 1;
                } else if(ActPos > (Init_Pos + KICK)) { //右入力検出
                    ride_count++;
                    state_1 = 11;
                } else {
                    set_MODE_T();
                }
            }

        } else if(state_1 == 1) { //前進→壁検出フェーズ
            if(dist1 < WALL && dist1 >= WALL_MIN) {
                vel_stop();
                wait(GETOFF_TIME);
                state_1 = 2;
            } else {
                set_ACC(ACC_RIDE);//加速度設定
                set_DEC(DEC_CLEAN);//減速度設定
                set_MODE_V();//速度制御モード送信
                vel_forward_con(RPM_RIDE);//前進速度指令
            }
        } else if(state_1 == 2) { //前進からの帰還フェーズ
            if(ActPos > -3000) {
                vel_stop();
                t_obj.reset();
                t_obj.start();
                state_1 = 0;
                wait(1.0);
            } else {
                vel_backward_con(RPM_RIDE);
            }
        } else if(state_1 == 11) { //右進→壁検出フェーズ
            if(dist3 < WALL && dist3 >= WALL_MIN) {
                vel_stop();
                wait(GETOFF_TIME);
                state_1 = 12;
            } else {
                set_ACC(ACC_RIDE);//加速度設定
                set_DEC(DEC_RIDE);//減速度設定
                set_MODE_V();//速度制御モード送信
                vel_right(RPM_RIDE);//R進速度指令
            }
        } else if(state_1 == 12) { //右進からの帰還フェーズ
            if(ActPos < 3000) {
                vel_stop();
                t_obj.reset();
                t_obj.start();
                state_1 = 0;
                wait(1.0);
            } else {
                vel_left(RPM_RIDE);
            }
        } else if(state_1 == 20) { //消毒モード
            if(state_2 == 0) {
                if(dist1 < WALL && dist1 >= WALL_MIN) {
                    X_POS_TMP = ActPos;
                    state_2 = 1;
                } else {
                    set_ACC(ACC_CLEAN);//加速度設定
                    set_DEC(DEC_CLEAN);//減速度設定
                    set_MODE_V();//速度制御モード送信
                    vel_forward_con(RPM_CLEAN);//前進速度指令
                }
            } else if(state_2 == 1) {
                if(abs(ActPos - X_POS_TMP) > CLEAN_OFFSET) {
                    state_2 == 2;
                } else {
                    if(dist3 < WALL && dist3 >= WALL_MIN) {
                        state_2 = 4;
                    } else {
                        set_ACC(ACC_CLEAN);//加速度設定
                        set_DEC(DEC_CLEAN);//減速度設定
                        set_MODE_V();//速度制御モード送信
                        vel_right(RPM_CLEAN);//右進速度指令
                    }
                }
            } else if(state_2 == 2) {
                if(ActPos > -3000) {
                    state_2 = 3;
                } else {
                    set_ACC(ACC_CLEAN);//加速度設定
                    set_DEC(DEC_CLEAN);//減速度設定
                    set_MODE_V();//速度制御モード送信
                    vel_backward_con(RPM_CLEAN);//後進速度指令
                }
            } else if(state_2 == 3) {
                if(abs(ActPos - X_POS_TMP ) > CLEAN_OFFSET) {
                    state_2 == 0;
                } else {
                    if(dist3 < WALL && dist3 >= WALL_MIN) {
                        state_2 = 4;
                    } else {
                        set_ACC(ACC_CLEAN);//加速度設定
                        set_DEC(DEC_CLEAN);//減速度設定
                        set_MODE_V();//速度制御モード送信
                        vel_right(RPM_CLEAN);//右進速度指令
                    }
                }
            } else if(state_2 == 4) {
                if(ActPos < 3000) {
                    state_2 = 5;
                } else {
                    vel_left(RPM_CLEAN);
                }
            } else if(state_2 == 5) {
                if(ActPos > -3000) {
                    t_obj.reset();
                    t_obj.start();
                    state_1 = 0;
                    state_2 = 0;
                } else {
                    vel_backward_con(RPM_CLEAN);
                }
            }
        }
    }
}