Miya Miyagawa / Mbed 2 deprecated Orewotaosita

If you wont to knock me down, look this file...

Dependencies:   mbed Servo

System/Process/Process.cpp@0:4df75b08b14a, 2019-05-03 (annotated)

Committer:
Ryosei
Date:
Fri May 03 11:47:27 2019 +0000
Revision:
0:4df75b08b14a
a

Who changed what in which revision?

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