File content as of revision 10:53a92f2ab1c4:

//2018/02/24のやつ

#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),
};
Timer Airtime;
Timer home;
void AirOut(int pin,int mode)
{
    Air[pin]=mode;
}
//
//////////////////////////////////
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(Switch::checkPushed(ARMlim)){
                motor[ARM].dir = BRAKE;
                motor[ARM].pwm = 100;
            }else{
                motor[ARM].dir = BACK;
                motor[ARM].pwm = 100;
            }
        }else if(controller->Button.Y){
            motor[ARM].dir = FOR;
            motor[ARM].pwm = 100;
        }else{
            motor[ARM].dir = BRAKE;
            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;
        }
        ///////////////////////////////////

        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);
    }
}