Miya Miyagawa
/
Orewotaosita
If you wont to knock me down, look this file...
Diff: System/Process/Process.cpp
- Revision:
- 0:4df75b08b14a
diff -r 000000000000 -r 4df75b08b14a System/Process/Process.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/System/Process/Process.cpp Fri May 03 11:47:27 2019 +0000 @@ -0,0 +1,538 @@ + + + +/* +このプログラムを見る人へ + +用語解説 + +ご挨拶モード:足回りでロボットを左右に振りご挨拶しつつ、回路リセットを行うおれたおになくてはならないモード +足回り:宮川は十字キー足回りを、新村はアナログスティック足回りを担当しました。宮川のプログラムは猫でも書けますが、新村のは難易度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); + } + } +