Takumi Shimada
/
small_cleaner
test
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(); } }