Miya Miyagawa
/
mainboardnrp2018
aaaaa
Diff: System/Process/Process.cpp
- Revision:
- 10:53a92f2ab1c4
- Parent:
- 9:6901b1dfa688
--- a/System/Process/Process.cpp Mon Feb 18 05:40:03 2019 +0000 +++ b/System/Process/Process.cpp Thu Feb 21 10:06:09 2019 +0000 @@ -26,11 +26,88 @@ #define TIRE_1 0 #define TIRE_2 1 #define TIRE_3 2 -#define ARM 3 -#define AIR_1 0 -#define AIR_2 1 -#define ARM_roll 0 +#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; //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑ //_____________________ @@ -41,90 +118,150 @@ Serial pc(SERIAL_TX,SERIAL_RX); XBEE::ControllerData *controller; MOTOR::MotorStatus motor[MOUNTING_MOTOR_NUM]; -DigitalOut Air[]={ - DigitalOut(A7), - DigitalOut(A6), - +////////////////////////////////// +//関数、タイマーの宣言 +DigitalOut Air[]= { + DigitalOut(A5), + DigitalOut(A4), }; Timer Airtime; -void AirOut(int pin,int mode){ +Timer home; +void AirOut(int pin,int mode) +{ Air[pin]=mode; } +// +////////////////////////////////// using namespace SWITCH; -void SystemProcess(void) { +void SystemProcess(void) +{ while(true) { controller = XBEE::Controller::GetData(); //____________________________ -/*------------------------ HOW TO WRITE ------------------------/ + /*------------------------ 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 聞いてください) + + ・リミットスイッチの値をもとに動作のプログラムを書く - ・コントローラから受け取ったデータをもとに動作のプログラムを書く - (コントローラのデータは controller-> で取る) + 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) - 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; + ・他にもやりたいことがあったら自由にどうぞ + + 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; } - motor[0].dirは FOR (正転) - BACK (逆転) - BRAKE (ブレーキ) - FREE (フリー) - - motor[0].pwmは 0.0(%) ~ 100.0(%) - - controllerは XBee.hの構造体の中身 - - (AnalogL・Rを使いたかったら、頑張って考える or 聞いてください) - - ・リミットスイッチの値をもとに動作のプログラムを書く + 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(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(); - pc.printf("time= %d",Airtime.read()); - if(controller->Button.ZR){ + //////////////////////////////////////////////////////////////////////////////////// + //精密モード + if(controller->Button.ZR) { precision=0.5; - }else{ + } else { precision=1.0; } + //精密モード + //////////////////////////////////////////////////////////////////////////////////// + + //////////////////////////////////////////////////////////////////////////////////// + //アーム機構 + //motor[ARM].pwm=100; if(controller->Button.X){ - motor[ARM].dir=FOR; - motor[ARM].pwm=80; - }else if((controller->Button.Y)&&(Switch::checkPushed(ARM_roll))){ - motor[ARM].dir=BACK; - motor[ARM].pwm=80; + if(Switch::checkPushed(ARMlim)){ + motor[ARM].dir = BRAKE; + motor[ARM].pwm = 100; + }else{ + motor[ARM].dir = BACK; + motor[ARM].pwm = 100; + } + }else if(controller->Button.Y){ + motor[ARM].dir = FOR; + motor[ARM].pwm = 100; }else{ - motor[ARM].dir=BRAKE; - motor[ARM].pwm=100; + motor[ARM].dir = BRAKE; + motor[ARM].pwm = 100; } + + + + + //アーム機構 + //////////////////////////////////////////////////////////////////////////////////// + + //////////////////////////////////////////////////////////////////////////////////// + //十字キー足回り(宮川ver) + if(controller->Button.L) { motor[TIRE_1].dir = FOR; motor[TIRE_2].dir = FOR; @@ -132,70 +269,85 @@ motor[TIRE_1].pwm = 100*precision; motor[TIRE_2].pwm = 100*precision; motor[TIRE_3].pwm = 100*precision; - }else if(controller->Button.R){ + } 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) { + } 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){ + } 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){ + } 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_1].pwm = 100; motor[TIRE_2].pwm = 100*precision; motor[TIRE_3].pwm = 100*precision; - }else if(controller->Button.UP){ + } 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)){ + } 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_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_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_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{ + 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; @@ -203,26 +355,152 @@ motor[TIRE_2].pwm=100; motor[TIRE_3].pwm=100; } - - if(controller->Button.A){ - AirOut(AIR_1,1); - }else if(controller->Button.B){ - AirOut(AIR_1,0); + /////////////////////////////////// + + if(controller->Button.A) { + AirOut(Air0,1); + } else if(controller->Button.B) { + AirOut(Air0,0); } - - if(controller->Button.ZL){ + + if(controller->Button.ZL) { Airtime.start(); - AirOut(AIR_2,0); + AirOut(Air1,0); } - if(s>1){ + if(s>1) { Airtime.stop(); Airtime.reset(); - AirOut(AIR_2,1); + AirOut(Air1,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); }