test

Dependencies:   mbed

main.cpp

Committer:
Shimada_Takumi
Date:
2019-07-10
Revision:
0:6d1c8d3e84cd

File content as of revision 0:6d1c8d3e84cd:

#include "mbed.h"

#define LF 0
#define LB 1
#define RF 2
#define RB 3

#define S 0
#define P 1
#define M 2

#define F 0
#define B 1
#define L 2
#define R 3
#define W 4

#define PRINT

Serial pc(SERIAL_TX, SERIAL_RX);

PwmOut dir[4][2] = {{PC_9,PB_8},
                    {PC_8,PC_6},
                    {PA_5,PA_6},
                    {PB_6,PB_7}
                    };
                    
DigitalIn step_sensor[2]={D5,D4};

Ticker flip;

int flag=0;

float duty=0.5;

char a;

/*モータ制御*/
void motor(int number,int move,float duty){
    switch(move){
        case S:
        dir[number][0]=0;
        dir[number][1]=0;
        break;
        
        case P:
        dir[number][0]=0;
        dir[number][1]=duty;
        break;
        
        case M:
        dir[number][0]=duty;
        dir[number][1]=0;
        break;
    }
}

void move(int v){
    switch(v){
        case F:
        motor(LF,F,duty);
        motor(LB,F,duty);
        motor(RF,B,duty);
        motor(RB,B,duty);
        break;
        
        case B:
        motor(LF,B,duty);
        motor(LB,B,duty);
        motor(RF,F,duty);
        motor(RB,F,duty);
        break;
        
        case L:
        motor(LF,B,duty);
        motor(LB,F,duty);
        motor(RF,F,duty);
        motor(RB,B,duty);
        break;
        
        case R:
        motor(LF,F,duty);
        motor(LB,B,duty);
        motor(RF,B,duty);
        motor(RB,F,duty);
        break;
        
        case W:
        motor(LF,S,duty);
        motor(LB,S,duty);
        motor(RF,S,duty);
        motor(RB,S,duty);
        break;
    }
}

//階段センサ
int sensor_move(){
    if(step_sensor[0]==1 || step_sensor[1]==1){
        return 1;
    }else{
        return 0;
    }
}

/*緊急停止割り込み*/
void flipper(){
}

/*コントロール*/
void control(){
    pc.printf("what?");
    
    a = pc.getc();
    
    switch(a){
        case 'w':
        move(F);
        #ifdef PRINT
        pc.printf("move_F\n\r");
        #endif
        wait(0.1);
        break;
        
        case 'a':
        move(L);
        #ifdef PRINT
        pc.printf("move_L\n\r");
        #endif
        wait(0.1);
        break;
        
        case 's':
        move(R);
        #ifdef PRINT
        pc.printf("move_B\n\r");
        #endif
        wait(0.1);
        break;
        
        case 'd':
        move(B);
        #ifdef PRINT
        pc.printf("move_R\n\r");
        #endif
        wait(0.1);
        break;
        
        case 'q':
        #ifdef PRINT
        pc.printf("END\n\r");
        #endif
        flag=0;
        break;
        
        default:
        move(W);
        #ifdef printf
        pc.printf("move_W\n\r");
        #endif
        break;
    }
    move(W);
}

/*初期化*/
void init(){
    pc.printf("hello\n\r");
    
    move(W);
    wait(3);
    
    flag = 1;
}

/*メイン*/
int main(void){
    init();
    
    while(flag == 1){
        control();
    }
}