RN4020(bluetoothモジュール)を用いてiphoneとarduinoでの通信しドローンの無線操作を行いたい

前提・実現したいこと

RN4020(bluetoothモジュール)を用いてiphoneとarduinoでの通信を行い、iphoneの画面で1を入力したならドローンが上昇、0を入力したら下降というプログラムを作成したいと考えています。
現在は、その前段階としてiphoneの画面で1を入力したならLEDが点灯0を入力したらLEDが消灯のプログラムを作成しました。

発生している問題・エラーメッセージ

Rn4020を用いてiphoneとarduinoでの通信とドローン制御自体は行うことができたのですが、ドローン制御とiphoneとarduinoでの通信のプログラムを組み合わせること(どのタイミングでプログラムを追加するかが分からない)ができなくて困っています。ご教授お願いします。

試したこと

ドローン制御はこちらのサイトのプログラムを使用しました。
https://tora-k.com/2019/10/12/quadpid/

RN4020を用いてiphoneとarduinoでの通信はドローンの上昇、下降の箇所はLEDの点灯にして試作しました。
こちらのサイトを参考にしました。
https://qiita.com/gome1234/items/a456cd4d1f0db2a99fd0
また、実際に組み合わせてみました(実行自体はできたが、1を入力するとLEDが点滅し続け、0を入力しても消灯しなかった)

該当のソースコード(RN4020を用いてiphoneとarduinoでの通信)

c
1#include <SoftwareSerial.h>
2SoftwareSerial mySerial(7, 8); // RX, TX
3String buff;
4char KEY  = 0;
5String RD;
6String TD; 
7int i  = 0, mode = 0 ;
8long oneSec = 0;
9int input=0;
10void setup() {
11  Serial.begin(115200); // ハードウェアシリアルを準備
12  mySerial.begin(115200);
13  
14  pinMode(9, OUTPUT);
15 Serial.println("/*****Arduino Nano*****/");
16
17//  /**************************************************************
18//   子機：ペリフェラル
19//  **************************************************************/
20//  //出荷時設定
21  mySerial.println("SF,1");
22  //ボーレート設定
23  mySerial.println("SB,1");
24  //Reboot
25  mySerial.println("R,1");
26  //ソフトウェアシリアル変更
27  mySerial.end();
28  mySerial.begin(9600);
29  //エコー
30  mySerial.println("+");
31  //Device Name
32  mySerial.println("SDM,Arduino_nano");
33//  /**************************************************************
34//   SS:サービス設定
35//   SetRegister
36//   0x00000001 : User
37//  **************************************************************/
38  mySerial.println("SS,00000001");
39//  /**************************************************************
40//   SR:機能設定
41//   SetRegister
42//   0x20000000 : パワーサイクル、再起動、切断後にアドバタイズ開始
43//   0x10000000 : シリアル非同期転送サービス
44//   0x04000000 : No Direct Advertisement
45//   0x02000000 : UART Flow
46//  **************************************************************/
47  mySerial.println("SR,36000000");
48  //Reboot
49  mySerial.println("R,1");
50  delay(500);
51    
52
53    
54  Check_CMD();
55  PORTB &= ~_BV(1);
56  Serial.println("/******START BLE******/");
57  Serial.println("Ready");
58
59while(1){
60  if(mySerial.available() > 0) {
61    input = mySerial.read();
62    if(input=='9'){
63    Serial.println("OK");
64      break;
65    }
66}
67  }
68}
69void loop() {
70
71if (mySerial.available() > 0) {
72    mode = mySerial.read();//シリアル通信で受け取ったデータを読み込む
73    if (mode == '1') {//1が送られてきたらLEDをON
74      digitalWrite(9, HIGH);
75      Serial.println("ON");
76 }
77}
78
79 if (mySerial.available() > 0) {
80  mode=mySerial.read();
81  if(mode == '0') {//0が送られてきたらLEDをOFF
82      digitalWrite(9, LOW);
83      Serial.println("OFF");
84
85 }
86 
87 }
88 
89}
90
91//RN4020 エコー確認
92void Echo_Print(){
93  while(1){
94    if(mySerial.available()>0){
95      buff = mySerial.readString();
96      Serial.print(buff);
97      break;
98    }
99  }
100}
101//RN4020 コマンド受付チェック
102void Check_CMD(){
103  while(1){
104    if(mySerial.available()){
105      buff = mySerial.readString();
106    }
107    if((buff.indexOf("CMD") >= 63) || (buff.indexOf("CMD") == 0)){
108      i = 0;
109      break;
110    }
111    if(i >= 1000){
112      Serial.println("Reset");
113      mySerial.println("R,1");
114      i = 0;
115    }
116    i++;
117    delay(1);
118  }
119}

######該当のソースコード(組み合わせた)

c
1#include <MadgwickAHRS.h>
2#include "MPU9250_ktora.h"
3#include <MadgwickAHRS.h>
4#include "MPU9250_ktora.h"
5#include <SoftwareSerial.h>
6SoftwareSerial mySerial(7, 8); // RX, TX 変える箇所
7MPU9250_ktora mpu9250; //MPU9250
8Madgwick MadgwickFilter; //Madgwick
9
10String buff;
11char KEY  = 0;
12String RD;
13String TD; 
14int i  = 0, mode = 0 ;
15long oneSec = 0;
16int input=0;
17
18float errDegY=0,preErrDegY=0,refDegY=0,sumErrY=0, outputRoll=0,throttle=10;
19float errDegX=0,preErrDegX=0,refDegX=0,sumErrX=0, outputPitch=0;
20float degY=0,degX=0,degZ=0;//目標角度
21
22float pitch = 0.0;
23float roll = 0.0;
24float yaw = 0.0;
25
26#define moter1 3
27#define moter2 5
28#define moter3 6
29#define moter4 9
30
31#define Kp 5
32#define Ki 0
33#define Kd 0
34#define Dt 5
35
36#define Pwm_Max 255 //最大パルス幅
37#define Pwm_Min 0 //最小パルス幅
38
39int LF=0;//moter1
40int RF=0;//moter2
41int RB=0;//moter3
42int LB=0;//moter4
43
44void setup() {
45  Serial.begin(115200); // ハードウェアシリアルを準備115200
46  mySerial.begin(115200);
47  //Serial.begin(9600);  9600にさげるべきかも
48  pinMode(9, OUTPUT);
49  pinMode(moter1,OUTPUT);
50  pinMode(moter2,OUTPUT);
51  pinMode(moter3,OUTPUT);
52  pinMode(moter4,OUTPUT);
53  Serial.println("/*****Arduino Nano*****/");
54  //  /**************************************************************
55//   子機：ペリフェラル
56//  **************************************************************/
57//  //出荷時設定
58  mySerial.println("SF,1");
59  //ボーレート設定
60  mySerial.println("SB,1");
61  //Reboot
62  mySerial.println("R,1");
63  //ソフトウェアシリアル変更
64  mySerial.end();
65  mySerial.begin(9600);
66  //エコー
67  mySerial.println("+");
68  //Device Name
69  mySerial.println("SDM,Arduino_nano");
70//  /**************************************************************
71//   SS:サービス設定
72//   SetRegister
73//   0x00000001 : User
74//  **************************************************************/
75  mySerial.println("SS,00000001");
76//  /**************************************************************
77//   SR:機能設定
78//   SetRegister
79//   0x20000000 : パワーサイクル、再起動、切断後にアドバタイズ開始
80//   0x10000000 : シリアル非同期転送サービス
81//   0x04000000 : No Direct Advertisement
82//   0x02000000 : UART Flow
83//  **************************************************************/
84  mySerial.println("SR,36000000");
85  //Reboot
86  mySerial.println("R,1");
87  delay(500);
88    
89  Check_CMD();
90  PORTB &= ~_BV(1);
91  Serial.println("/******START BLE******/");
92  Serial.println("Ready");
93
94while(1){
95  if(mySerial.available() > 0) {
96    input = mySerial.read();
97    if(input=='9'){
98    Serial.println("OK");
99      break;
100    }
101}
102  }
103  
104  Wire.begin(); //I2C通信を開始する
105  mpu9250.set_Wire(&Wire); //Wireのコピー
106  mpu9250.setup_Mpu9250(); //mpu9250の設定
107//mpu9250.set_Offset(0,0,0,0,0,0,-28.10,-11.27,-6.04); //オフセットの設定
108  mpu9250.calc_Offset(); //オフセットの計算
109
110  MadgwickFilter.begin(20); //20hz
111  delay(3000); //3秒待機
112}
113
114//RN4020 コマンド受付チェック
115void Check_CMD(){
116  while(1){
117    if(mySerial.available()){
118      buff = mySerial.readString();
119    }
120    if((buff.indexOf("CMD") >= 63) || (buff.indexOf("CMD") == 0)){
121      i = 0;
122      break;
123    }
124    if(i >= 1000){
125      Serial.println("Reset");
126      mySerial.println("R,1");
127      i = 0;
128    }
129    i++;
130    delay(1);
131  }
132}
133
134void loop() {
135  mpu9250.update_Data_Mpu9250(); //データの更新
136  
137  float AccX = mpu9250.get_AccX();
138  float AccY = mpu9250.get_AccY();
139  float AccZ = mpu9250.get_AccZ();
140
141  float GyrX = mpu9250.get_GyrX();
142  float GyrY = mpu9250.get_GyrY();
143  float GyrZ = mpu9250.get_GyrZ();
144
145  float MagX = mpu9250.get_MagX();
146  float MagY = mpu9250.get_MagY();
147  float MagZ = mpu9250.get_MagZ();
148
149  if (mySerial.available() > 0) {
150    mode = mySerial.read();//シリアル通信で受け取ったデータを読み込む
151    if (mode == '1') {//1が送られてきたらLEDをON
152      digitalWrite(9, HIGH);
153      Serial.println("ON");
154 }
155  }
156
157 if (mySerial.available() > 0) {
158  mode=mySerial.read();
159  if(mode == '0') {//0が送られてきたらLEDをOFF
160      digitalWrite(9, LOW);
161      Serial.println("OFF");
162
163 }
164 
165 }
166
167  //計算処理
168  MadgwickFilter.update(GyrX,GyrY,GyrZ,AccX,AccY,AccZ,MagX,MagY,MagZ);
169  
170  pitch = MadgwickFilter.getPitch();
171  roll = MadgwickFilter.getRoll();
172  yaw = MadgwickFilter.getYaw() -180; //(-180~180)に調整
173
174  errDegY = roll - refDegY;
175  sumErrY = sumErrY + errDegY * Dt / 1000.0;//PID用の偏差の積分値(roll)
176  outputRoll = Kp * errDegY + Ki * sumErrY + Kd * (errDegY - preErrDegY);
177  preErrDegY = errDegY;
178
179  errDegX = pitch - refDegX;
180  sumErrX = sumErrX + errDegX * Dt / 1000.0;//PID用の偏差の積分値(roll)
181  outputPitch = Kp * errDegX + Ki * sumErrX + Kd * (errDegX - preErrDegX);
182  preErrDegX = errDegX;
183
184  LF = throttle - outputRoll - outputPitch;
185  RF = throttle - outputRoll + outputPitch;
186  RB = throttle + outputRoll + outputPitch;
187  LB = throttle + outputRoll - outputPitch;
188
189  
190  
191  LF=max(LF,Pwm_Min);
192  RF=max(RF,Pwm_Min);
193  RB=max(RB,Pwm_Min);
194  LB=max(LB,Pwm_Min);
195
196  LF=min(LF,Pwm_Max);
197  RF=min(RF,Pwm_Max);
198  RB=min(RB,Pwm_Max);
199  LB=min(LB,Pwm_Max);
200
201  Serial.print(LF); Serial.print(" ");
202  Serial.print(RF); Serial.print(" ");
203  Serial.print(RB); Serial.print(" ");
204  Serial.println(LB);
205  
206  analogWrite(moter1,LF);
207  analogWrite(moter2,RF);
208  analogWrite(moter3,RB);
209  analogWrite(moter4,LB);
210
211  delay(5);
212}
213
214

y_waiwai

2021/01/21 07:04

blurtoothではそもそも十数メートルしか届かないもんですが、そこらへんは大丈夫なんでしょうか

komekopan

2021/01/21 07:11

室内で目の見える範囲で飛ばす予定なので大丈夫です

行動規範の内容に同意します

回答1件

送信をprintlnで、行単位で送信してるんですから、受信も行単位で処理しましょう。
ある程度の大きさのバッファを用意しておいて、そこに受信文字を順番に格納していくようにします。
0. 受信文字があるか、なければリターン
0. 受信文字が改行か、
0. 改行でないなら、バッファに受信文字を格納してリターン
0. 改行なら、バッファに'\0'(文字列終端文字)を入れて、そのバッファを1行分の受信メッセージとして処理を行う

というシーケンスにして、1行単位で受信、処理しましょう

投稿2021/01/21 07:23

y_waiwai

総合スコア88038