fc2ブログ

Snake Robot No.2 White Snake その３

その３　ソフトウェア編　2021年4月7日
このSnakeRobotの原型は2017年にjoesinstructablesがinsutructables circuitsに発表された作品を参考にしています。
ここにハード面・ソフト面共に詳しく解説されています。興味のある方は製作の前に塾続して下さい。

先ずは、White Snakeの動画をご覧下さい。


コントローラにBlynkを使います。
先の1号機では専用の自作BLEリモコンを使いましたが、White Snakeでは汎用性を考慮してスマートフォンアプリ
Blinkを使ったコントローラーを使います。
BlynkはBluetoothLEに対応していて更に、OSがAndroidとiPhpneのスマートフォンを共にサポートしています。
Blynk APPをAndroid版はGoogle PlayにてBlynkを、iPhone版はAPP StoreにてBlynk - IoT for Arduino, ESP32を
夫々ダウンロードとインストールを済ませておきます。

Blynkを使ったソースファイル
ArduinoIDEに張り付け用の行番号なしのファイルはこちをクリックすると開きます。⇒　WhiteSnake.txt

使用するサーボモータの特性やServo libraryの関係でハード的に90度に正しく設定していても左右どちらかへ偏る
ことが有り、ソフト面これを補正する必要があります。
また、IRセンサーの感知角度が狭いことや車型ロボットと違って蛇行しながら進むので障害物感知が難しいと思います。
このソースファイルは私のWhite Snake用で、新たに作られる方はそのロボットに合わせた変更が必要です。

以下に要点のみ注釈を付けました。
006:#define BLYNK_PRINT Serial
007:#define BLYNK_USE_DIRECT_CONNECT
008:#include "BlynkSimpleEsp32_BLE.h"
009:#include "ESP32Servo.h" // Ver.0.90 https://www.arduinolibraries.info/libraries/esp32-servo
010:#include "Wire.h" // I2c　測距センサーに使用。
011:#define SENSOR_ADRS   0x40
012:#define DISTANCE_ADRS 0x5E
013://先の1号機より、サーボモーターを2個減らして12個+1個としました。
014:Servo servo1;
015:Servo servo2;
016:Servo servo3;
017:Servo servo4;
018:Servo servo5;
019:Servo servo6;
020:Servo servo7;
021:Servo servo8;
022:Servo servo9;
023:Servo servo10;
024:Servo servo11;
025:Servo servo12;
026:Servo servo_pan;
027://使用するピン番号も変更しています。
028:int servo1Pin = 13;
029:int servo2Pin = 12;
030:int servo3Pin = 14;
031:int servo4Pin = 27;
032:int servo5Pin = 26;
033:int servo6Pin = 25;
034:int servo7Pin = 33;
035:int servo8Pin = 15;
036:int servo9Pin = 2;
037:int servo10Pin = 4;
038:int servo11Pin = 16;
039:int servo12Pin = 17;
040:int servo_panPin = 32; // 測距センサー用サーボモータ
041:// 以下の2行はSG90サーボモータ用設定値。必要に応じて調整が可能です。
042:int minUs = 500;
043:int maxUs = 2400;
044:int counter = 0; // ループカウンタの変数
045:float lag = .5712; // セグメント間の位相遅れ
046:int frequency = 1; // セグメントの発振周波数。
047:int amplitude = 40; // 蛇行運動の振幅
048:int rightOffset = 6; // Right turn Offset 使用したサーボモーターの特性で左へ偏るので右側を5+1としています。
049:int leftOffset = -5; // Left turn Offset
050:int delayTime = 5; // Delay between limb movements
051:int startPause = 5000;  // ロボットを配置するための遅延時間
052:int Offset = 1; // 048行と同じ理由で直進せずに少し左寄りに進むため、右寄りに+1補正しています。
053:// 上のOffsetはサーボモータの偏りを補正しています。.
054:int ModeLED = 5; // Mode Indicator LED
055:int Mode = 0; // Mode initial value
056:int button2 = 0; // Forwardボタンの初期値
057:int button3 = 0; // TurnRightボタン初期値
058:int button4 = 0; // Backwardボタンの初期値
059:int button5 = 0; // TurnLeftボタンの初期値
060:int wallDistanceTolerance = 30; // 障害物感知距離の設定、ここでは30㎝に設定しています。
061:byte des[2] ;
062:uint8_t Front_distance = 0;
063:uint8_t Left_distance = 0;
064:uint8_t Right_distance = 0;
065:char auth[] = "********************"; // Blynkの設定時に取得したAuth Tokenを記入します。
066:
067:void setup() {
068:  Serial.begin(115200) ; // USB Serial
069:  Wire.begin() ; // I2c
070:  Serial.println("Waiting for connections...");
071:  Blynk.setDeviceName("WHITE SNAKE"); // 判りやすい任意の名前を付けます。
072:  Blynk.begin(auth);
073:  pinMode(ModeLED, OUTPUT); digitalWrite(ModeLED, HIGH); // Mode LED default settings.
074:  // Attach segments to Pins
075:  servo1.setPeriodHertz(50);
076:  servo2.setPeriodHertz(50);
077:  servo3.setPeriodHertz(50);
078:  servo4.setPeriodHertz(50);
079:  servo5.setPeriodHertz(50);
080:  servo6.setPeriodHertz(50);
081:  servo7.setPeriodHertz(50);
082:  servo8.setPeriodHertz(50);
083:  servo9.setPeriodHertz(50);
084:  servo10.setPeriodHertz(50);
085:  servo11.setPeriodHertz(50);
086:  servo12.setPeriodHertz(50);
087:  servo_pan.setPeriodHertz(50);
088:
089:  servo1.attach(servo1Pin, minUs, maxUs);
090:  servo2.attach(servo2Pin, minUs, maxUs);
091:  servo3.attach(servo3Pin, minUs, maxUs);
092:  servo4.attach(servo4Pin, minUs, maxUs);
093:  servo5.attach(servo5Pin, minUs, maxUs);
094:  servo6.attach(servo6Pin, minUs, maxUs);
095:  servo7.attach(servo7Pin, minUs, maxUs);
096:  servo8.attach(servo8Pin, minUs, maxUs);
097:  servo9.attach(servo9Pin, minUs, maxUs);
098:  servo10.attach(servo10Pin, minUs, maxUs);
099:  servo11.attach(servo11Pin, minUs, maxUs);
100:  servo12.attach(servo12Pin, minUs, maxUs);
101:  servo_pan.attach(servo_panPin, minUs, maxUs);
102:
103:  // 初期の姿勢になります。1モーションが終了すると元の姿勢で止まります。
104:  servo1.write(90 + Offset + amplitude * cos(5 * lag));
105:  servo2.write(90 + Offset + amplitude * cos(4 * lag));
106:  servo3.write(90 + Offset + amplitude * cos(3 * lag));
107:  servo4.write(90 + Offset + amplitude * cos(2 * lag));
108:  servo5.write(90 + Offset + amplitude * cos(1 * lag));
109  servo6.write(90 + Offset + amplitude * cos(0 * lag));
110:  servo7.write(90 + Offset + amplitude * cos(-1 * lag));
111:  servo8.write(90 + Offset + amplitude * cos(-2 * lag));
112:  servo9.write(90 + Offset + amplitude * cos(-3 * lag));
113:  servo10.write(90 + Offset + amplitude * cos(-4 * lag));
114:  servo11.write(90 + Offset + amplitude * cos(-5 * lag));
115:  servo12.write(90 + Offset + amplitude * cos(-6 * lag));
116:  servo_pan.write(90);
117:}
118:
119:BLYNK_WRITE(V0) {　//　自動／手動　切り替えボタンの設定
120:  int button0 = param.asInt();
121:  if (button0 == 1)
122:  {
123:    Serial.print("Autonomous Mode\n");
124:    Mode = 1; // Autonomous Mode
125:    digitalWrite(ModeLED, LOW);
126:  } else {
127:    Serial.print("Manual Mode\n");
128:    Mode = 0; // Manual Mode
129:    digitalWrite(ModeLED, HIGH);
130: }
131:}
132:
133:BLYNK_WRITE(V1) {　//　キャリブレーションボタンの設定
134:  int button1 = param.asInt();
135:  if (button1 == 1)
136:  {
137:    Serial.print("Calibrate ON\n");
138:    Calibrate();
139:  } else {
140:    Serial.print("Calibrate OFF\n");
141:  }
142:}
143:
144:BLYNK_WRITE(V2) {　//　Forwardボタンの設定
145:  button2 = param.asInt();
146:}
147:
148:BLYNK_WRITE(V3) {　//　TurnRightボタンの設定
149:  button3 = param.asInt();
150:}
151:
152:BLYNK_WRITE(V4) {　//　Backwardボタンの設定
153:  button4 = param.asInt();
154:}
155:
156:BLYNK_WRITE(V5) {　//　TurnLeftボタンの設定
157:  button5 = param.asInt();
158:}
159:
160:void Forward() { // 前進
161:  for (counter = 0; counter < 360; counter += 1)  {
162:    delay(delayTime);
163:    servo1.write(90 + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 + 5 * lag));
164:    servo2.write(90 + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 + 4 * lag));
165:    servo3.write(90 + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 + 3 * lag));
166:    servo4.write(90 + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 + 2 * lag));
167:    servo5.write(90 + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 + 1 * lag));
168:    servo6.write(90 + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 + 0 * lag));
169:    servo7.write(90 + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 - 1 * lag));
170:    servo8.write(90 + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 - 2 * lag));
171:    servo9.write(90 + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 - 3 * lag));
172:    servo10.write(90 + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 - 4 * lag));
173:    servo11.write(90 + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 - 5 * lag));
174:    servo12.write(90 + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 - 6 * lag));
175:  }
176:}
177:
178:void Backward() { // 後退
179:  for (counter = 360; counter > 0; counter -= 1)  {
180:    delay(delayTime);
181:    servo1.write(90 + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 + 5 * lag));
182:    servo2.write(90 + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 + 4 * lag));
183:    servo3.write(90 + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 + 3 * lag));
184:    servo4.write(90 + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 + 2 * lag));
185:    servo5.write(90 + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 + 1 * lag));
186:    servo6.write(90 + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 + 0 * lag));
187:    servo7.write(90 + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 - 1 * lag));
188:    servo8.write(90 + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 - 2 * lag));
189:    servo9.write(90 + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 - 3 * lag));
190:    servo10.write(90 + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 - 4 * lag));
191:    servo11.write(90 + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 - 5 * lag));
192:    servo12.write(90 + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 - 6 * lag));
193:  }
194:}
195:
196:void Turnleft() { // 左旋回
197:  for (counter = 0; counter < 10; counter += 1)  {
198:    delay(delayTime);
199:    servo1.write(90 + Offset + .1 * counter * leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 5 * lag));
200:    servo2.write(90 + Offset + .1 * counter * leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 4 * lag));
201:    servo3.write(90 + Offset + .1 * counter * leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 3 * lag));
202:    servo4.write(90 + Offset + .1 * counter * leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 2 * lag));
203:    servo5.write(90 + Offset + .1 * counter * leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 1 * lag));
204:    servo6.write(90 + Offset + .1 * counter * leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 0 * lag));
205:    servo7.write(90 + Offset + .1 * counter * leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 1 * lag));
206:    servo8.write(90 + Offset + .1 * counter * leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 2 * lag));
207:    servo9.write(90 + Offset + .1 * counter * leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 3 * lag));
208:    servo10.write(90 + Offset + .1 * counter * leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 4 * lag));
209:    servo11.write(90 + Offset + .1 * counter * leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 5 * lag));
210:    servo12.write(90 + Offset + .1 * counter * leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 6 * lag));
211:  }
212:  // Continue left turn
213:  for (counter = 11; counter < 350; counter += 1)  {
214:    delay(delayTime);
215:    servo1.write(90 + Offset + leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 5 * lag));
216:    servo2.write(90 + Offset + leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 4 * lag));
217:    servo3.write(90 + Offset + leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 3 * lag));
218:    servo4.write(90 + Offset + leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 2 * lag));
219:    servo5.write(90 + Offset + leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 1 * lag));
220:    servo6.write(90 + Offset + leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 0 * lag));
221:    servo7.write(90 + Offset + leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 1 * lag));
222:    servo8.write(90 + Offset + leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 2 * lag));
223:    servo9.write(90 + Offset + leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 3 * lag));
224:    servo10.write(90 + Offset + leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 4 * lag));
225:    servo11.write(90 + Offset + leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 5 * lag));
226:    servo12.write(90 + Offset + leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 6 * lag));
227:  }
228:  // Ramp down turn Offset
229:  for (counter = 350; counter < 360; counter += 1)  {
230:    delay(delayTime);
231:    servo1.write(90 + Offset + .1 * (360 - counter)*leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 5 * lag));
232:    servo2.write(90 + Offset + .1 * (360 - counter)*leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 4 * lag));
233:    servo3.write(90 + Offset + .1 * (360 - counter)*leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 3 * lag));
234:    servo4.write(90 + Offset + .1 * (360 - counter)*leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 2 * lag));
235:    servo5.write(90 + Offset + .1 * (360 - counter)*leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 1 * lag));
236:    servo6.write(90 + Offset + .1 * (360 - counter)*leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 0 * lag));
237:    servo7.write(90 + Offset + .1 * (360 - counter)*leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 1 * lag));
238:    servo8.write(90 + Offset + .1 * (360 - counter)*leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 2 * lag));
239:    servo9.write(90 + Offset + .1 * (360 - counter)*leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 3 * lag));
240:    servo10.write(90 + Offset + .1 * (360 - counter)*leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 4 * lag));
241:    servo11.write(90 + Offset + .1 * (360 - counter)*leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 5 * lag));
242:    servo12.write(90 + Offset + .1 * (360 - counter)*leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 6 * lag));
243:  }
244:}
245:
246:void Turnright() { // 右旋回
247:  for (counter = 0; counter < 10; counter += 1)  {
248:    delay(delayTime);
249:    servo1.write(90 + Offset + .1 * counter * rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 5 * lag));
250:    servo2.write(90 + Offset + .1 * counter * rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 4 * lag));
251:    servo3.write(90 + Offset + .1 * counter * rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 3 * lag));
252:    servo4.write(90 + Offset + .1 * counter * rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 2 * lag));
253:    servo5.write(90 + Offset + .1 * counter * rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 1 * lag));
254:    servo6.write(90 + Offset + .1 * counter * rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 0 * lag));
255:    servo7.write(90 + Offset + .1 * counter * rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 1 * lag));
256:    servo8.write(90 + Offset + .1 * counter * rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 2 * lag));
257:    servo9.write(90 + Offset + .1 * counter * rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 3 * lag));
258:    servo10.write(90 + Offset + .1 * counter * rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 4 * lag));
259:    servo11.write(90 + Offset + .1 * counter * rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 5 * lag));
260:    servo12.write(90 + Offset + .1 * counter * rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 6 * lag));
261:  }
262:  // Continue right turn
263:  for (counter = 11; counter < 350; counter += 1)  {
264:    delay(delayTime);
265:    servo1.write(90 + Offset + rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 5 * lag));
266:    servo2.write(90 + Offset + rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 4 * lag));
267:    servo3.write(90 + Offset + rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 3 * lag));
268:    servo4.write(90 + Offset + rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 2 * lag));
269:    servo5.write(90 + Offset + rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 1 * lag));
270:    servo6.write(90 + Offset + rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 0 * lag));
271:    servo7.write(90 + Offset + rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 1 * lag));
272:    servo8.write(90 + Offset + rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 2 * lag));
273:    servo9.write(90 + Offset + rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 3 * lag));
274:    servo10.write(90 + Offset + rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 4 * lag));
275:    servo11.write(90 + Offset + rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 5 * lag));
276:    servo12.write(90 + Offset + rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 6 * lag));
277:  }
278:  // Ramp down turn Offset
279:  for (counter = 350; counter < 360; counter += 1)  {
280:    delay(delayTime);
281:    servo1.write(90 + Offset + .1 * (360 - counter)*rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 5 * lag));
282:    servo2.write(90 + Offset + .1 * (360 - counter)*rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 4 * lag));
283:    servo3.write(90 + Offset + .1 * (360 - counter)*rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 3 * lag));
284:    servo4.write(90 + Offset + .1 * (360 - counter)*rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 2 * lag));
285:    servo5.write(90 + Offset + .1 * (360 - counter)*rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 1 * lag));
286:    servo6.write(90 + Offset + .1 * (360 - counter)*rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 0 * lag));
287:    servo7.write(90 + Offset + .1 * (360 - counter)*rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 1 * lag));
288:    servo8.write(90 + Offset + .1 * (360 - counter)*rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 2 * lag));
289:    servo9.write(90 + Offset + .1 * (360 - counter)*rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 3 * lag));
290:    servo10.write(90 + Offset + .1 * (360 - counter)*rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 4 * lag));
291:    servo11.write(90 + Offset + .1 * (360 - counter)*rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 5 * lag));
292:    servo12.write(90 + Offset + .1 * (360 - counter)*rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 6 * lag));
293:  }
264:}
295:
296:void Calibrate() { // サーボモータの90度設定用
297:  servo1.write(90);
298:  servo2.write(90);
299:  servo3.write(90);
300:  servo4.write(90);
301:  servo5.write(90);
302:  servo6.write(90);
303:  servo7.write(90);
304:  servo8.write(90);
305:  servo9.write(90);
306:  servo10.write(90);
307:  servo11.write(90);
308:  servo12.write(90);
309:}
310:
311:void Autonomous_Mode() { // Autonomous mode　IRセンサーによる自律行動を行います。
312:  servo_pan.write(170);
313:  Left_distance = 0;
314:  delay(500);
315:  Wire.beginTransmission(SENSOR_ADRS) ;
316:  Wire.write(DISTANCE_ADRS) ;
317:  Right_distance = Wire.endTransmission() ;
318:  if (Left_distance == 0) {
319:    Left_distance = Wire.requestFrom(SENSOR_ADRS, 2) ;
320:    des[0] = Wire.read()  ;
321:    des[1] = Wire.read()  ;
322:    Left_distance = ((des[0] * 16 + des[1]) / 16) / 4 ; // 左側の測距値
323:    Serial.print("Left_distance = ") ;
324:    Serial.print(Left_distance) ;
325:    Serial.println("cm") ;
326:  } else {
327:    Serial.print("ERROR NO.=") ;
328:  }
329:  delay(500);
330:  servo_pan.write(10);
331:  Right_distance = 0;
332:  delay(500);
333:  Wire.beginTransmission(SENSOR_ADRS) ;
334:  Wire.write(DISTANCE_ADRS) ;
335:  Right_distance = Wire.endTransmission() ;
336:  if (Right_distance == 0) {
337:    Right_distance = Wire.requestFrom(SENSOR_ADRS, 2) ;
338:    des[0] = Wire.read()  ;
339:    des[1] = Wire.read()  ;
340:    Right_distance = ((des[0] * 16 + des[1]) / 16) / 4 ； // 右側の測距値
341:    Serial.print("Right_distance = ") ;
342:    Serial.print(Right_distance) ;
343:    Serial.println("cm") ;
344:  } else {
345:    Serial.print("ERROR NO.=") ;
346:  }
347:  delay(500);
348:  servo_pan.write(90);
349:  if (Left_distance > Right_distance ) { // 左側測距値より右側測距値が小さい場合
350:    Backward(); // １モーション後退
351:    delay(200) ;
352:    Turnleft(); // 1モーション左旋回
353    Serial.println("Turnleft");
354:  }
355:  else if (Left_distance <= Right_distance ) { // 左側測距値より右側測距値が大きい場合
356:    Backward();// １モーション後退
357:    delay(200);
358:    Turnright(); // 1モーション右旋回
359:    Serial.println("Turnright");
360:  }
361:  else {
362:    Forward(); // 障害物が無い場合は前進
363:    Serial.println("Forward");
364:  }
365:  delay(100) ;
366:}
367:
368:void loop() { // メインループ
369:  Blynk.run(); 
370:  if (button2 == 1) {
371:    Serial.print("Forward\n");
372:    Forward();
373:  }
374:  if (button3 == 1) {
375:    Serial.print("Turnright\n");
376:    Turnright();
377:  }
378:  if (button4 == 1) {
379:    Serial.print("Backward\n");
380:    Backward();
381:  }
382:  if (button5 == 1) {
383:    Serial.print("Turnleft\n");
384:    Turnleft();
385:  }
386:
387:  if (Mode == 1) { // Autonomous Mode　自律行動ON 
388:    Forward(); 
389:    servo_pan.write(90);
390:    delay(200);
391:    // Read basic measurement
392:    Wire.beginTransmission(SENSOR_ADRS) ;
393:    Wire.write(DISTANCE_ADRS) ;
394:    Front_distance = Wire.endTransmission() ;
395:    if (Front_distance == 0) {
396:      Front_distance = Wire.requestFrom(SENSOR_ADRS, 2) ;
397:      des[0] = Wire.read()  ;
398:      des[1] = Wire.read()  ;
399:      Front_distance = ((des[0] * 16 + des[1]) / 16) / 4 ; \\ 前方の測距値
400:      Serial.print("Front_distance = ") ;
401:      Serial.print(Front_distance) ;
402:      Serial.println("cm") ;
403:    } else {
404:      Serial.print("ERROR NO.=") ;
405:    }
406:    delay(100);
407:    if (Front_distance < wallDistanceTolerance) { // 30センチ以内に障害物が有れば自律モードに移ります。
408:      Autonomous_Mode();
409:    }
410:  }
411:}

最後までご覧頂きまして有難うございます。
皆様の参考になれば幸いです。
by Paradise
スポンサーサイト



テーマ : 電子工作
ジャンル : 趣味・実用

Pagetop

Snake Robot No.2 White Snake その２

Blynk_logo_diamond.png
Blynk APPをスマートフォンにインストールしてSnake Robotをコントロール
WhiteSnake4.jpg

スネークロボットのコントロールに必要なBLEに対応したAPPがなく、Blynkを使って簡単なコントローラを作りました。
Blynkを使うもう一つの理由は、AndroidとiPhoneの両方がサポートされているからです。
先ずは、Android版はGoogle PlayにてBlynkを、iPhone版はAPP StoreにてBlynk - IoT for Arduino, ESP32を
夫々ダウンロードとインストールを行います。
Android版とiPhone版ではON/OFFスイッチの位置が逆など画面の構成や操作方法に僅かなが異なりますが、
ここではAndroid版を使った説明を行います。

Blynk APPの設定方法
1.
Blynkをインストールしたら
最初にアカウントを作成します。
Facebook accountでログイン又
は新規アカウンを作成します。
更にBlynkアカウントをお持ち



2.
新規作成の場合、
e-mailとパスワードを
入力します。





3.
サインアップすると
Create New Projectが開きます。
プロジェクト名を入力
SNAKE ROBOT BLE
デバイス名
ESP32 Dev Boardを選択
コネクションタイプ
BLEを選択してCreateをタップ
Screenshot_Blynk_01.jpgScreenshot_Blynk_02.jpgScreenshot_Blynk_03.jpg
4. 
OKをタップすと登録した
メールアドレスにAuth Tokenが
送られてきます。
このTokenをソースファイルに
書き込むので大切にします。



5.
作成初期画面
六角形アイコンがNew Project
Create New Projecを開く
丸＋アイコンがWidget Boxの
部品選択画面です。
三角形アイコンが完成画面を
開きます。		6.
Widget Box
ここで使用部品を選びます。
各部品に点数を表示していて
2000点迄は無料。
ここで1番上のButton200点を
6個選択します。合計1200点
なので無料で使えます。
Screenshot_Blynk_04.jpgScreenshot_Blynk_05.jpgScreenshot_Blynk_06.jpg
7. 選択した6個のButton
後で各Buttonを長押しと
スライドして配置換えします。


8. 次にWidget Boxを下げて
BLEアイコンを選択します。



9. 各ButtonとBLEアイコンを
長押しとスライドして
この画面のように配置を
換えます。
Screenshot_Blynk_07.jpgScreenshot_Blynk_08.jpgScreenshot_Blynk_09.jpg
10.
コントロールボタンの設定
左上Buttonをタップすると
ボタン設定画面①が開きます。
ボタン名にcontrolと入力
下側のOUTPUTをタップすると
次の設定面②が開きます。




11.
設定画面②
ここでVirtualとV0を選択して
Select PinをOKします。







12.
新しく開いた画面では
PinがV0に替わっています。
次に下側のMODEをPUSHから
SWITCH（トグル）に切り替えて
下側のOFFをManual>に
ONをAUTO>に書き換えます。
下側のFont Sizeを大きくします。
注意：iPhonではON/OFFが逆
でFont Sizeが固定です。
Screenshot_Blynk_10.jpgScreenshot_Blynk_11.jpgScreenshot_Blynk_12.jpg
13.
今度は右側のCALボタンを
設定します。
使ったV0にはUSEDとマーク
されています。
今度はV1を選択して
Select PinをOKします。		14.
V1の設定画面では
ボタン名称を90 angleとし、
SWITCH（トグル）を使用。
その他は画像を参考にします。
変更します。


15.
V2　Forwardボタンの設定
画像を参考にして各項目を
設定します。
PUSHスイッチを使います。
その他は画像を参考にします。


Screenshot_Blynk_13.jpgScreenshot_Blynk_14.jpgScreenshot_Blynk_15.jpg
16.
V3　Right Turnボタンの設定
画像を参考にして各項目を
設定します。
PUSHスイッチを使います。
その他は画像を参考にします。		17.
V4　Backwardボタンの設定
画像を参考にして各項目をL
設定します。
PUSHスイッチを使います。
その他は画像を参考にします。		18.
V5 Left Turnボタンの設定
画像を参考にして各項目をL
設定します。
PUSHスイッチを使います。
その他は画像を参考にします。
Screenshot_Blynk_16.jpgScreenshot_Blynk_17.jpgScreenshot_Blynk_18.jpg
19.
V1からV6のの設定完了後
画面がこのようになります。


右端の三角形アイコンをタップ
すると完成画面に替わります。




20.
完成画面では各ボタンが
OFF状態の名称を表示します。
ボタン色がON状態で反転表示
Controlと90angleボタンは
トグルスイッチで、タップ毎に
色が反転します。
デバイスに接続していない時は
アイコンに赤マークを表示。		21.
BLEのコネクト方法
Snake Robotの電源を入れ、
Connect BLE deviceをタップ
すると次の画面に替わります。





Screenshot_Blynk_19.jpgScreenshot_Blynk_20.jpgScreenshot_Blynk_21.jpg
22.
Select BLE device画面
スキャンが実行され、コネクト
可能なデバイス名が表示
されます。
その中のBlynk選んでOKを
タップします。		23.
コネクト完了画面
コネクトするとデバイスの
アイコンに付いていた赤マーク
が消えます。
画像はControlボタンがONの
状態で表示色が反転中です。		24.
Project Settings画面にて
ショートカットアイコンが
作れます。
以上にてオリジナルBlynk
コントローラが完成です。


Screenshot_Blynk_22.jpgScreenshot_Blynk_23.jpgScreenshot_Blynk_24.jpg

次回はBlynkを使ったソースファイルを掲載予定です。
皆様の参考になれば幸いです。
by Paradise

テーマ : 電子工作
ジャンル : 趣味・実用

Pagetop
ホーム
Pagetop
プロフィール

CNC_Paradise

Author:CNC_Paradise
木工・彫刻・機械工作・電子工作が
大好きで自作のCNCを使って工作を
楽しんでます。

最新記事
最新コメント
月別アーカイブ
カテゴリ
何でも工作室
検索フォーム


RSSリンクの表示
リンク
このブログをリンクに追加する
ＱＲコード
QR