Miya Miyagawa / Mbed 2 deprecated mainboardnrp2018

aaaaa

Dependencies:   mbed Servo

System/Process/Process.cpp@10:53a92f2ab1c4, 2019-02-21 (annotated)

Committer:
Ryosei
Date:
Thu Feb 21 10:06:09 2019 +0000
Revision:
10:53a92f2ab1c4
Parent:
9:6901b1dfa688 
unkounko

Who changed what in which revision?

UserRevisionLine numberNew contents of line
t_yamamoto 8:cb53beff4bb2 1 //2018/02/24のやつ
t_yamamoto 5:a802ab60df1b 2
t_yamamoto 0:562021ed1ba9 3 #include "Process.h"
t_yamamoto 0:562021ed1ba9 4
t_yamamoto 0:562021ed1ba9 5 #include "mbed.h"
t_yamamoto 0:562021ed1ba9 6 #include "../../Communication/XBee/XBee.h"
t_yamamoto 0:562021ed1ba9 7 #include "../../Input/Switch/Switch.h"
t_yamamoto 0:562021ed1ba9 8 #include "../../Output/Motor/Motor.h"
t_yamamoto 8:cb53beff4bb2 9 #include "../../Output/Servo/Servo.h"
t_yamamoto 8:cb53beff4bb2 10
t_yamamoto 0:562021ed1ba9 11 //_____________________
t_yamamoto 0:562021ed1ba9 12 /*---------------- HOW TO WRITE ----------------/
t_yamamoto 0:562021ed1ba9 13
t_yamamoto 0:562021ed1ba9 14 ・motor の割り当てを決める
t_yamamoto 0:562021ed1ba9 15 #define TIRE_L 1
t_yamamoto 0:562021ed1ba9 16
t_yamamoto 0:562021ed1ba9 17 ・リミットスイッチの割り当てを決める
t_yamamoto 0:562021ed1ba9 18 #define ARM_L 1
t_yamamoto 0:562021ed1ba9 19
t_yamamoto 0:562021ed1ba9 20 ・他にも自由に定義してもいいです (pwmとか)
t_yamamoto 0:562021ed1ba9 21
t_yamamoto 0:562021ed1ba9 22 /---------------- HOW TO WRITE ----------------*/
t_yamamoto 0:562021ed1ba9 23 //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓
t_yamamoto 0:562021ed1ba9 24
t_yamamoto 0:562021ed1ba9 25
Ryosei 9:6901b1dfa688 26 #define TIRE_1 0
Ryosei 9:6901b1dfa688 27 #define TIRE_2 1
Ryosei 9:6901b1dfa688 28 #define TIRE_3 2
Ryosei 10:53a92f2ab1c4 29 #define ARM 3
Ryosei 10:53a92f2ab1c4 30
Ryosei 10:53a92f2ab1c4 31 #define ARMlim 0
Ryosei 10:53a92f2ab1c4 32
Ryosei 10:53a92f2ab1c4 33 int Air0=0;
Ryosei 10:53a92f2ab1c4 34 int Air1=1;
Ryosei 10:53a92f2ab1c4 35
Ryosei 10:53a92f2ab1c4 36 #define a 0
Ryosei 10:53a92f2ab1c4 37 #define b 1
Ryosei 10:53a92f2ab1c4 38 #define c 2
Ryosei 10:53a92f2ab1c4 39
Ryosei 10:53a92f2ab1c4 40 int a_array[15][15] = {
Ryosei 10:53a92f2ab1c4 41 { -30, -20, -10, 0, 0, 0, 0, 0, 0, 0, 0, 0, 10, 20, 30 },
Ryosei 10:53a92f2ab1c4 42 { -40, -30, -20, -10, 0, 0, 0, 0, 0, 0, 0, 10, 20, 30, 40 },
Ryosei 10:53a92f2ab1c4 43 { -50, -40, -30, -20, -10, 0, 0, 0, 0, 0, 10, 20, 30, 40, 50 },
Ryosei 10:53a92f2ab1c4 44 { -60, -50, -40, -30, -20, -10, 0, 0, 0, 10, 20, 30, 40, 50, 60 },
Ryosei 10:53a92f2ab1c4 45 { -70, -60, -50, -40, -30, -20, -10, 0, 10, 20, 30, 40, 50, 60, 70 },
Ryosei 10:53a92f2ab1c4 46 { -80, -70, -60, -50, -40, -30, -20, 0, 20, 30, 40, 50, 60, 70, 80 },
Ryosei 10:53a92f2ab1c4 47 { -90, -80, -70, -60, -50, -40, -30, 0, 30, 40, 50, 60, 70, 80, 90 },
Ryosei 10:53a92f2ab1c4 48 { -100, -90, -80, -70, -60, -50, -40, 0, 40, 50, 60, 70, 80, 90, 100 },
Ryosei 10:53a92f2ab1c4 49 { -90, -80, -70, -60, -50, -40, -30, 0, 30, 40, 50, 60, 70, 80, 90 },
Ryosei 10:53a92f2ab1c4 50 { -80, -70, -60, -50, -40, -30, -20, 0, 20, 30, 40, 50, 60, 70, 80 },
Ryosei 10:53a92f2ab1c4 51 { -70, -60, -50, -40, -30, -20, -10, 0, 10, 20, 30, 40, 50, 60, 70 },
Ryosei 10:53a92f2ab1c4 52 { -60, -50, -40, -30, -20, -10, 0, 0, 0, 10, 20, 30, 40, 50, 60 },
Ryosei 10:53a92f2ab1c4 53 { -50, -40, -30, -20, -10, 0, 0, 0, 0, 0, 10, 20, 30, 40, 50 },
Ryosei 10:53a92f2ab1c4 54 { -40, -30, -20, -10, 0, 0, 0, 0, 0, 0, 0, 10, 20, 30, 40 },
Ryosei 10:53a92f2ab1c4 55 { -30, -20, -10, 0, 0, 0, 0, 0, 0, 0, 0, 0, 10, 20, 30 }
Ryosei 10:53a92f2ab1c4 56 };
t_yamamoto 0:562021ed1ba9 57
Ryosei 10:53a92f2ab1c4 58 int b_array[15][15] = {
Ryosei 10:53a92f2ab1c4 59 { 100, 92, 84, 76, 68, 60, 52, 100, 36, 28, 20, 12, 4, 0, 0 },
Ryosei 10:53a92f2ab1c4 60 { 92, 84, 76, 68, 60, 52, 44, 36, 28, 20, 12, 4, 0, 0, 0 },
Ryosei 10:53a92f2ab1c4 61 { 84, 76, 68, 60, 52, 44, 36, 28, 20, 12, 4, 0, 0, 0, -4 },
Ryosei 10:53a92f2ab1c4 62 { 76, 68, 60, 52, 44, 36, 28, 20, 12, 4, 0, 0, 0, -4, -12 },
Ryosei 10:53a92f2ab1c4 63 { 68, 60, 52, 44, 36, 28, 20, 12, 4, 0, 0, 0, -4, -12, -20 },
Ryosei 10:53a92f2ab1c4 64 { 60, 52, 44, 36, 28, 20, 12, 4, 0, 0, 0, -4, -12, -20, -28 },
Ryosei 10:53a92f2ab1c4 65 { 52, 44, 36, 28, 20, 12, 4, 0, 0, 0, -4, -12, -20, -28, -36 },
Ryosei 10:53a92f2ab1c4 66 { 100, 36, 28, 20, 12, 4, 0, 0, 0, -4, -12, -20, -28, -36, -44 },
Ryosei 10:53a92f2ab1c4 67 { 85, 28, 20, 12, 4, 0, 0, 0, -4, -12, -20, -28, -36, -44, -52 },
Ryosei 10:53a92f2ab1c4 68 { 70, 20, 12, 4, 0, 0, 0, -4, -12, -20, -28, -36, -44, -52, -60 },
Ryosei 10:53a92f2ab1c4 69 { 55, 12, 4, 0, 0, 0, -4, -12, -20, -28, -36, -44, -52, -60, -68 },
Ryosei 10:53a92f2ab1c4 70 { 40, 4, 0, 0, 0, -4, -12, -20, -28, -36, -44, -52, -60, -68, -76 },
Ryosei 10:53a92f2ab1c4 71 { 25, 0, 0, 0, -4, -12, -20, -28, -36, -44, -52, -60, -68, -76, -84 },
Ryosei 10:53a92f2ab1c4 72 { 10, 0, 0, -4, -12, -20, -28, -36, -44, -52, -60, -68, -76, -84, -92 },
Ryosei 10:53a92f2ab1c4 73 { 0, 0, -4, -12, -20, -28, -36, -100, -52, -60, -68, -76, -84, -92, -100 }
Ryosei 10:53a92f2ab1c4 74 };
Ryosei 10:53a92f2ab1c4 75
Ryosei 10:53a92f2ab1c4 76
Ryosei 10:53a92f2ab1c4 77 int c_array[15][15] = {
Ryosei 10:53a92f2ab1c4 78 { 0, 0, 4, 12, 20, 28, 36, 100, 52, 60, 68, 76, 84, 92, 100 },
Ryosei 10:53a92f2ab1c4 79 { 0, 0, 0, 4, 12, 20, 28, 36, 44, 52, 60, 68, 76, 84, 92 },
Ryosei 10:53a92f2ab1c4 80 { -4, 0, 0, 0, 4, 12, 20, 28, 36, 44, 52, 60, 68, 76, 84 },
Ryosei 10:53a92f2ab1c4 81 { -12, -4, 0, 0, 0, 4, 12, 20, 28, 36, 44, 52, 60, 68, 76 },
Ryosei 10:53a92f2ab1c4 82 { -20, -12, -4, 0, 0, 0, 4, 12, 20, 28, 36, 44, 52, 60, 68 },
Ryosei 10:53a92f2ab1c4 83 { -28, -20, -12, -4, 0, 0, 0, 4, 12, 20, 28, 36, 44, 52, 60 },
Ryosei 10:53a92f2ab1c4 84 { -36, -28, -20, -12, -4, 0, 0, 0, 4, 12, 20, 28, 36, 44, 52 },
Ryosei 10:53a92f2ab1c4 85 { -44, -36, -28, -20, -12, -4, 0, 0, 0, 4, 12, 20, 28, 36, 44 },
Ryosei 10:53a92f2ab1c4 86 { -52, -44, -36, -28, -20, -12, 4, 0, 0, 0, 4, 12, 20, 28, 36 },
Ryosei 10:53a92f2ab1c4 87 { -60, -52, -44, -36, -28, -20, -12, -4, 0, 0, 0, 4, 12, 20, 28 },
Ryosei 10:53a92f2ab1c4 88 { -68, -60, -52, -44, -36, -28, -20, -12, -4, 0, 0, 0, 4, 12, 20 },
Ryosei 10:53a92f2ab1c4 89 { -76, -68, -60, -52, -44, -36, -28, -20, -12, -4, 0, 0, 0, 4, 12 },
Ryosei 10:53a92f2ab1c4 90 { -84, -76, -68, -60, -52, -44, -36, -28, -20, -12, -4, 0, 0, 0, 4 },
Ryosei 10:53a92f2ab1c4 91 { -92, -84, -76, -68, -60, -52, -44, -36, -28, -20, -12, -4, 0, 0, 0 },
Ryosei 10:53a92f2ab1c4 92 { -100, -92, -84, -76, -76, -60, -100, -100, -36, -28, -20, -12, -4, 0, 0 }
Ryosei 10:53a92f2ab1c4 93 };
Ryosei 10:53a92f2ab1c4 94
Ryosei 10:53a92f2ab1c4 95 int pwm_array[15] = { 30, 25, 20, 15, 10, 5, 0, 0, 0, -5, -10, -15, -20, -25, -30 };
Ryosei 10:53a92f2ab1c4 96
Ryosei 10:53a92f2ab1c4 97 #define usiro 0
Ryosei 10:53a92f2ab1c4 98 #define mae 0
Ryosei 10:53a92f2ab1c4 99 uint8_t motorData[5];
Ryosei 10:53a92f2ab1c4 100 uint8_t pwmData[5];
Ryosei 10:53a92f2ab1c4 101
Ryosei 10:53a92f2ab1c4 102 int conlx;
Ryosei 10:53a92f2ab1c4 103 int conly;
Ryosei 10:53a92f2ab1c4 104 int conrx;
Ryosei 10:53a92f2ab1c4 105 int conry;
Ryosei 10:53a92f2ab1c4 106 int conba;
Ryosei 10:53a92f2ab1c4 107 int conbb;
Ryosei 10:53a92f2ab1c4 108 int conbx;
Ryosei 10:53a92f2ab1c4 109 int conby;
Ryosei 10:53a92f2ab1c4 110 int homes=0;
t_yamamoto 0:562021ed1ba9 111 //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑
t_yamamoto 0:562021ed1ba9 112 //_____________________
t_yamamoto 0:562021ed1ba9 113
t_yamamoto 8:cb53beff4bb2 114 //#define USE_USB_SERIAL
t_yamamoto 0:562021ed1ba9 115 #ifdef USE_USB_SERIAL
t_yamamoto 0:562021ed1ba9 116 Serial pc(SERIAL_TX, SERIAL_RX);
t_yamamoto 0:562021ed1ba9 117 #endif
Ryosei 9:6901b1dfa688 118 Serial pc(SERIAL_TX,SERIAL_RX);
t_yamamoto 0:562021ed1ba9 119 XBEE::ControllerData *controller;
t_yamamoto 0:562021ed1ba9 120 MOTOR::MotorStatus motor[MOUNTING_MOTOR_NUM];
Ryosei 10:53a92f2ab1c4 121 //////////////////////////////////
Ryosei 10:53a92f2ab1c4 122 //関数、タイマーの宣言
Ryosei 10:53a92f2ab1c4 123 DigitalOut Air[]= {
Ryosei 10:53a92f2ab1c4 124 DigitalOut(A5),
Ryosei 10:53a92f2ab1c4 125 DigitalOut(A4),
Ryosei 9:6901b1dfa688 126 };
Ryosei 9:6901b1dfa688 127 Timer Airtime;
Ryosei 10:53a92f2ab1c4 128 Timer home;
Ryosei 10:53a92f2ab1c4 129 void AirOut(int pin,int mode)
Ryosei 10:53a92f2ab1c4 130 {
Ryosei 9:6901b1dfa688 131 Air[pin]=mode;
Ryosei 9:6901b1dfa688 132 }
Ryosei 10:53a92f2ab1c4 133 //
Ryosei 10:53a92f2ab1c4 134 //////////////////////////////////
t_yamamoto 0:562021ed1ba9 135 using namespace SWITCH;
t_yamamoto 0:562021ed1ba9 136
Ryosei 10:53a92f2ab1c4 137 void SystemProcess(void)
Ryosei 10:53a92f2ab1c4 138 {
t_yamamoto 0:562021ed1ba9 139 while(true) {
t_yamamoto 0:562021ed1ba9 140 controller = XBEE::Controller::GetData();
t_yamamoto 0:562021ed1ba9 141 //____________________________
Ryosei 10:53a92f2ab1c4 142 /*------------------------ HOW TO WRITE ------------------------/
Ryosei 10:53a92f2ab1c4 143
Ryosei 10:53a92f2ab1c4 144 ここにメインのプログラムを書く
Ryosei 10:53a92f2ab1c4 145
Ryosei 10:53a92f2ab1c4 146 ・コントローラから受け取ったデータをもとに動作のプログラムを書く
Ryosei 10:53a92f2ab1c4 147 (コントローラのデータは controller-> で取る)
t_yamamoto 0:562021ed1ba9 148
Ryosei 10:53a92f2ab1c4 149 if(controller->Button.RIGHT) {
Ryosei 10:53a92f2ab1c4 150 motor[TIRE_L].dir = FOR;
Ryosei 10:53a92f2ab1c4 151 motor[TIRE_R].dir = BACK;
Ryosei 10:53a92f2ab1c4 152 motor[TIRE_L].pwm = 12.3;
Ryosei 10:53a92f2ab1c4 153 motor[TIRE_R].pwm = 12.3;
Ryosei 10:53a92f2ab1c4 154 }
Ryosei 10:53a92f2ab1c4 155
Ryosei 10:53a92f2ab1c4 156 motor[0].dirは FOR (正転)
Ryosei 10:53a92f2ab1c4 157 BACK (逆転)
Ryosei 10:53a92f2ab1c4 158 BRAKE (ブレーキ)
Ryosei 10:53a92f2ab1c4 159 FREE (フリー)
Ryosei 10:53a92f2ab1c4 160
Ryosei 10:53a92f2ab1c4 161 motor[0].pwmは 0.0(%) ~ 100.0(%)
Ryosei 10:53a92f2ab1c4 162
Ryosei 10:53a92f2ab1c4 163 controllerは XBee.hの構造体の中身
Ryosei 10:53a92f2ab1c4 164
Ryosei 10:53a92f2ab1c4 165 (AnalogL・Rを使いたかったら、頑張って考える or 聞いてください)
Ryosei 10:53a92f2ab1c4 166
Ryosei 10:53a92f2ab1c4 167 ・リミットスイッチの値をもとに動作のプログラムを書く
t_yamamoto 0:562021ed1ba9 168
Ryosei 10:53a92f2ab1c4 169 if(Switch::CheckPushed(ARM_L))
Ryosei 10:53a92f2ab1c4 170 {
Ryosei 10:53a92f2ab1c4 171 if(controller->Button.L)
Ryosei 10:53a92f2ab1c4 172 {
Ryosei 10:53a92f2ab1c4 173 motor[ARM].dir = FOR;
Ryosei 10:53a92f2ab1c4 174 motor[ARM].pwm = 80.0;
Ryosei 10:53a92f2ab1c4 175 }
Ryosei 10:53a92f2ab1c4 176 if(motor[ARM].dir == BACK)
Ryosei 10:53a92f2ab1c4 177 {
Ryosei 10:53a92f2ab1c4 178 motor[ARM].dir = BRAKE;
Ryosei 10:53a92f2ab1c4 179 }
Ryosei 10:53a92f2ab1c4 180 }
Ryosei 10:53a92f2ab1c4 181
Ryosei 10:53a92f2ab1c4 182 →関数 Switch::CheckPushed の引数はリミットスイッチの名前 (limitSw[0]みたいな), 返り値はbool型 (true or false)
t_yamamoto 0:562021ed1ba9 183
Ryosei 10:53a92f2ab1c4 184 ・他にもやりたいことがあったら自由にどうぞ
Ryosei 10:53a92f2ab1c4 185
Ryosei 10:53a92f2ab1c4 186 ps.わからないことがあったら聞いてください
Ryosei 10:53a92f2ab1c4 187
Ryosei 10:53a92f2ab1c4 188 /------------------------ HOW TO WRITE ------------------------*/
Ryosei 10:53a92f2ab1c4 189 //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓
Ryosei 10:53a92f2ab1c4 190
Ryosei 10:53a92f2ab1c4 191 float precision; //精密モード変数
Ryosei 10:53a92f2ab1c4 192 float s=Airtime.read();//エアータイム
Ryosei 10:53a92f2ab1c4 193 float hometime=home.read();//ホームタイム
Ryosei 10:53a92f2ab1c4 194 float goaisatsu_time=0.2;
Ryosei 10:53a92f2ab1c4 195 //////////////////////////////////////////////////////////////////////////////////
Ryosei 10:53a92f2ab1c4 196 //ご挨拶モード
Ryosei 10:53a92f2ab1c4 197
Ryosei 10:53a92f2ab1c4 198 if(controller->Button.HOME){
Ryosei 10:53a92f2ab1c4 199 home.start();
Ryosei 10:53a92f2ab1c4 200 homes=1;
t_yamamoto 0:562021ed1ba9 201 }
t_yamamoto 0:562021ed1ba9 202
Ryosei 10:53a92f2ab1c4 203 if((hometime<=goaisatsu_time)&&(!(homes==0))){
Ryosei 10:53a92f2ab1c4 204 homes=2;
Ryosei 10:53a92f2ab1c4 205 }else if(hometime<=2*goaisatsu_time){
Ryosei 10:53a92f2ab1c4 206 homes=1;
Ryosei 10:53a92f2ab1c4 207 }else if(hometime<=3*goaisatsu_time){
Ryosei 10:53a92f2ab1c4 208 homes=2;
Ryosei 10:53a92f2ab1c4 209 }else if(hometime<4*goaisatsu_time){
Ryosei 10:53a92f2ab1c4 210 homes=1;
Ryosei 10:53a92f2ab1c4 211 }else if(hometime<5*goaisatsu_time){
Ryosei 10:53a92f2ab1c4 212 homes=0;
Ryosei 10:53a92f2ab1c4 213 home.stop();
Ryosei 10:53a92f2ab1c4 214 home.reset();
Ryosei 10:53a92f2ab1c4 215 }else{
Ryosei 10:53a92f2ab1c4 216 homes=0;
Ryosei 10:53a92f2ab1c4 217 home.stop();
Ryosei 10:53a92f2ab1c4 218 home.reset();
Ryosei 10:53a92f2ab1c4 219 }
Ryosei 10:53a92f2ab1c4 220
Ryosei 10:53a92f2ab1c4 221 //ご挨拶モード
Ryosei 10:53a92f2ab1c4 222 ////////////////////////////////////////////////////////////////////////////////////
t_yamamoto 0:562021ed1ba9 223
Ryosei 10:53a92f2ab1c4 224 ////////////////////////////////////////////////////////////////////////////////////
Ryosei 10:53a92f2ab1c4 225 //精密モード
Ryosei 10:53a92f2ab1c4 226 if(controller->Button.ZR) {
Ryosei 9:6901b1dfa688 227 precision=0.5;
Ryosei 10:53a92f2ab1c4 228 } else {
Ryosei 9:6901b1dfa688 229 precision=1.0;
Ryosei 9:6901b1dfa688 230 }
Ryosei 10:53a92f2ab1c4 231 //精密モード
Ryosei 10:53a92f2ab1c4 232 ////////////////////////////////////////////////////////////////////////////////////
Ryosei 10:53a92f2ab1c4 233
Ryosei 10:53a92f2ab1c4 234 ////////////////////////////////////////////////////////////////////////////////////
Ryosei 10:53a92f2ab1c4 235 //アーム機構
Ryosei 10:53a92f2ab1c4 236 //motor[ARM].pwm=100;
Ryosei 9:6901b1dfa688 237
Ryosei 9:6901b1dfa688 238 if(controller->Button.X){
Ryosei 10:53a92f2ab1c4 239 if(Switch::checkPushed(ARMlim)){
Ryosei 10:53a92f2ab1c4 240 motor[ARM].dir = BRAKE;
Ryosei 10:53a92f2ab1c4 241 motor[ARM].pwm = 100;
Ryosei 10:53a92f2ab1c4 242 }else{
Ryosei 10:53a92f2ab1c4 243 motor[ARM].dir = BACK;
Ryosei 10:53a92f2ab1c4 244 motor[ARM].pwm = 100;
Ryosei 10:53a92f2ab1c4 245 }
Ryosei 10:53a92f2ab1c4 246 }else if(controller->Button.Y){
Ryosei 10:53a92f2ab1c4 247 motor[ARM].dir = FOR;
Ryosei 10:53a92f2ab1c4 248 motor[ARM].pwm = 100;
Ryosei 9:6901b1dfa688 249 }else{
Ryosei 10:53a92f2ab1c4 250 motor[ARM].dir = BRAKE;
Ryosei 10:53a92f2ab1c4 251 motor[ARM].pwm = 100;
Ryosei 9:6901b1dfa688 252 }
Ryosei 9:6901b1dfa688 253
Ryosei 9:6901b1dfa688 254
Ryosei 10:53a92f2ab1c4 255
Ryosei 10:53a92f2ab1c4 256
Ryosei 10:53a92f2ab1c4 257
Ryosei 10:53a92f2ab1c4 258
Ryosei 10:53a92f2ab1c4 259 //アーム機構
Ryosei 10:53a92f2ab1c4 260 ////////////////////////////////////////////////////////////////////////////////////
Ryosei 10:53a92f2ab1c4 261
Ryosei 10:53a92f2ab1c4 262 ////////////////////////////////////////////////////////////////////////////////////
Ryosei 10:53a92f2ab1c4 263 //十字キー足回り（宮川ver）
Ryosei 10:53a92f2ab1c4 264
Ryosei 9:6901b1dfa688 265 if(controller->Button.L) {
Ryosei 9:6901b1dfa688 266 motor[TIRE_1].dir = FOR;
Ryosei 9:6901b1dfa688 267 motor[TIRE_2].dir = FOR;
Ryosei 9:6901b1dfa688 268 motor[TIRE_3].dir = FOR;
Ryosei 9:6901b1dfa688 269 motor[TIRE_1].pwm = 100*precision;
Ryosei 9:6901b1dfa688 270 motor[TIRE_2].pwm = 100*precision;
Ryosei 9:6901b1dfa688 271 motor[TIRE_3].pwm = 100*precision;
Ryosei 10:53a92f2ab1c4 272 } else if(controller->Button.R) {
Ryosei 9:6901b1dfa688 273 motor[TIRE_1].dir = BACK;
Ryosei 9:6901b1dfa688 274 motor[TIRE_2].dir = BACK;
Ryosei 9:6901b1dfa688 275 motor[TIRE_3].dir = BACK;
Ryosei 9:6901b1dfa688 276 motor[TIRE_1].pwm = 100*precision;
Ryosei 9:6901b1dfa688 277 motor[TIRE_2].pwm = 100*precision;
Ryosei 9:6901b1dfa688 278 motor[TIRE_3].pwm = 100*precision;
Ryosei 10:53a92f2ab1c4 279 } else if(controller->Button.RIGHT) {
Ryosei 9:6901b1dfa688 280 motor[TIRE_1].dir = BACK;
Ryosei 9:6901b1dfa688 281 motor[TIRE_2].dir = FOR;
Ryosei 9:6901b1dfa688 282 motor[TIRE_3].dir = FOR;
Ryosei 9:6901b1dfa688 283 motor[TIRE_1].pwm = 100*precision;
Ryosei 9:6901b1dfa688 284 motor[TIRE_2].pwm = 60*precision;
Ryosei 9:6901b1dfa688 285 motor[TIRE_3].pwm = 60*precision;
Ryosei 10:53a92f2ab1c4 286 } else if(controller->Button.LEFT) {
Ryosei 9:6901b1dfa688 287 motor[TIRE_1].dir = FOR;
Ryosei 9:6901b1dfa688 288 motor[TIRE_2].dir = BACK;
Ryosei 9:6901b1dfa688 289 motor[TIRE_3].dir = BACK;
Ryosei 9:6901b1dfa688 290 motor[TIRE_1].pwm = 100*precision;
Ryosei 9:6901b1dfa688 291 motor[TIRE_2].pwm = 60*precision;
Ryosei 9:6901b1dfa688 292 motor[TIRE_3].pwm = 60*precision;
Ryosei 10:53a92f2ab1c4 293 } else if(controller->Button.DOWN) {
Ryosei 9:6901b1dfa688 294 motor[TIRE_1].dir = BRAKE;
Ryosei 9:6901b1dfa688 295 motor[TIRE_2].dir = FOR;
Ryosei 9:6901b1dfa688 296 motor[TIRE_3].dir = BACK;
Ryosei 10:53a92f2ab1c4 297 motor[TIRE_1].pwm = 100;
Ryosei 9:6901b1dfa688 298 motor[TIRE_2].pwm = 100*precision;
Ryosei 9:6901b1dfa688 299 motor[TIRE_3].pwm = 100*precision;
Ryosei 10:53a92f2ab1c4 300 } else if(controller->Button.UP) {
Ryosei 9:6901b1dfa688 301 motor[TIRE_1].dir = BRAKE;
Ryosei 9:6901b1dfa688 302 motor[TIRE_2].dir = BACK;
Ryosei 9:6901b1dfa688 303 motor[TIRE_3].dir = FOR;
Ryosei 9:6901b1dfa688 304 motor[TIRE_1].pwm = 100;
Ryosei 9:6901b1dfa688 305 motor[TIRE_2].pwm = 100*precision;
Ryosei 9:6901b1dfa688 306 motor[TIRE_3].pwm = 100*precision;
Ryosei 10:53a92f2ab1c4 307 } else if((controller->Button.UP) && (controller->Button.RIGHT)) {
Ryosei 9:6901b1dfa688 308 motor[TIRE_1].dir = BACK;
Ryosei 9:6901b1dfa688 309 motor[TIRE_2].dir = BRAKE;
Ryosei 9:6901b1dfa688 310 motor[TIRE_3].dir = FOR;
Ryosei 9:6901b1dfa688 311 motor[TIRE_1].pwm = 100*precision;
Ryosei 9:6901b1dfa688 312 motor[TIRE_2].pwm = 100;
Ryosei 10:53a92f2ab1c4 313 motor[TIRE_3].pwm = 100*precision;
Ryosei 10:53a92f2ab1c4 314 } else if((controller->Button.UP) && (controller->Button.LEFT)) {
Ryosei 9:6901b1dfa688 315 motor[TIRE_1].dir = FOR;
Ryosei 9:6901b1dfa688 316 motor[TIRE_2].dir = BACK;
Ryosei 9:6901b1dfa688 317 motor[TIRE_3].dir = BRAKE;
Ryosei 9:6901b1dfa688 318 motor[TIRE_1].pwm = 100*precision;
Ryosei 9:6901b1dfa688 319 motor[TIRE_2].pwm = 100*precision;
Ryosei 10:53a92f2ab1c4 320 motor[TIRE_3].pwm = 100;
Ryosei 10:53a92f2ab1c4 321 } else if((controller->Button.DOWN) && (controller->Button.RIGHT)) {
Ryosei 9:6901b1dfa688 322 motor[TIRE_1].dir = BACK;
Ryosei 9:6901b1dfa688 323 motor[TIRE_2].dir = FOR;
Ryosei 9:6901b1dfa688 324 motor[TIRE_3].dir = BRAKE;
Ryosei 9:6901b1dfa688 325 motor[TIRE_1].pwm = 100*precision;
Ryosei 9:6901b1dfa688 326 motor[TIRE_2].pwm = 100*precision;
Ryosei 10:53a92f2ab1c4 327 motor[TIRE_3].pwm = 100;
Ryosei 10:53a92f2ab1c4 328 } else if((controller->Button.DOWN) && (controller->Button.LEFT)) {
Ryosei 9:6901b1dfa688 329 motor[TIRE_1].dir = FOR;
Ryosei 9:6901b1dfa688 330 motor[TIRE_2].dir = BRAKE;
Ryosei 9:6901b1dfa688 331 motor[TIRE_3].dir = BACK;
Ryosei 9:6901b1dfa688 332 motor[TIRE_1].pwm = 100*precision;
Ryosei 9:6901b1dfa688 333 motor[TIRE_2].pwm = 100;
Ryosei 10:53a92f2ab1c4 334 motor[TIRE_3].pwm = 100*precision;
Ryosei 10:53a92f2ab1c4 335 } else if(homes==1) {
Ryosei 10:53a92f2ab1c4 336 motor[TIRE_1].dir = FOR;
Ryosei 10:53a92f2ab1c4 337 motor[TIRE_2].dir = FOR;
Ryosei 10:53a92f2ab1c4 338 motor[TIRE_3].dir = FOR;
Ryosei 10:53a92f2ab1c4 339 motor[TIRE_1].pwm = 70*precision;
Ryosei 10:53a92f2ab1c4 340 motor[TIRE_2].pwm = 70*precision;
Ryosei 10:53a92f2ab1c4 341 motor[TIRE_3].pwm = 70*precision;
Ryosei 10:53a92f2ab1c4 342 } else if(homes==2) {
Ryosei 10:53a92f2ab1c4 343 motor[TIRE_1].dir = BACK;
Ryosei 10:53a92f2ab1c4 344 motor[TIRE_2].dir = BACK;
Ryosei 10:53a92f2ab1c4 345 motor[TIRE_3].dir = BACK;
Ryosei 10:53a92f2ab1c4 346 motor[TIRE_1].pwm = 70*precision;
Ryosei 10:53a92f2ab1c4 347 motor[TIRE_2].pwm = 70*precision;
Ryosei 10:53a92f2ab1c4 348 motor[TIRE_3].pwm = 70*precision;
Ryosei 10:53a92f2ab1c4 349 }
Ryosei 10:53a92f2ab1c4 350 else {
Ryosei 9:6901b1dfa688 351 motor[TIRE_1].dir=BRAKE;
Ryosei 9:6901b1dfa688 352 motor[TIRE_2].dir=BRAKE;
Ryosei 9:6901b1dfa688 353 motor[TIRE_3].dir=BRAKE;
Ryosei 9:6901b1dfa688 354 motor[TIRE_1].pwm=100;
Ryosei 9:6901b1dfa688 355 motor[TIRE_2].pwm=100;
Ryosei 9:6901b1dfa688 356 motor[TIRE_3].pwm=100;
Ryosei 9:6901b1dfa688 357 }
Ryosei 10:53a92f2ab1c4 358 ///////////////////////////////////
Ryosei 10:53a92f2ab1c4 359
Ryosei 10:53a92f2ab1c4 360 if(controller->Button.A) {
Ryosei 10:53a92f2ab1c4 361 AirOut(Air0,1);
Ryosei 10:53a92f2ab1c4 362 } else if(controller->Button.B) {
Ryosei 10:53a92f2ab1c4 363 AirOut(Air0,0);
Ryosei 9:6901b1dfa688 364 }
Ryosei 10:53a92f2ab1c4 365
Ryosei 10:53a92f2ab1c4 366 if(controller->Button.ZL) {
Ryosei 9:6901b1dfa688 367 Airtime.start();
Ryosei 10:53a92f2ab1c4 368 AirOut(Air1,0);
Ryosei 9:6901b1dfa688 369 }
Ryosei 10:53a92f2ab1c4 370 if(s>1) {
Ryosei 9:6901b1dfa688 371 Airtime.stop();
Ryosei 9:6901b1dfa688 372 Airtime.reset();
Ryosei 10:53a92f2ab1c4 373 AirOut(Air1,1);
Ryosei 9:6901b1dfa688 374 }
t_yamamoto 0:562021ed1ba9 375
t_yamamoto 0:562021ed1ba9 376
Ryosei 10:53a92f2ab1c4 377 /////////////////////////////////////////////////////////////////////////////////////
Ryosei 10:53a92f2ab1c4 378 //アナログスティック足回り（新村ver）
Ryosei 10:53a92f2ab1c4 379
Ryosei 10:53a92f2ab1c4 380 conlx = controller->AnalogL.X;
Ryosei 10:53a92f2ab1c4 381 conly = controller->AnalogL.Y;
Ryosei 10:53a92f2ab1c4 382 conrx = controller->AnalogR.X;
Ryosei 10:53a92f2ab1c4 383 conry = controller->AnalogR.Y;
Ryosei 10:53a92f2ab1c4 384 if((!(controller->Button.RIGHT))&&(!(controller->Button.DOWN))&&
Ryosei 10:53a92f2ab1c4 385 (!(controller->Button.LEFT))&&(!(controller->Button.UP))&&
Ryosei 10:53a92f2ab1c4 386 (!(controller->Button.L))&&(!(controller->Button.R))&&
Ryosei 10:53a92f2ab1c4 387 (!(controller->Button.HOME))) {
Ryosei 10:53a92f2ab1c4 388 if(((conlx >6) && (conly <8)) ||((conrx >6)&&(conry <8))) {
Ryosei 10:53a92f2ab1c4 389 motor[a].dir = BRAKE;
Ryosei 10:53a92f2ab1c4 390 motor[b].dir = BRAKE;
Ryosei 10:53a92f2ab1c4 391 motor[c].dir = BRAKE;
Ryosei 10:53a92f2ab1c4 392 motor[a].pwm = 100.0;
Ryosei 10:53a92f2ab1c4 393 motor[b].pwm = 100.0;
Ryosei 10:53a92f2ab1c4 394 motor[c].pwm = 100.0;
Ryosei 10:53a92f2ab1c4 395 }
Ryosei 10:53a92f2ab1c4 396
Ryosei 10:53a92f2ab1c4 397 if(a_array[conly][conlx]<0) {
Ryosei 10:53a92f2ab1c4 398 motor[a].pwm = -1*a_array[conly][conlx]*precision;
Ryosei 10:53a92f2ab1c4 399 motor[a].dir = FOR;
Ryosei 10:53a92f2ab1c4 400 } else if(a_array[conly][conlx] > 0) {
Ryosei 10:53a92f2ab1c4 401 motor[a].pwm = a_array[conly][conlx]*precision;
Ryosei 10:53a92f2ab1c4 402 motor[a].dir = BACK;
Ryosei 10:53a92f2ab1c4 403 } else if(a_array[conly][conlx] == 0) {
Ryosei 10:53a92f2ab1c4 404 if(conry < 6) {
Ryosei 10:53a92f2ab1c4 405 motor[a].dir = BACK;
Ryosei 10:53a92f2ab1c4 406 motor[b].dir = BACK;
Ryosei 10:53a92f2ab1c4 407 motor[c].dir = BACK;
Ryosei 10:53a92f2ab1c4 408 motor[a].pwm = 100.0*precision;
Ryosei 10:53a92f2ab1c4 409 motor[b].pwm = 100.0*precision;
Ryosei 10:53a92f2ab1c4 410 motor[c].pwm = 100.0*precision;
Ryosei 10:53a92f2ab1c4 411 } else if(conry > 8) {
Ryosei 10:53a92f2ab1c4 412 motor[a].dir = FOR;
Ryosei 10:53a92f2ab1c4 413 motor[b].dir = FOR;
Ryosei 10:53a92f2ab1c4 414 motor[c].dir = FOR;
Ryosei 10:53a92f2ab1c4 415 motor[a].pwm = 100.0*precision;
Ryosei 10:53a92f2ab1c4 416 motor[b].pwm = 100.0*precision;
Ryosei 10:53a92f2ab1c4 417 motor[c].pwm = 100.0*precision;
Ryosei 10:53a92f2ab1c4 418 } else {
Ryosei 10:53a92f2ab1c4 419 motor[a].dir = BRAKE;
Ryosei 10:53a92f2ab1c4 420 motor[b].dir = BRAKE;
Ryosei 10:53a92f2ab1c4 421 motor[c].dir = BRAKE;
Ryosei 10:53a92f2ab1c4 422 motor[a].pwm = 100.0;
Ryosei 10:53a92f2ab1c4 423 motor[b].pwm = 100.0;
Ryosei 10:53a92f2ab1c4 424 motor[c].pwm = 100.0;
Ryosei 10:53a92f2ab1c4 425 }
Ryosei 10:53a92f2ab1c4 426 }
Ryosei 10:53a92f2ab1c4 427
Ryosei 10:53a92f2ab1c4 428 if(b_array[conly][conlx] <0 ) {
Ryosei 10:53a92f2ab1c4 429 motor[b].pwm = -1*b_array[conly][conlx]*precision;
Ryosei 10:53a92f2ab1c4 430 motor[b].dir = FOR;
Ryosei 10:53a92f2ab1c4 431 } else if(b_array[conly][conlx] > 0) {
Ryosei 10:53a92f2ab1c4 432 motor[b].pwm = b_array[conly][conlx]*precision;
Ryosei 10:53a92f2ab1c4 433 motor[b].dir = BACK;
Ryosei 10:53a92f2ab1c4 434 } else if(a_array[conly][conlx] == 0 ) {
Ryosei 10:53a92f2ab1c4 435 if(conrx < 6 ) {
Ryosei 10:53a92f2ab1c4 436 motor[a].dir = BACK;
Ryosei 10:53a92f2ab1c4 437 motor[b].dir = BACK;
Ryosei 10:53a92f2ab1c4 438 motor[c].dir = BACK;
Ryosei 10:53a92f2ab1c4 439 motor[a].pwm = 100.0*precision;
Ryosei 10:53a92f2ab1c4 440 motor[b].pwm = 100.0*precision;
Ryosei 10:53a92f2ab1c4 441 motor[c].pwm = 100.0*precision;
Ryosei 10:53a92f2ab1c4 442 } else if(conrx > 8) {
Ryosei 10:53a92f2ab1c4 443 motor[a].dir = FOR*precision;
Ryosei 10:53a92f2ab1c4 444 motor[b].dir = FOR*precision;
Ryosei 10:53a92f2ab1c4 445 motor[c].dir = FOR*precision;
Ryosei 10:53a92f2ab1c4 446 motor[a].pwm = 100.0*precision;
Ryosei 10:53a92f2ab1c4 447 motor[b].pwm = 100.0*precision;
Ryosei 10:53a92f2ab1c4 448 motor[c].pwm = 100.0*precision;
Ryosei 10:53a92f2ab1c4 449 } else {
Ryosei 10:53a92f2ab1c4 450 motor[a].dir = BRAKE;
Ryosei 10:53a92f2ab1c4 451 motor[b].dir = BRAKE;
Ryosei 10:53a92f2ab1c4 452 motor[c].dir = BRAKE;
Ryosei 10:53a92f2ab1c4 453 motor[a].pwm = 100.0;
Ryosei 10:53a92f2ab1c4 454 motor[b].pwm = 100.0;
Ryosei 10:53a92f2ab1c4 455 motor[c].pwm = 100.0;
Ryosei 10:53a92f2ab1c4 456 }
Ryosei 10:53a92f2ab1c4 457 }
Ryosei 10:53a92f2ab1c4 458
Ryosei 10:53a92f2ab1c4 459
Ryosei 10:53a92f2ab1c4 460 if(c_array[conly][conlx] <0 ) {
Ryosei 10:53a92f2ab1c4 461 motor[c].pwm =-1*c_array[conly][conlx]*precision;
Ryosei 10:53a92f2ab1c4 462 motor[c].dir = BACK;
Ryosei 10:53a92f2ab1c4 463 } else if(c_array[conly][conlx] > 0) {
Ryosei 10:53a92f2ab1c4 464 motor[c].pwm = c_array[conly][conlx]*precision;
Ryosei 10:53a92f2ab1c4 465 motor[c].dir = FOR;
Ryosei 10:53a92f2ab1c4 466 } else if(a_array[conly][conlx] == 0 ) {
Ryosei 10:53a92f2ab1c4 467 if(conrx < 6 ) {
Ryosei 10:53a92f2ab1c4 468 motor[a].dir = BACK;
Ryosei 10:53a92f2ab1c4 469 motor[b].dir = BACK;
Ryosei 10:53a92f2ab1c4 470 motor[c].dir = BACK;
Ryosei 10:53a92f2ab1c4 471 motor[a].pwm = 100.0*precision;
Ryosei 10:53a92f2ab1c4 472 motor[b].pwm = 100.0*precision;
Ryosei 10:53a92f2ab1c4 473 motor[c].pwm = 100.0*precision;
Ryosei 10:53a92f2ab1c4 474 } else if(conrx > 8) {
Ryosei 10:53a92f2ab1c4 475 motor[a].dir = FOR;
Ryosei 10:53a92f2ab1c4 476 motor[b].dir = FOR;
Ryosei 10:53a92f2ab1c4 477 motor[c].dir = FOR;
Ryosei 10:53a92f2ab1c4 478 motor[a].pwm = 100.0*precision;
Ryosei 10:53a92f2ab1c4 479 motor[b].pwm = 100.0*precision;
Ryosei 10:53a92f2ab1c4 480 motor[c].pwm = 100.0*precision;
Ryosei 10:53a92f2ab1c4 481 } else {
Ryosei 10:53a92f2ab1c4 482 motor[a].dir = BRAKE;
Ryosei 10:53a92f2ab1c4 483 motor[b].dir = BRAKE;
Ryosei 10:53a92f2ab1c4 484 motor[c].dir = BRAKE;
Ryosei 10:53a92f2ab1c4 485 motor[a].pwm = 100.0;
Ryosei 10:53a92f2ab1c4 486 motor[b].pwm = 100.0;
Ryosei 10:53a92f2ab1c4 487 motor[c].pwm = 100.0;
Ryosei 10:53a92f2ab1c4 488 }
Ryosei 10:53a92f2ab1c4 489 //アナログスティック足回り（新村ver）
Ryosei 10:53a92f2ab1c4 490 /////////////////////////////////////////////////////////////////////////////////////
Ryosei 10:53a92f2ab1c4 491
Ryosei 10:53a92f2ab1c4 492
Ryosei 10:53a92f2ab1c4 493
Ryosei 10:53a92f2ab1c4 494
Ryosei 10:53a92f2ab1c4 495
Ryosei 10:53a92f2ab1c4 496
Ryosei 10:53a92f2ab1c4 497 }
Ryosei 10:53a92f2ab1c4 498 }
Ryosei 10:53a92f2ab1c4 499
Ryosei 10:53a92f2ab1c4 500
Ryosei 10:53a92f2ab1c4 501
t_yamamoto 0:562021ed1ba9 502 //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑
Ryosei 10:53a92f2ab1c4 503
t_yamamoto 0:562021ed1ba9 504
t_yamamoto 0:562021ed1ba9 505 MOTOR::Motor::Update(motor);
t_yamamoto 0:562021ed1ba9 506 }
t_yamamoto 0:562021ed1ba9 507 }
t_yamamoto 0:562021ed1ba9 508