kaahddhgfhgfghgf

Dependencies:   mbed Servo

System/Process/Process.cpp

Committer:
kishibekairohan
Date:
8 weeks ago
Revision:
12:419fc52505ee
Parent:
11:de0dc9fba6c9

File content as of revision 12:419fc52505ee:

//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 0_L 1

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

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

#define TIRE_LF 
#define TIRE_LB 1
#define TIRE_RB 2
#define TIRE_RF 3
#define ARM_M   4

#define Air1_pin A7
#define Air2_pin A6
#define Air3_pin A5
#define ARM_T   3




const int omni[15][15]= {
    {   0,     2,      6,     15,     26,     41,     59,     80,     80,     80,     80,     80,    80,    80,    80},
    {  -2,     0,      2,      6,     15,     26,     41,     59,     61,     65,     73,     80,    80,    80,    80},
    {  -6,    -2,      0,      2,      6,     15,     26,     41,     43,     47,     56,     67,    80,    80,    80},
    { -15,    -6,     -2,      0,      2,      6,     15,     26,     28,     33,     41,     52,    67,    80,    80},
    { -26,   -15,     -6,     -2,      0,      2,      6,     15,     17,     21,     29,     41,    56,    73,    80},
    { -41,   -26,    -15,     -6,     -2,      0,      2,      6,      8,     13,     21,     33,    48,    65,    80},
    { -59,   -41,    -26,    -15,     -6,     -2,      0,      2,      4,      8,     16,     28,    42,    61,    80},
    { -80,   -59,    -41,    -26,    -15,     -6,     -2,      0,      2,      6,     15,     26,    41,    59,    80},
    { -80,   -61,    -42,    -28,    -16,     -8,     -4,     -2,      0,      2,      6,     15,    26,    41,    59},
    { -80,   -65,    -48,    -33,    -21,    -13,     -8,     -6,     -2,      0,      2,      6,    15,    26,    41},
    { -80,   -73,    -56,    -41,    -29,    -21,    -17,    -15,     -6,     -2,      0,      2,     6,    15,    26},
    { -80,   -80,    -67,    -52,    -41,    -33,    -28,    -26,    -15,     -6,     -2,      0,     2,     6,    15},
    { -80,   -80,    -80,    -67,    -56,    -47,    -43,    -41,    -26,    -15,     -6,     -2,     0,     2,     6},
    { -80,   -80,    -80,    -80,    -73,    -65,    -61,    -59,    -41,    -26,    -15,     -6,    -2,     0,     2},
    { -80,   -80,    -80,    -80,    -80,    -80,    -80,    -80,    -59,    -41,    -26,    -15,    -6,    -2,     0}
};

const int curve[15] = {-64, -47, -33, -21, -12, -5, -1, 0, 1, 5, 12, 21, 33, 47, 64};



uint8_t SetStatus(int);
uint8_t SetStatus(int pwmVal)
{
    if(pwmVal < 0) return BACK;
    else if(pwmVal > 0) return FOR;
    else if(pwmVal == 0) return BRAKE;
}
uint8_t SetPWM(int);
uint8_t SetPWM(int pwmVal)
{
    if(pwmVal == 0 || pwmVal >  80 || pwmVal < -80) return 80;
    else return abs(pwmVal);
}

//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑
//_____________________

//#define USE_USB_SERIAL
#ifdef USE_USB_SERIAL
Serial pc(SERIAL_TX, SERIAL_RX);
#endif
XBEE::ControllerData *controller;
MOTOR::MotorStatus motor[MOUNTING_MOTOR_NUM];



using namespace SWITCH;
/////////////////////////////////
DigitalOut AirPin[] = {
    DigitalOut(Air1_pin),
    DigitalOut(Air2_pin),
    DigitalOut(Air3_pin),
};

void AirOut(int pin,int mode)
{
    if(mode==1) {
        AirPin[pin]=1;
    } else if(mode==0) {
        AirPin[pin]=0;
    } else {
        AirPin[pin]=0;
    }
}
//////////////////////////////////
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(0_L))
                {
                    if(controller->Button.L)
                    {
                        motor[0].dir = FOR;
                        motor[0].pwm = 80.0;
                    }
                    if(motor[0].dir == BACK)
                    {
                        motor[0].dir = BRAKE;
                    }
                }

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

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

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

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


        if(controller->AnalogL.X >= 6 && controller->AnalogL.X <= 8 && controller->AnalogL.Y >= 6 && controller->AnalogL.Y <= 8) {
            motor[TIRE_LF].dir = FREE;
            motor[TIRE_LB].dir = FREE;
            motor[TIRE_RB].dir = FREE;
            motor[TIRE_RF].dir = FREE;
        }

        if(controller->AnalogR.X >= 6 && controller->AnalogR.X <= 8 && controller->AnalogR.Y >= 6 && controller->AnalogR.Y <= 8) {
            motor[TIRE_LF].dir = FREE;
            motor[TIRE_LB].dir = FREE;
            motor[TIRE_RB].dir = FREE;
            motor[TIRE_RF].dir = FREE;
        }

        motor[TIRE_RF].dir = SetStatus(-omni[controller->AnalogL.Y][14-controller->AnalogL.X]     + curve[controller->AnalogR.X]);
        motor[TIRE_LF].dir = SetStatus(omni[controller->AnalogL.Y][controller->AnalogL.X]         + curve[controller->AnalogR.X]);
        motor[TIRE_RB].dir = SetStatus(-omni[14-controller->AnalogL.X][14-controller->AnalogL.Y]  + curve[controller->AnalogR.X]);
        motor[TIRE_LB].dir = SetStatus(omni[controller->AnalogL.X][14-controller->AnalogL.Y]      + curve[controller->AnalogR.X]);

        motor[TIRE_RF].pwm = SetPWM(omni[controller->AnalogL.Y][14-controller->AnalogL.X])    ;
        motor[TIRE_LF].pwm = SetPWM(omni[controller->AnalogL.Y][controller->AnalogL.X])       ;
        motor[TIRE_RB].pwm = SetPWM(omni[14-controller->AnalogL.X][14-controller->AnalogL.Y]) ;
        motor[TIRE_LB].pwm = SetPWM(omni[controller->AnalogL.X][14-controller->AnalogL.Y])    ;

        if(controller->Button.R) {
            AirOut(0,1);
        }
        if(controller->Button.L) {
            AirOut(0,0);
        }

        if(controller->Button.A) {
            AirOut(1,1);
        }
        if(controller->Button.B) {
            AirOut(1,0);
        }

        if(controller->Button.X) {
            AirOut(2,1);
        }
        if(controller->Button.Y) {
            AirOut(2,0);
        }

        if(controller->Button.UP) {
                if(Switch::checkPushed(ARM_T)){
                    motor[ARM_M].dir = BRAKE;
                    motor[ARM_M].pwm = 50;
                }else{
                    motor[ARM_M].dir = BACK;
                    motor[ARM_M].pwm = 50;
                }
        }else if(controller->Button.DOWN) {
            motor[ARM_M].dir = FOR;
            motor[ARM_M].pwm = 50;
        }else {
            motor[ARM_M].dir = FREE;
        }
        





//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑
//____________________________

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