1: /* PS4BT_arduino_pro mini_tank_DRV8835_MP3_M1A2_ver05c
2:
3: Arduino pro min + ミニUSBホストシールド2.0 + PS4コントローラ
4: 効果音はDFPlayer Mini
5: モータドライバはDRV8835
6:
7: 効果音制御はサブシステム(arduino_pro mini_tank_MP3_sub_ver04switch)で実施
8: シリアル通信データ
9: エンジン始動音停止 100
10: 砲撃 101
11: 銃撃 102
12: エンジン停止/始動 110
13: ライトスイッチ 120
14: 砲身center 130
15: 砲身UP 131
16: 砲身DOWN 132
17:
18: ver05 2022/03/25
19: 砲身上下操作を右スティックから△□✕ボタンに変更:信号が複数回発生しているのを1回発生に改善。
20:
21: Arduino1.8.15
22: 最大30720バイトのフラッシュメモリのうち、スケッチが24428バイト(79%)を使っています。
23: 最大2048バイトのRAMのうち、グローバル変数が1211バイト(59%)を使っていて、
24: ローカル変数で837バイト使うことができます。
25:
26: 砲撃時のリコイルアクション 砲身のみ (車体のリコイル動作廃止) ver03 2022/03/22
27: ヘッドライト、ブレーキランプはミニUSBシールドのGPOUTピン0,1,2,3
28:
29: 【操作手順】
30: SHARE+PSボタンでペアリング開始。白色インジケータランプ高速点滅。
31: 戦車の電源投入。
32: インジケータランプ青色点灯でペアリング完了。
33:
34: 【走行コントロール】
35: コントローラーの左スティック上下で前後進、
36: 右スティックの左右でステアリングを制御。
37: ステアリングを振っていくと回転軸側が減速していき(緩旋回から信地旋回)、
38: 左スティックを中央に戻し、右スティックを左右一杯に振ると超信地旋回。(速度60%)
39: 停止時にブレーキランプ点灯 ミニUSB GPOUT- 3ピン
40:
41: 【砲身上下】 TRIANGLEボタンでUP。 SQUAREボタンでセンター。 CROSSボタンでDOWN。 ver04b
42: 【砲塔旋回】 左スティック左右で砲塔旋回。離した位置で止まる。
43: 【主砲】 R1ボタンで主砲発射
44: 【機銃】 L1ボタンで機銃射撃
45: 【ヘッドライト】RIGHTボタンを押して点灯、LEFTボタンを押して消灯。ミニUSB GPOUT- 2ピン
46: 【ブレーキランプ】停車時のみ点灯。ミニUSB GPOUT- 3ピン
47: 【BOモード】UPボタンを押してBOライトモード、DOWNボタンを押して通常モード。初期は通常モード。
48: BOライト&BOテール=A0ピン、 BOブレーキ=A1ピン
49: 【エンジン始動+アイドリング効果音】〇ボタンで発生、停止を交互に実施。R3ボタンで強制停止。
50: 【エンジン始動音およびアイドリング音の強制停止】 R3ボタンで強制停止
51: */
52:
53: #include
54: #include
55:
56: #ifdef dobogusinclude
57: #include
58: #endif
59: #include
60:
61: USB Usb;
62: //USBHub Hub1(&Usb); // Some dongles have a hub inside
63: BTD Btd(&Usb); // You have to create the Bluetooth Dongle instance like so
64:
65: /* You can create the instance of the PS4BT class in two ways */
66: // This will start an inquiry and then pair with the PS4 controller - you only have to do this once
67: // You will need to hold down the PS and Share button at the same time,
68: // the PS4 controller will then start to blink rapidly indicating that it is in pairing mode
69: PS4BT PS4(&Btd, PAIR);
70:
71: // After that you can simply create the instance like so and then press the PS button on the device
72: //PS4BT PS4(&Btd);
73:
74: bool printAngle, printTouch;
75: uint8_t oldL2Value, oldR2Value;
76:
77: //#include // needed by Arduino IDE
78:
79: #include // GPOUTピンで主砲LEDを発光させるため
80: MaxGPIO max;
81:
82: byte ser_data;
83:
84: #include
85: VarSpeedServo myservo_2;
86: int servo2 = 3; //servo2 砲塔旋回
87:
88: //const int servo2_sp = 5; //砲塔回転スピード
89: const int LED_5 = A0; //BOライト & BOテール
90: const int LED_6 = A1; //BOブレーキ
91:
92: int ch = 0;
93: int pos_y;
94: int pos_x;
95: int pos_ry;
96: int pos_rx;
97: int motor_speed;
98: int steering;
99: int PWMA = 5; // A入力2/AENABLE 左モータ AIN2
100: int PWMB = 6; // B入力2/BENABLE 右モータ BIN2
101: int AIN1 = 4; // A入力1/APHASE 左モータ AIN1
102: int BIN1 = 7; // B入力1/BPHASE 右モータ BIN1
103: int silent = 1;
104: int state = 0; //変数stateを設定
105: int state1 = 0;
106: int BO = 0;
107: int HL = 0 ;
108:
109: void setup() {
110: Serial.begin (115200);
111: #if !defined(__MIPSEL__)
112: while (!Serial); // Wait for serial port to connect - used on Leonardo,
113: //Teensy and other boards with built-in USB CDC serial connection
114: #endif
115: if (Usb.Init() == -1) {
116: Serial.print(F("\r\nOSC did not start"));
117: while (1); // Halt
118: }
119: Serial.print(F("\r\nPS4 Bluetooth Library Started"));
120:
121: max.write(3, HIGH); //ブレーキ点灯
122:
123: pinMode(PWMA, OUTPUT);
124: pinMode(PWMB, OUTPUT);
125: pinMode(AIN1, OUTPUT);
126: pinMode(BIN1, OUTPUT);
127: pinMode(LED_5, OUTPUT);
128: pinMode(LED_6, OUTPUT);
129: pinMode(servo2, OUTPUT); //servo 2 砲塔旋回
130:
131:
132: //ペアリングミス改善のためサーボの設定はBT接続の後にする
133: myservo_2.attach(3); //servo2 砲塔旋回
134: myservo_2.write(90);
135: myservo_2.detach(); //centerセット時はコメントアウト
136: }
137:
138: void loop() {
139: Usb.Task();
140: if (PS4.connected()) {
141: // myDFPlayer.play(5);
142: // delay(100);
143: if (PS4.getButtonClick(R3)) { //エンジン始動音停止
144: sendData(100);
145: }
146:
147: if (PS4.getButtonClick(CIRCLE)) { //エンジン音停止
148: sendData(110); //エンジン始動
149: }
150:
151:
152: //BOモード ⇔ 通常モード
153: if (PS4.getButtonClick(UP)) {
154: BO = 1; //BOモード
155: lampchk(HL, BO);
156: }
157: if (PS4.getButtonClick(DOWN)) {
158: BO = 0; //通常モード
159: lampchk(HL, BO);
160: }
161:
162: //砲身上下
163: if ( PS4.getButtonClick(SQUARE)) { //CENTER
164: sendData(130);
165: }
166: if (PS4.getButtonClick(TRIANGLE)) { // UP
167: sendData(131);
168: }
169: if (PS4.getButtonClick(CROSS)) { //DOWN
170: sendData(132);
171: }
172:
173: //砲塔旋回
174: pos_rx = PS4.getAnalogHat(LeftHatX);
175: if ( pos_rx > 200) { //右旋回
176: myservo_2.attach(servo2);
177: myservo_2.write(83);
178: } else if (pos_rx < 55) { //左旋回
179: myservo_2.attach(servo2);
180: myservo_2.write(102);
181: } else { //停止
182: myservo_2.attach(servo2); //回転初めに一瞬反対側に回転するのでコメントアウトする
183: myservo_2.write(90);
184: myservo_2.detach();
185: }
186:
187: //砲撃 + リコイル
188: if (PS4.getButtonClick(R1) ) {
189: sendData(101); // 砲撃信号送信
190: }
191:
192: //銃撃
193: if ( PS4.getButtonClick(L1)) {
194: sendData(102); // 銃撃信号送信
195: }
196:
197: //ヘッドライト
198: if (PS4.getButtonClick(RIGHT)) {
199: HL = 1;
200: lampchk(HL, BO);
201: }
202:
203: if (PS4.getButtonClick(LEFT)) {
204: HL = 0;
205: sendData(120); // ライトスイッチ信号送信
206: max.write(2, LOW); //消灯
207: digitalWrite(LED_5, LOW); // BOライト消灯
208: }
209:
210: //走行コントロール
211: pos_y = PS4.getAnalogHat(LeftHatY);
212: pos_x = PS4.getAnalogHat(RightHatX);
213:
214: //左スティックがセンター付近は停止(ブレーキ)
215: if (pos_x >= 102 && pos_x <= 152 && pos_y >= 102 && pos_y <= 152) {
216: motor_run(0, 0, 0, 0, 1, BO);
217: }
218:
219: //前進
220: if (pos_y <= 102 && pos_x >= 102 && pos_x <= 152) {
221: motor_speed = map(pos_y, 102, 0, 0, 255);
222: motor_run(motor_speed, 0, motor_speed, 0, 0, BO);
223: }
224:
225: //後進
226: else if ( pos_y >= 152 && pos_x >= 102 && pos_x <= 152) {
227: motor_speed = map(pos_y, 152, 255, 0, 255) ;
228: motor_run(motor_speed, 1, motor_speed, 1, 0, BO);
229: }
230:
231: //前進右緩旋回、信地旋回
232: else if ( pos_y <= 102 && pos_x > 152) {
233: motor_speed = map(pos_y, 102, 0, 0, 255);
234: steering = motor_speed * (1.00 - (map(pos_x, 152, 255, 0, 255) / 255.00));
235: motor_run(motor_speed, 0, steering, 0, 0, BO);
236: }
237:
238: //前進左緩旋回、信地旋回
239: //else if ( pos_y <= 102 && pos_x < 102 && pos_x >= 10) {
240: else if ( pos_y <= 102 && pos_x < 102) {
241: motor_speed = map(pos_y, 102, 0, 0, 255);
242: steering = motor_speed * (1.00 - (map(pos_x, 102, 0, 0, 255) / 255.00));
243: motor_run(steering, 0, motor_speed, 0, 0, BO);
244: }
245:
246: //後進右緩旋回、信地旋回
247: //else if ( pos_y >= 152 && pos_x > 152 && pos_x <= 245) {
248: else if ( pos_y >= 152 && pos_x > 152 ) {
249: motor_speed = map(pos_y, 152, 255, 0, 255);
250: steering = motor_speed * (1.00 - (map(pos_x, 152, 255, 0, 255) / 255.00));
251: motor_run(motor_speed, 1, steering, 1, 0, BO);
252: }
253:
254: //後進左緩旋回、信地旋回
255: //else if ( pos_y >= 152 && pos_x < 102 && pos_x >= 10) {
256: else if ( pos_y >= 152 && pos_x < 102 ) {
257: motor_speed = map(pos_y, 152, 255, 0, 255);
258: steering = motor_speed * (1.00 - (map(pos_x, 102, 0, 0, 255) / 255.00));
259: motor_run(steering, 1, motor_speed, 1, 0, BO);
260: }
261:
262: //右超信地旋回
263: else if (pos_x > 245 && pos_y >= 102 && pos_y <= 152) {
264: motor_speed = 155;
265: motor_run(motor_speed, 0, motor_speed, 1, 0, BO);
266: }
267:
268: //左超信地旋回
269: else if (pos_x < 10 && pos_y >= 102 && pos_y <= 152) {
270: motor_speed = 155;
271: motor_run(motor_speed, 1, motor_speed, 0, 0, BO);
272: }
273: else { //停止(ブレーキ)
274: motor_run(0, 0, 0, 0, 1, BO);
275: }
276: }
277: }
278:
279: void motor_run(int D0, int D1, int D2, int D3, int D4, int D5) {
280: /* D0 : モータスピード(左)
281: D1 : モータA(左)1 = HIGH / 0 = LOW
282: D2 : モータスピード(右)
283: D3 : モータB(右)1 = HIGH / 0 = LOW
284: D4 : LED_3 ON/OFF 1 = HIGH / 0 = LOW
285: D5 : BOモード
286: */
287: analogWrite(PWMA, D0);
288: digitalWrite(AIN1, D1);
289: analogWrite(PWMB, D2);
290: digitalWrite(BIN1, D3);
291: //digitalWrite(LED_3, D4);
292:
293: if ((D4 == 1) && (D5 == 0)) {
294: max.write(3, HIGH); //ブレーキ点灯
295: digitalWrite(LED_6, LOW); //BOブレーキ消灯
296: }
297: if ((D4 == 1) && (D5 == 1)) {
298: max.write(3, LOW); //ブレーキ消灯
299: digitalWrite(LED_6, HIGH); //BOブレーキ点灯
300: }
301: if (D4 == 0) {
302: max.write(3, LOW); //ブレーキ消灯
303: digitalWrite(LED_6, LOW); //BOブレーキ消灯
304: }
305: }
306:
307: void lampchk(int D7, int D8) {
308: /*ヘッドライトBOモードチェック
309: D7:HL ヘッドライトon-off
310: D8:BOモードon-off
311: */
312: sendData(120); // ライトスイッチ信号送信
313: if ((D7 == 1) && (D8 == 0)) {
314: max.write(2, HIGH); //ヘッドライト点灯
315: digitalWrite(LED_5, LOW); //BOライト消灯
316: }
317: if ((D7 == 1) && (D8 == 1)) {
318: max.write(2, LOW); //ヘッドライト消灯
319: digitalWrite(LED_5, HIGH); // BOライト点灯
320: }
321: }
322:
323: void sendData(byte val) {
324: Serial.write('H'); // ヘッダ送信
325: Serial.write(val); // データ送信
326: }
327:
1: /* arduino_pro mini_M1A2_MP3_sub_ver04c
2: 2022/04/06
3: switch caseコマンドで書き直す
4: 最大30720バイトのフラッシュメモリのうち、スケッチが6906バイト(22%)を使っています。
5: 最大2048バイトのRAMのうち、グローバル変数が520バイト(25%)を使っていて、
6: ローカル変数で1528バイト使うことができます。
7:
8: 本サブシステムの内容
9: MP3プレーヤーで効果音を発生
10: エンジン始動信号受信でエンジン始動、アイドリング音発生。
11:
12: 走行制御に影響する動作をサブ側へ移動
13: 主砲発砲の砲身リコイルと発光をサブ側で対応
14: 機銃発行をサブ側で対応
15: 砲身上下動作をサブ側で対応
16:
17: シリアル通信データ
18: エンジン始動 100
19: 砲撃 101
20: 銃撃 102
21: エンジン停止/始動 110
22: ライトスイッチ 120
23: 砲身center 130
24: 砲身UP 131
25: 砲身DOWN 132
26: */
27:
28: #include
29:
30: #include
31: SoftwareSerial mySerial(A2, A3); // DFPlayer TX,RX
32: DFRobotDFPlayerMini myDFPlayer;
33:
34: #include
35: VarSpeedServo myservo_1;
36: VarSpeedServo myservo_3;
37:
38: int servo1 = 2; //servo1 砲身上下
39: int servo3 = 8; //servo3 砲身リコイル
40: int servo1_pos = 90; //servo1 砲身上下
41: int servo3_pos = 90; //servo3 砲身リコイル
42: int servo1_sp = 20; //砲身上下スピード
43:
44: byte recv_data; // 受信データ
45: int silent = 1;
46: int d_time = 200;
47: int LED_1 = 4; //主砲
48: int LED_2 = 5; //機銃
49:
50: void trancelate() //
51: {
52: switch (recv_data) {
53: case 100: Serial.println("100"); eng_s(); break; // エンジン始動音停止
54: case 101: Serial.println("101"); fire(); break; //砲撃
55: case 102: Serial.println("102"); gun(); break; //銃撃
56: case 110: Serial.println("110"); eng_e_s(); break; //エンジン停止/始動
57: case 120: Serial.println("120"); light_sw(); break; //ライトスイッチ
58: case 130: Serial.println("130"); barrel_c(); break; //砲身CENTER
59: case 131: Serial.println("131"); barrel_u(); break; //砲身UP
60: case 132: Serial.println("132"); barrel_d(); break; //砲身DOWN
61: default: //どのcaseにも合致しないとき
62: Serial.println(" other button ");
63: } // End Case
64: delay(100);
65: } //END translate
66:
67: void eng_s() { //100 エンジン始動音停止
68: myDFPlayer.disableLoopAll(); //stop loop all mp3 files.
69: silent = 1;
70: }
71:
72: void fire() { //101 砲撃
73: PLAY_MP3(26); //砲撃音
74:
75: digitalWrite(LED_1, HIGH); //主砲発光
76: myservo_3.attach(servo3);
77: myservo_3.write(servo3_pos - 70, 255); //砲身リコイル
78: //myservo_3.wait();
79: delay(100);
80: //motor_run(100, 1, 100, 1, 1, BO);
81: //ch = 1000; //約1秒間は砲撃不可
82: delay(60);
83: //motor_run(0, 0, 0, 0, 1, BO);
84: digitalWrite(LED_1, LOW); //主砲消灯
85: delay(80);
86: myservo_3.write(servo3_pos, 20, true);
87: //motor_run(50, 0, 50, 0, 1, BO);
88: //delay(100);
89: //motor_run(0, 0, 0, 0, 1, BO);
90: //myservo_3.wait();
91: //delay(500);
92: myservo_3.detach();
93: }
94:
95: void gun() { //102 銃撃
96: PLAY_MP3(24);
97: for ( int i = 0; i < 7 ; i++) {
98: digitalWrite(LED_2, HIGH); //機銃発光
99: delay(80);
100: digitalWrite(LED_2, LOW); //機銃消灯
101: delay(120);
102: }
103: }
104:
105: void eng_e_s() { //110 エンジン停止/始動
106: silent = 1 - silent;
107: // recv_data = 0;
108: }
109:
110: void light_sw() { //120 ライトスイッチ
111: PLAY_MP3(27);
112: }
113:
114: void barrel_c() { //130 砲身center
115: myservo_1.attach(servo1);
116: myservo_1.write(servo1_pos, servo1_sp, true);
117: myservo_1.detach();
118: }
119:
120: void barrel_u() { //131 砲身up
121: myservo_1.attach(servo1);
122: myservo_1.write(servo1_pos - 60, servo1_sp, true);
123: myservo_1.detach();
124: }
125:
126: void barrel_d() { //132 砲身down
127: myservo_1.attach(servo1);
128: myservo_1.write(servo1_pos + 30, servo1_sp, true);
129: myservo_1.detach();
130: }
131:
132:
133: void setup() {
134: Serial.begin (115200);
135: mySerial.begin (9600);
136:
137: myDFPlayer.begin(mySerial);
138: delay(100); //設定待ち時間は仮置き
139: myDFPlayer.volume(25); //Set volume value (0~30)
140: myDFPlayer.play(28); //エンジン始動
141: delay(1000);
142: //myDFPlayer.loop(5); //Loop 5th mp3 アイドリング
143:
144: pinMode(LED_1, OUTPUT); //主砲
145: pinMode(LED_2, OUTPUT); //機銃
146: //pinMode(servo1, OUTPUT); //servo 1 砲身上下
147: //pinMode(servo3, OUTPUT); //servo 3 砲身リコイル
148:
149: myservo_1.attach(servo1);
150: myservo_1.write(servo1_pos, 255, true);
151: myservo_1.detach();
152: myservo_3.attach(servo3);
153: myservo_3.write(servo3_pos, 255, true);
154: myservo_3.detach();
155: }
156:
157: void loop() {
158:
159: Serial.println("*");
160: //受信バッファに2バイト(ヘッダ+データ)以上のデータが着ているか確認
161: if ( Serial.available() >= 2 ) {
162: // ヘッダの確認
163: if ( Serial.read() == 'H' ) {
164: recv_data = Serial.read() ;
165: trancelate();
166: }
167: }
168:
169: if (silent == 1) {
170: myDFPlayer.disableLoop(); //アイドリング停止
171: } else if ( (silent == 0) && (recv_data == 110)) { //エンジン始動
172: myDFPlayer.play(28); //エンジン始動
173: delay(2000);
174: myDFPlayer.loop(5); //Loop 5th mp3 アイドリング
175: delay(d_time);
176: } else {
177: myDFPlayer.loop(5); //Loop 5th mp3 アイドリング
178: delay(d_time);
179: }
180: recv_data = 0;
181: }
182:
183: void PLAY_MP3(int LT) {
184: recv_data = 0;
185: myDFPlayer.play(LT);
186: //delay(100);
187: }
188: