![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
If you wont to knock me down, look this file...
System/Process/Process.cpp
- Committer:
- Ryosei
- Date:
- 2019-05-03
- Revision:
- 0:4df75b08b14a
File content as of revision 0:4df75b08b14a:
/* このプログラムを見る人へ 用語解説 ご挨拶モード:足回りでロボットを左右に振りご挨拶しつつ、回路リセットを行うおれたおになくてはならないモード 足回り:宮川は十字キー足回りを、新村はアナログスティック足回りを担当しました。宮川のプログラムは猫でも書けますが、新村のは難易度S+です。わからないことがあれば新村へ わからないことがあればアナログスティック以外、なんでも宮川に聞いてください */ //------------------------------------------------------------------------------------------------------------------------------------------ #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), }; DigitalIn limit[]= { DigitalIn(A7), DigitalIn(A6), }; Timer Airtime; Timer home; void AirOut(int pin,int mode) { Air[pin]=mode; } bool LimitRead(int pin) { int x; x=limit[pin]; if(x==0) { return false; } else if(x==1) { return true; } else { return false; } } //関数、タイマーの宣言 ////////////////////////////////// //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(LimitRead(0)){ motor[ARM].dir = BACK; motor[ARM].pwm = 100; }else{ motor[ARM].dir = FREE; motor[ARM].pwm = 100; } }else if(controller->Button.Y) { motor[ARM].dir = FOR; motor[ARM].pwm = 100; }else{ motor[ARM].dir = FREE; 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; } //十字キー足回り(みやがわver) /////////////////////////////////// /////////////////////////////////// //課題1 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); } //課題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); } }