File content as of revision 9:ec30ae33cc9e:

//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 a 0
#define b 1
#define c 2

//DigitalOut aira(PA_7);
//DigitalOut airb(PA_6);

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;

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

//#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;

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 ------------------------*/
//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓

conlx = controller->AnalogL.X;
conly = controller->AnalogL.Y;
conrx = controller->AnalogR.X;
conry = controller->AnalogR.Y;
//conba = controller->Button.A;
//conbb = controller->Button.B;
//conby = controller->Button.Y;
//conbx = controller->Button.X;

 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];
        motor[a].dir = FOR;
    }else if(a_array[conly][conlx] > 0){
        motor[a].pwm = a_array[conly][conlx];
        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;
            motor[b].pwm = 100.0;
            motor[c].pwm = 100.0;
        }else if(conry > 8){
            motor[a].dir = FOR;
            motor[b].dir = FOR;
            motor[c].dir = FOR;
            motor[a].pwm = 100.0;
            motor[b].pwm = 100.0;
            motor[c].pwm = 100.0;
        }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];
        motor[b].dir = FOR;
    }else if(b_array[conly][conlx] > 0){
        motor[b].pwm = b_array[conly][conlx];
        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;
                motor[b].pwm = 100.0;
                motor[c].pwm = 100.0;
        }else if(conrx > 8){
                motor[a].dir = FOR;
                motor[b].dir = FOR;
                motor[c].dir = FOR;
                motor[a].pwm = 100.0;
                motor[b].pwm = 100.0;
                motor[c].pwm = 100.0;
        }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];
        motor[c].dir = BACK;
    }else if(c_array[conly][conlx] > 0){
        motor[c].pwm = c_array[conly][conlx];
        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;
            motor[b].pwm = 100.0;
            motor[c].pwm = 100.0;
        }else if(conrx > 8){
            motor[a].dir = FOR;
            motor[b].dir = FOR;
            motor[c].dir = FOR;
            motor[a].pwm = 100.0;
            motor[b].pwm = 100.0;
            motor[c].pwm = 100.0;
        }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 (a_array[conlx>6][conly<8]){
//    motor[a].dir = BRAKE;
//    motor[a].pwm = 100.0;
//    }
//if (b_array[conlx>6][conly<8]){
//    motor[b].dir = BRAKE;
//    motor[b].pwm = 100.0;
//    }
//if (c_array[conlx>6][conly<8]){
//    motor[c].dir = BRAKE;
//    motor[c].pwm = 100.0;
//    }
//
//        
        //(conba)
        //{aira=0;}
       // if(conbb)
//        {airb=0;}
        
        
        
        
        
        
        
        
        
    }
    
    
            
        
//        
//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑
//___________________________


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