Orewotaosita - If you wont to knock me down, look this file...

Users » Ryosei » Code » Orewotaosita
Miya Miyagawa / Mbed 2 deprecated Orewotaosita
If you wont to knock me down, look this file...
System/Process/Process.cpp

Committer:: Ryosei
Date:: 2019-05-03
Revision:: 0:4df75b08b14a
File content as of revision 0:4df75b08b14a:




/*
このプログラムを見る人へ

用語解説

ご挨拶モード：足回りでロボットを左右に振りご挨拶しつつ、回路リセットを行うおれたおになくてはならないモード
足回り：宮川は十字キー足回りを、新村はアナログスティック足回りを担当しました。宮川のプログラムは猫でも書けますが、新村のは難易度S＋です。わからないことがあれば新村へ

わからないことがあればアナログスティック以外、なんでも宮川に聞いてください
*/




//------------------------------------------------------------------------------------------------------------------------------------------
#include "Process.h"

#include "mbed.h"
#include "../../Communication/XBee/XBee.h"
//#include "../../Input/Switch/Switch.h"
#include "../../Output/Motor/Motor.h"
#include "../../Output/Servo/Servo.h"

//_____________________
/*---------------- HOW TO WRITE ----------------/

    ・motor の割り当てを決める
        #define TIRE_L 1

    ・リミットスイッチの割り当てを決める
        #define ARM_L 1

    ・他にも自由に定義してもいいです (pwmとか)

/---------------- HOW TO WRITE ----------------*/
//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓


#define TIRE_1 0
#define TIRE_2 1
#define TIRE_3 2
#define ARM    3

#define ARMlim 0

int Air0=0;
int Air1=1;

#define a 0
#define b 1
#define c 2

int a_array[15][15] = {
    {   -30,   -20,   -10,     0,     0,     0,   0,   0,   0,    0,    0,    0,   10,   20,   30 },
    {   -40,   -30,   -20,   -10,     0,     0,   0,   0,   0,    0,    0,   10,   20,   30,   40  },
    {   -50,   -40,   -30,   -20,   -10,     0,   0,   0,   0,    0,   10,   20,   30,   40,   50  },
    {   -60,   -50,   -40,   -30,   -20,   -10,   0,   0,   0,   10,   20,   30,   40,   50,   60  },
    {   -70,   -60,   -50,   -40,   -30,   -20, -10,   0,  10,   20,   30,   40,   50,   60,   70  },
    {   -80,   -70,   -60,   -50,   -40,   -30, -20,   0,  20,   30,   40,   50,   60,   70,   80  },
    {   -90,   -80,   -70,   -60,   -50,   -40, -30,   0,  30,   40,   50,   60,   70,   80,   90  },
    {   -100,   -90,   -80,   -70,   -60,   -50, -40,   0,  40,   50,   60,   70,   80,   90,  100  },
    {   -90,   -80,   -70,   -60,   -50,   -40, -30,   0,  30,   40,   50,   60,   70,   80,   90  },
    {   -80,   -70,   -60,   -50,   -40,   -30, -20,   0,  20,   30,   40,   50,   60,   70,   80  },
    {   -70,   -60,   -50,   -40,   -30,   -20, -10,   0,  10,   20,   30,   40,   50,   60,   70  },
    {   -60,   -50,   -40,   -30,   -20,   -10,   0,   0,   0,   10,   20,   30,   40,   50,   60  },
    {   -50,   -40,   -30,   -20,   -10,     0,   0,   0,   0,    0,   10,   20,   30,   40,   50  },
    {   -40,   -30,   -20,   -10,     0,     0,   0,   0,   0,    0,    0,   10,   20,   30,   40  },
    {   -30,   -20,   -10,     0,     0,     0,   0,   0,   0,    0,    0,    0,   10,   20,   30  }
};

int b_array[15][15] = {
    {  100,   92,   84,   76,   68,   60,   52,  100,   36,   28,   20,   12,   4,    0,   0   },
    {   92,   84,   76,   68,   60,   52,   44,   36,   28,   20,   12,    4,   0,   0,   0   },
    {   84,   76,   68,   60,   52,   44,   36,   28,   20,   12,    4,   0,   0,   0,   -4  },
    {   76,   68,   60,   52,   44,   36,   28,   20,   12,    4,   0,   0,   0,    -4,   -12 },
    {   68,   60,   52,   44,   36,   28,   20,   12,    4,   0,   0,   0,   -4,   -12,   -20 },
    {   60,   52,   44,   36,   28,   20,   12,    4,    0,   0,   0,   -4,   -12,   -20,   -28 },
    {   52,   44,   36,   28,   20,   12,    4,   0,   0,   0,    -4,   -12,   -20,   -28,   -36 },
    {  100,   36,   28,   20,   12,    4,   0,   0,   0,    -4,   -12,   -20,   -28,   -36,   -44 },
    {   85,   28,   20,   12,    4,   0,   0,   0,    -4,   -12,   -20,   -28,   -36,   -44,   -52 },
    {   70,   20,   12,    4,   0,   0,   0,   -4,   -12,   -20,   -28,   -36,   -44,   -52,   -60 },
    {   55,   12,    4,   0,   0,   0,    -4,   -12,   -20,   -28,   -36,   -44,   -52,   -60,   -68 },
    {   40,    4,   0,   0,   0,    -4,   -12,   -20,   -28,   -36,   -44,   -52,   -60,   -68,   -76 },
    {   25,   0,   0,   0,    -4,   -12,   -20,   -28,   -36,   -44,   -52,   -60,   -68,   -76,   -84 },
    {   10,   0,   0,    -4,   -12,   -20,   -28,   -36,   -44,   -52,   -60,   -68,   -76,   -84,   -92 },
    {    0,   0,   -4,   -12,   -20,   -28,  -36,   -100,   -52,   -60,   -68,   -76,   -84,   -92,  -100 }
};


int c_array[15][15] = {
    {   0,   0,    4,    12,   20,   28,   36,  100,   52,   60,   68,   76,   84,   92,  100  },
    {   0,   0,    0,     4,   12,   20,   28,   36,   44,   52,   60,   68,   76,   84,   92  },
    {   -4,   0,    0,     0,    4,   12,   20,   28,   36,   44,   52,   60,   68,   76,   84  },
    {   -12,   -4,    0,     0,   0,    4,   12,   20,   28,   36,   44,   52,   60,   68,   76  },
    {   -20,   -12,   -4,     0,   0,   0,    4,   12,   20,   28,   36,   44,   52,   60,   68  },
    {   -28,   -20,   -12,    -4,   0,   0,    0,    4,   12,   20,   28,   36,   44,   52,   60  },
    {   -36,   -28,   -20,   -12,    -4,   0,    0,   0,    4,   12,   20,   28,   36,   44,   52  },
    {   -44,   -36,   -28,   -20,   -12,    -4,    0,   0,    0,   4,   12,   20,   28,   36,   44  },
    {   -52,   -44,   -36,   -28,   -20,   -12,     4,   0,    0,   0,   4,   12,    20,   28,   36  },
    {   -60,   -52,   -44,   -36,   -28,   -20,   -12,    -4,    0,   0,   0,   4,   12,   20,   28  },
    {   -68,   -60,   -52,   -44,   -36,   -28,   -20,   -12,    -4,   0,   0,   0,   4,   12,   20  },
    {   -76,   -68,   -60,   -52,   -44,   -36,   -28,   -20,   -12,    -4,   0,   0,   0,   4,   12  },
    {   -84,   -76,   -68,   -60,   -52,   -44,   -36,   -28,   -20,   -12,   -4,   0,   0,   0,    4  },
    {   -92,   -84,   -76,   -68,   -60,   -52,   -44,   -36,   -28,   -20,   -12,    -4,   0,   0,    0  },
    {  -100,   -92,   -84,   -76,   -76,   -60,  -100,  -100,   -36,   -28,   -20,   -12,  -4,   0,    0  }
};

int pwm_array[15] = {   30,   25,   20,   15,   10,   5,   0,   0,   0,   -5,   -10,   -15,   -20,   -25,   -30 };

#define usiro 0
#define mae 0
uint8_t motorData[5];
uint8_t pwmData[5];

int conlx;
int conly;
int conrx;
int conry;
int conba;
int conbb;
int conbx;
int conby;
int homes=0;
//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑
//_____________________

//#define USE_USB_SERIAL
#ifdef USE_USB_SERIAL
Serial pc(SERIAL_TX, SERIAL_RX);
#endif
Serial pc(SERIAL_TX,SERIAL_RX);
XBEE::ControllerData *controller;
MOTOR::MotorStatus motor[MOUNTING_MOTOR_NUM];
//////////////////////////////////
//関数、タイマーの宣言
DigitalOut Air[]= {
    DigitalOut(A5),
    DigitalOut(A4),
};
DigitalIn limit[]= {
    DigitalIn(A7),
    DigitalIn(A6),
};
Timer Airtime;
Timer home;
void AirOut(int pin,int mode)
{
    Air[pin]=mode;
}
bool LimitRead(int pin)
{
    int x;
    x=limit[pin];
    if(x==0) {
        return false;
    } else if(x==1) {
        return true;
    } else {
        return false;
    }
}
//関数、タイマーの宣言
//////////////////////////////////
//using namespace SWITCH;

void SystemProcess(void)
{
    while(true) {
        controller = XBEE::Controller::GetData();
//____________________________
        /*------------------------ HOW TO WRITE ------------------------/

            ここにメインのプログラムを書く

            ・コントローラから受け取ったデータをもとに動作のプログラムを書く
             (コントローラのデータは controller-> で取る)

                if(controller->Button.RIGHT) {
                    motor[TIRE_L].dir = FOR;
                    motor[TIRE_R].dir = BACK;
                    motor[TIRE_L].pwm = 12.3;
                    motor[TIRE_R].pwm = 12.3;
                }

             motor[0].dirは     FOR   (正転)
                                BACK  (逆転)
                                BRAKE (ブレーキ)
                                FREE  (フリー)

             motor[0].pwmは     0.0(%) ~ 100.0(%)

             controllerは       XBee.hの構造体の中身

             (AnalogL・Rを使いたかったら、頑張って考える or 聞いてください)

            ・リミットスイッチの値をもとに動作のプログラムを書く

                if(Switch::CheckPushed(ARM_L))
                {
                    if(controller->Button.L)
                    {
                        motor[ARM].dir = FOR;
                        motor[ARM].pwm = 80.0;
                    }
                    if(motor[ARM].dir == BACK)
                    {
                        motor[ARM].dir = BRAKE;
                    }
                }

             →関数 Switch::CheckPushed の引数はリミットスイッチの名前 (limitSw[0]みたいな), 返り値はbool型 (true or false)

            ・他にもやりたいことがあったら自由にどうぞ

            ps.わからないことがあったら聞いてください

        /------------------------ HOW TO WRITE ------------------------*/
//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓

        float precision; //精密モード変数
        float s=Airtime.read();//エアータイム
        float hometime=home.read();//ホームタイム（ご挨拶モード兼回路リセット）
        float goaisatsu_time=0.2;//ご挨拶タイム
        //////////////////////////////////////////////////////////////////////////////////
        //ご挨拶モード

        if(controller->Button.HOME) {
            home.start();
            homes=1;
        }

        if((hometime<=goaisatsu_time)&&(!(homes==0))) {
            homes=2;
        } else if(hometime<=2*goaisatsu_time) {
            homes=1;
        } else if(hometime<=3*goaisatsu_time) {
            homes=2;
        } else if(hometime<4*goaisatsu_time) {
            homes=1;
        } else if(hometime<5*goaisatsu_time) {
            homes=0;
            home.stop();
            home.reset();
        } else {
            homes=0;
            home.stop();
            home.reset();
        }

        //ご挨拶モード
        ////////////////////////////////////////////////////////////////////////////////////

        ////////////////////////////////////////////////////////////////////////////////////
        //精密モード
        if(controller->Button.ZR) {
            precision=0.5;
        } else {
            precision=1.0;
        }
        //精密モード
        ////////////////////////////////////////////////////////////////////////////////////

        ////////////////////////////////////////////////////////////////////////////////////
        //アーム機構
        //motor[ARM].pwm=100;

        if(controller->Button.X) {
            if(LimitRead(0)){
                motor[ARM].dir = BACK;
                motor[ARM].pwm = 100;
            }else{
                motor[ARM].dir = FREE;
                motor[ARM].pwm = 100;
            }
        }else if(controller->Button.Y) {
            motor[ARM].dir = FOR;
            motor[ARM].pwm = 100;
        }else{
            motor[ARM].dir = FREE;
            motor[ARM].pwm = 100;
        }

            //アーム機構
            ////////////////////////////////////////////////////////////////////////////////////

            ////////////////////////////////////////////////////////////////////////////////////
            //十字キー足回り（宮川ver）

            if(controller->Button.L) {
                motor[TIRE_1].dir = FOR;
                motor[TIRE_2].dir = FOR;
                motor[TIRE_3].dir = FOR;
                motor[TIRE_1].pwm = 100*precision;
                motor[TIRE_2].pwm = 100*precision;
                motor[TIRE_3].pwm = 100*precision;
            } else if(controller->Button.R) {
                motor[TIRE_1].dir = BACK;
                motor[TIRE_2].dir = BACK;
                motor[TIRE_3].dir = BACK;
                motor[TIRE_1].pwm = 100*precision;
                motor[TIRE_2].pwm = 100*precision;
                motor[TIRE_3].pwm = 100*precision;
            } else if(controller->Button.RIGHT) {
                motor[TIRE_1].dir = BACK;
                motor[TIRE_2].dir = FOR;
                motor[TIRE_3].dir = FOR;
                motor[TIRE_1].pwm = 100*precision;
                motor[TIRE_2].pwm = 60*precision;
                motor[TIRE_3].pwm = 60*precision;
            } else if(controller->Button.LEFT) {
                motor[TIRE_1].dir = FOR;
                motor[TIRE_2].dir = BACK;
                motor[TIRE_3].dir = BACK;
                motor[TIRE_1].pwm = 100*precision;
                motor[TIRE_2].pwm = 60*precision;
                motor[TIRE_3].pwm = 60*precision;
            } else if(controller->Button.DOWN) {
                motor[TIRE_1].dir = BRAKE;
                motor[TIRE_2].dir = FOR;
                motor[TIRE_3].dir = BACK;
                motor[TIRE_1].pwm = 100;
                motor[TIRE_2].pwm = 100*precision;
                motor[TIRE_3].pwm = 100*precision;
            } else if(controller->Button.UP) {
                motor[TIRE_1].dir = BRAKE;
                motor[TIRE_2].dir = BACK;
                motor[TIRE_3].dir = FOR;
                motor[TIRE_1].pwm = 100;
                motor[TIRE_2].pwm = 100*precision;
                motor[TIRE_3].pwm = 100*precision;
            } else if((controller->Button.UP) && (controller->Button.RIGHT)) {
                motor[TIRE_1].dir = BACK;
                motor[TIRE_2].dir = BRAKE;
                motor[TIRE_3].dir = FOR;
                motor[TIRE_1].pwm = 100*precision;
                motor[TIRE_2].pwm = 100;
                motor[TIRE_3].pwm = 100*precision;
            } else if((controller->Button.UP) && (controller->Button.LEFT)) {
                motor[TIRE_1].dir = FOR;
                motor[TIRE_2].dir = BACK;
                motor[TIRE_3].dir = BRAKE;
                motor[TIRE_1].pwm = 100*precision;
                motor[TIRE_2].pwm = 100*precision;
                motor[TIRE_3].pwm = 100;
            } else if((controller->Button.DOWN) && (controller->Button.RIGHT)) {
                motor[TIRE_1].dir = BACK;
                motor[TIRE_2].dir = FOR;
                motor[TIRE_3].dir = BRAKE;
                motor[TIRE_1].pwm = 100*precision;
                motor[TIRE_2].pwm = 100*precision;
                motor[TIRE_3].pwm = 100;
            } else if((controller->Button.DOWN) && (controller->Button.LEFT)) {
                motor[TIRE_1].dir = FOR;
                motor[TIRE_2].dir = BRAKE;
                motor[TIRE_3].dir = BACK;
                motor[TIRE_1].pwm = 100*precision;
                motor[TIRE_2].pwm = 100;
                motor[TIRE_3].pwm = 100*precision;
            } else if(homes==1) {
                motor[TIRE_1].dir = FOR;
                motor[TIRE_2].dir = FOR;
                motor[TIRE_3].dir = FOR;
                motor[TIRE_1].pwm = 70*precision;
                motor[TIRE_2].pwm = 70*precision;
                motor[TIRE_3].pwm = 70*precision;
            } else if(homes==2) {
                motor[TIRE_1].dir = BACK;
                motor[TIRE_2].dir = BACK;
                motor[TIRE_3].dir = BACK;
                motor[TIRE_1].pwm = 70*precision;
                motor[TIRE_2].pwm = 70*precision;
                motor[TIRE_3].pwm = 70*precision;
            } else {
                motor[TIRE_1].dir=BRAKE;
                motor[TIRE_2].dir=BRAKE;
                motor[TIRE_3].dir=BRAKE;
                motor[TIRE_1].pwm=100;
                motor[TIRE_2].pwm=100;
                motor[TIRE_3].pwm=100;
            }
            //十字キー足回り（みやがわver）
            ///////////////////////////////////

            ///////////////////////////////////
            //課題１
            if(controller->Button.A) {
                AirOut(Air0,1);
            } else if(controller->Button.B) {
                AirOut(Air0,0);
            }

            if(controller->Button.ZL) {
                Airtime.start();
                AirOut(Air1,0);
            }
            if(s>1) {
                Airtime.stop();
                Airtime.reset();
                AirOut(Air1,1);
            }
            //課題１
            ///////////////////////////////////

            /////////////////////////////////////////////////////////////////////////////////////
            //アナログスティック足回り（新村ver）

            conlx = controller->AnalogL.X;
            conly = controller->AnalogL.Y;
            conrx = controller->AnalogR.X;
            conry = controller->AnalogR.Y;
            if((!(controller->Button.RIGHT))&&(!(controller->Button.DOWN))&&
                    (!(controller->Button.LEFT))&&(!(controller->Button.UP))&&
                    (!(controller->Button.L))&&(!(controller->Button.R))&&
                    (!(controller->Button.HOME))) {
                if(((conlx >6) && (conly <8)) ||((conrx >6)&&(conry <8))) {
                    motor[a].dir = BRAKE;
                    motor[b].dir = BRAKE;
                    motor[c].dir = BRAKE;
                    motor[a].pwm = 100.0;
                    motor[b].pwm = 100.0;
                    motor[c].pwm = 100.0;
                }

                if(a_array[conly][conlx]<0) {
                    motor[a].pwm = -1*a_array[conly][conlx]*precision;
                    motor[a].dir = FOR;
                } else if(a_array[conly][conlx] > 0) {
                    motor[a].pwm = a_array[conly][conlx]*precision;
                    motor[a].dir = BACK;
                } else if(a_array[conly][conlx] == 0) {
                    if(conry < 6) {
                        motor[a].dir = BACK;
                        motor[b].dir = BACK;
                        motor[c].dir = BACK;
                        motor[a].pwm = 100.0*precision;
                        motor[b].pwm = 100.0*precision;
                        motor[c].pwm = 100.0*precision;
                    } else if(conry > 8) {
                        motor[a].dir = FOR;
                        motor[b].dir = FOR;
                        motor[c].dir = FOR;
                        motor[a].pwm = 100.0*precision;
                        motor[b].pwm = 100.0*precision;
                        motor[c].pwm = 100.0*precision;
                    } else {
                        motor[a].dir = BRAKE;
                        motor[b].dir = BRAKE;
                        motor[c].dir = BRAKE;
                        motor[a].pwm = 100.0;
                        motor[b].pwm = 100.0;
                        motor[c].pwm = 100.0;
                    }
                }

                if(b_array[conly][conlx] <0 ) {
                    motor[b].pwm = -1*b_array[conly][conlx]*precision;
                    motor[b].dir = FOR;
                } else if(b_array[conly][conlx] > 0) {
                    motor[b].pwm = b_array[conly][conlx]*precision;
                    motor[b].dir = BACK;
                } else if(a_array[conly][conlx] == 0 ) {
                    if(conrx < 6 ) {
                        motor[a].dir = BACK;
                        motor[b].dir = BACK;
                        motor[c].dir = BACK;
                        motor[a].pwm = 100.0*precision;
                        motor[b].pwm = 100.0*precision;
                        motor[c].pwm = 100.0*precision;
                    } else if(conrx > 8) {
                        motor[a].dir = FOR*precision;
                        motor[b].dir = FOR*precision;
                        motor[c].dir = FOR*precision;
                        motor[a].pwm = 100.0*precision;
                        motor[b].pwm = 100.0*precision;
                        motor[c].pwm = 100.0*precision;
                    } else {
                        motor[a].dir = BRAKE;
                        motor[b].dir = BRAKE;
                        motor[c].dir = BRAKE;
                        motor[a].pwm = 100.0;
                        motor[b].pwm = 100.0;
                        motor[c].pwm = 100.0;
                    }
                }


                if(c_array[conly][conlx] <0 ) {
                    motor[c].pwm =-1*c_array[conly][conlx]*precision;
                    motor[c].dir = BACK;
                } else if(c_array[conly][conlx] > 0) {
                    motor[c].pwm = c_array[conly][conlx]*precision;
                    motor[c].dir = FOR;
                } else if(a_array[conly][conlx] == 0 ) {
                    if(conrx < 6 ) {
                        motor[a].dir = BACK;
                        motor[b].dir = BACK;
                        motor[c].dir = BACK;
                        motor[a].pwm = 100.0*precision;
                        motor[b].pwm = 100.0*precision;
                        motor[c].pwm = 100.0*precision;
                    } else if(conrx > 8) {
                        motor[a].dir = FOR;
                        motor[b].dir = FOR;
                        motor[c].dir = FOR;
                        motor[a].pwm = 100.0*precision;
                        motor[b].pwm = 100.0*precision;
                        motor[c].pwm = 100.0*precision;
                    } else {
                        motor[a].dir = BRAKE;
                        motor[b].dir = BRAKE;
                        motor[c].dir = BRAKE;
                        motor[a].pwm = 100.0;
                        motor[b].pwm = 100.0;
                        motor[c].pwm = 100.0;
                    }
                    //アナログスティック足回り（新村ver）
                    /////////////////////////////////////////////////////////////////////////////////////






                }
            }



//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑


            MOTOR::Motor::Update(motor);
        }
    }
Repository toolbox

Export to desktop IDE
Build repository
Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	03 May 2019
Imports:	2
Forks:	0
Commits:	1
Dependents:	0
Dependencies:	2
Followers:	1
System/Process/Process.cpp

File content as of revision 0:4df75b08b14a:

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning
