Miya Miyagawa
If you wont to knock me down, look this file...
Diff: System/Process/Process.cpp
- Revision:
- 0:4df75b08b14a
--- /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）
+ ///////////////////////////////////
+
+ ///////////////////////////////////
+ //課題１
+ 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);
+ }
+ //課題１
+ ///////////////////////////////////
+
+ /////////////////////////////////////////////////////////////////////////////////////
+ //アナログスティック足回り（新村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);
+ }
+ }
+