001: /*This source code is based on the snake-shaped robot announced by joes on April 23, 2017 at https://www.instructables.com/Snake-Robot/.
002: I replaced the servo motor with a small SG90, changed the microcomputer to ESP32, and controlled it remotely with Bluetooth LE.
003: March 7, 2021 By vsr_Paradise*/
004: 
005: #include "BLEDevice.h" // Ver.1.01 https://www.arduinolibraries.info/libraries/esp32-ble-arduino 
006: #include <ESP32Servo.h> // Ver.0.90 https://www.arduinolibraries.info/libraries/esp32-servo
007: #include <Wire.h> // I2c
008: #define ADDRESS       0x40 // SHARP GP2Y0E03 is a distance measuring sensor unit,I2C address.
009: #define SHIFT_ADDR    0x35
010: #define DISTANCE_ADDR 0x5E
011: 
012: Servo servo1;
013: Servo servo2;
014: Servo servo3;
015: Servo servo4;
016: Servo servo5;
017: Servo servo6;
018: Servo servo7;
019: Servo servo8;
020: Servo servo9;
021: Servo servo10;
022: Servo servo11;
023: Servo servo12;
024: Servo servo13;
025: Servo servo14;
026: Servo servo_pan;
027: 
028: int servo1Pin = 2;
029: int servo2Pin = 4;
030: int servo3Pin = 12;
031: int servo4Pin = 13;
032: int servo5Pin = 14;
033: int servo6Pin = 15;
034: int servo7Pin = 16;
035: int servo8Pin = 17;
036: int servo9Pin = 18;
037: int servo10Pin = 19;
038: int servo11Pin = 23;
039: int servo12Pin = 25;
040: int servo13Pin = 26;
041: int servo14Pin = 27;
042: int servo_panPin = 32; //Distance measurement sensor on Servo.
043: // Published values for SG90 servos; adjust if needed
044: int minUs = 500;
045: int maxUs = 2400;
046: int counter = 0; // Loop counter variable
047: float lag = .5712; // Phase lag between segments
048: int frequency = 1; // Oscillation frequency of segments.
049: int amplitude = 40; // Amplitude of the serpentine motion of the snake
050: int rightOffset = -10; // Right turn Offset //5
051: int leftOffset = 10; // Left turn Offset //-5
052: int Offset = 0;
053: // Correction value when 90 degree PWM signal is input.
054: int s1Offset = 2; 
055: int s2Offset = 11;
056: int s3Offset = 9;
057: int s3Offset = 12;
058: int s5Offset = 8;
059: int s6Offset = 0;
060: int s7Offset = 11;
061: int s8Offset = 6;
062: int s9Offset = 6;
063: int s10Offset = 13;
064: int s11Offset = 1;
065: int s12Offset = -1;
066: int s13Offset = 10;
067: int s14Offset = -2;
068: int delayTime = 5; // Delay between limb movements //7
069: int startPause = 5000;  // Delay time to position robot
070: int ModeLED = 33; // Mode Indicator LED
071: int Mode = 0; // Mode initial value
072: int CAL_SW = 35; // 90 degree servo test switch.
073: int State = 0;
074: int wallDistanceTolerance = 30; // Obstacle detection set distance.
075: uint8_t distance[2] = { 0 };
076: uint8_t shift = 0;
077: uint8_t Front_distance = 0;
078: uint8_t Left_distance = 0;
079: uint8_t Right_distance = 0;
080: char buf_F[100];
081: char buf_L[100];
082: char buf_R[100];
083: 
084: //Get your UUID from Online UUID Generator.
085: //https://www.uuidgenerator.net/version4
086: #define SERVICE_UUID        "********-****-****-****-************"
087: #define CHARACTERISTIC_UUID "********-****-****-****-************"
088: #define DEVICE_NAME         "esp32_BLE"
089: 
090: static BLEUUID  serviceUUID(SERVICE_UUID);
091: static BLEUUID  charUUID(CHARACTERISTIC_UUID);
092: static BLEAddress *pServerAddress;
093: static boolean doConnect = false;
094: static boolean connected = false;
095: static BLERemoteCharacteristic* pRemoteCharacteristic;
096: 
097: static void notifyCallback(
098:   BLERemoteCharacteristic* pBLERemoteCharacteristic,
099:   uint8_t* pData,
100:   size_t length,
101:   bool isNotify) {
102: }
103: 
104: bool connectToServer(BLEAddress pAddress) {
105:   Serial.print("Forming a connection to ");
106:   Serial.println(pAddress.toString().c_str());
107:   BLEClient*  pClient  = BLEDevice::createClient();
108:   pClient->connect(pAddress);
109:   BLERemoteService* pRemoteService = pClient->getService(serviceUUID);
110:   Serial.print(" - Connected to server :");
111:   Serial.println(serviceUUID.toString().c_str());
112:   if (pRemoteService == nullptr) {
113:     Serial.print("Failed to find our service UUID: ");
114:     Serial.println(serviceUUID.toString().c_str());
115:     return false;
116:   }
117:   pRemoteCharacteristic = pRemoteService->getCharacteristic(charUUID);
118:   Serial.print(" - Found our characteristic UUID: ");
119:   Serial.println(charUUID.toString().c_str());
120:   if (pRemoteCharacteristic == nullptr) {
121:     Serial.print("Failed to find our characteristic UUID: ");
122:     return false;
123:   }
124:   Serial.println(" - Found our characteristic");
125:   pRemoteCharacteristic->registerForNotify(notifyCallback);
126: }
127: 
128: class MyAdvertisedDeviceCallbacks: public BLEAdvertisedDeviceCallbacks {
129:     void onResult(BLEAdvertisedDevice advertisedDevice) {
130:       Serial.print("BLE Advertised Device found: ");
131:       Serial.println(advertisedDevice.toString().c_str());
132:       Serial.println(advertisedDevice.getName().c_str());
133: 
134:       if (advertisedDevice.getName() == DEVICE_NAME) { //
135:         Serial.println(advertisedDevice.getAddress().toString().c_str());
136:         advertisedDevice.getScan()->stop();
137:         pServerAddress = new BLEAddress(advertisedDevice.getAddress());
138:         doConnect = true;
139:       }
140:     }
141: };
142: 
143: void setup() {
144:   Serial.begin(115200) ; // USB Serial
145:   Wire.begin() ; // I2c
146:   pinMode(ModeLED, OUTPUT); digitalWrite(ModeLED, HIGH); // Mode LED default settings.
147:   pinMode(CAL_SW, INPUT); digitalWrite(CAL_SW, HIGH); // Initial setting of 90 degree correction test switch.
148:   // Attach segments to Pins
149:   servo1.setPeriodHertz(50);
150:   servo2.setPeriodHertz(50);
151:   servo3.setPeriodHertz(50);
152:   servo4.setPeriodHertz(50);
153:   servo5.setPeriodHertz(50);
154:   servo6.setPeriodHertz(50);
155:   servo7.setPeriodHertz(50);
156:   servo8.setPeriodHertz(50);
157:   servo9.setPeriodHertz(50);
158:   servo10.setPeriodHertz(50);
159:   servo11.setPeriodHertz(50);
160:   servo12.setPeriodHertz(50);
161:   servo13.setPeriodHertz(50);
162:   servo14.setPeriodHertz(50);
163:   servo_pan.setPeriodHertz(50);
164: 
165:   servo1.attach(servo1Pin, minUs, maxUs);
166:   servo2.attach(servo2Pin, minUs, maxUs);
167:   servo3.attach(servo3Pin, minUs, maxUs);
168:   servo4.attach(servo4Pin, minUs, maxUs);
169:   servo5.attach(servo5Pin, minUs, maxUs);
170:   servo6.attach(servo6Pin, minUs, maxUs);
171:   servo7.attach(servo7Pin, minUs, maxUs);
172:   servo8.attach(servo8Pin, minUs, maxUs);
173:   servo9.attach(servo9Pin, minUs, maxUs);
174:   servo10.attach(servo10Pin, minUs, maxUs);
175:   servo11.attach(servo11Pin, minUs, maxUs);
176:   servo12.attach(servo12Pin, minUs, maxUs);
177:   servo13.attach(servo13Pin, minUs, maxUs);
178:   servo14.attach(servo14Pin, minUs, maxUs);
179:   servo_pan.attach(servo_panPin, minUs, maxUs);
180: 
181:   // Put snake in starting position
182:   servo1.write(90 + s1Offset + Offset + amplitude * cos(6 * lag));
183:   servo2.write(90 + s2Offset + Offset + amplitude * cos(5 * lag));
184:   servo3.write(90 + s3Offset + Offset + amplitude * cos(4 * lag));
185:   servo4.write(90 + s3Offset + Offset + amplitude * cos(3 * lag));
186:   servo5.write(90 + s5Offset + Offset + amplitude * cos(2 * lag));
187:   servo6.write(90 + s6Offset + Offset + amplitude * cos(1 * lag));
188:   servo7.write(90 + s7Offset + Offset + amplitude * cos(0 * lag));
189:   servo8.write(90 + s8Offset + Offset + amplitude * cos(-1 * lag));
190:   servo9.write(90 + s9Offset + Offset + amplitude * cos(-2 * lag));
191:   servo10.write(90 + s10Offset + Offset + amplitude * cos(-3 * lag));
192:   servo11.write(90 + s11Offset + Offset + amplitude * cos(-4 * lag));
193:   servo12.write(90 + s12Offset + Offset + amplitude * cos(-5 * lag));
194:   servo13.write(90 + s13Offset + Offset + mplitude * cos(-6 * lag));
195:   servo14.write(90 + s14Offset + Offset + amplitude * cos(-7 * lag));
196:   servo_pan.write(90);
197:   Serial.println("Initializing");
198:   Wire.beginTransmission(ADDRESS);
199:   Wire.write(byte(SHIFT_ADDR));
200:   Wire.endTransmission();
201:   Wire.requestFrom(ADDRESS, 1);
202:   if (1 <= Wire.available()) {
203:     shift = Wire.read();
204:   }
205:   Serial.print("Read shift bit: ");
206:   Serial.println(shift, HEX);
207:   delay(startPause);  // Pause to position robot*/
208: 
209:   BLEDevice::init(DEVICE_NAME);
210:   BLEScan* pBLEScan = BLEDevice::getScan();
211:   Serial.println("getScan");
212:   pBLEScan->setAdvertisedDeviceCallbacks(new MyAdvertisedDeviceCallbacks());
213:   Serial.println("setAdvertisedDeviceCallbacks");
214:   pBLEScan->setActiveScan(true);
215:   pBLEScan->start(10);
216:   Serial.println("");
217:   Serial.println("End of setup");
218: }
219: 
220: void Forward() {
221:   for (counter = 0; counter < 360; counter += 1)  {
222:     delay(delayTime);
223:     servo1.write(90 + s1Offset + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 + 6 * lag));
224:     servo2.write(90 + s2Offset + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 + 5 * lag));
225:     servo3.write(90 + s3Offset + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 + 4 * lag));
226:     servo4.write(90 + s3Offset + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 + 3 * lag));
227:     servo5.write(90 + s5Offset + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 + 2 * lag));
228:     servo6.write(90 + s6Offset + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 + 1 * lag));
229:     servo7.write(90 + s7Offset + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 + 0 * lag));
230:     servo8.write(90 + s8Offset + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 - 1 * lag));
231:     servo9.write(90 + s9Offset + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 - 2 * lag));
232:     servo10.write(90 + s10Offset + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 - 3 * lag));
233:     servo11.write(90 + s11Offset + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 - 4 * lag));
234:     servo12.write(90 + s12Offset + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 - 5 * lag));
235:     servo13.write(90 + s13Offset + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 - 6 * lag));
236:     servo14.write(90 + s14Offset + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 - 7 * lag));
237:   }
238: }
239: 
240: void Backward() {
241:   for (counter = 360; counter > 0; counter -= 1)  {
242:     delay(delayTime);
243:     servo1.write(90 + s1Offset + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 + 6 * lag));
244:     servo2.write(90 + s2Offset + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 + 5 * lag));
245:     servo3.write(90 + s3Offset + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 + 4 * lag));
246:     servo4.write(90 + s3Offset + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 + 3 * lag));
247:     servo5.write(90 + s5Offset + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 + 2 * lag));
248:     servo6.write(90 + s6Offset + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 + 1 * lag));
249:     servo7.write(90 + s7Offset + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 + 0 * lag));
250:     servo8.write(90 + s8Offset + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 - 1 * lag));
251:     servo9.write(90 + s9Offset + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 - 2 * lag));
252:     servo10.write(90 + s10Offset + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 - 3 * lag));
253:     servo11.write(90 + s11Offset + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 - 4 * lag));
254:     servo12.write(90 + s12Offset + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 - 5 * lag));
255:     servo13.write(90 + s13Offset + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 - 6 * lag));
256:     servo14.write(90 + s14Offset + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 - 7 * lag));
257:   }
258: }
259: 
260: void Turnleft() {
261:   for (counter = 0; counter < 12; counter += 1)  {
262:     delay(delayTime);
263:     servo1.write(90 + s1Offset + Offset + .1 * counter * leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 6 * lag));
264:     servo2.write(90 + s2Offset + Offset + .1 * counter * leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 5 * lag));
265:     servo3.write(90 + s3Offset + Offset + .1 * counter * leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 4 * lag));
266:     servo4.write(90 + s3Offset + Offset + .1 * counter * leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 3 * lag));
267:     servo5.write(90 + s5Offset + Offset + .1 * counter * leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 2 * lag));
268:     servo6.write(90 + s6Offset + Offset + .1 * counter * leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 1 * lag));
269:     servo7.write(90 + s7Offset + Offset + .1 * counter * leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 0 * lag));
270:     servo8.write(90 + s8Offset + Offset + .1 * counter * leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 1 * lag));
271:     servo9.write(90 + s9Offset + Offset + .1 * counter * leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 2 * lag));
272:     servo10.write(90 + s10Offset + Offset + .1 * counter * leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 3 * lag));
273:     servo11.write(90 + s11Offset + Offset + .1 * counter * leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 4 * lag));
274:     servo12.write(90 + s12Offset + Offset + .1 * counter * leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 5 * lag));
275:     servo13.write(90 + s13Offset + Offset + .1 * counter * leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 6 * lag));
276:     servo14.write(90 + s14Offset + Offset + .1 * counter * leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 7 * lag));
277:   }
278:   // Continue left turn
279:   for (counter = 13; counter < 350; counter += 1)  {
280:     delay(delayTime);
281:     servo1.write(90 + s1Offset + Offset + leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 6 * lag));
282:     servo2.write(90 + s2Offset + Offset + leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 5 * lag));
283:     servo3.write(90 + s3Offset + Offset + leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 4 * lag));
284:     servo4.write(90 + s3Offset + Offset + leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 3 * lag));
285:     servo5.write(90 + s5Offset + Offset + leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 2 * lag));
286:     servo6.write(90 + s6Offset + Offset + leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 1 * lag));
287:     servo7.write(90 + s7Offset + Offset + leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 0 * lag));
288:     servo8.write(90 + s8Offset + Offset + leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 1 * lag));
289:     servo9.write(90 + s9Offset + Offset + leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 2 * lag));
290:     servo10.write(90 + s10Offset + Offset + leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 3 * lag));
291:     servo11.write(90 + s11Offset + Offset + leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 4 * lag));
292:     servo12.write(90 + s12Offset + Offset + leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 5 * lag));
293:     servo13.write(90 + s13Offset + Offset + leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 6 * lag));
294:     servo14.write(90 + s14Offset + Offset + leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 7 * lag));
295:   }
296:   // Ramp down turn Offset
297:   for (counter = 350; counter < 360; counter += 1)  {
298:     delay(delayTime);
299:     servo1.write(90 + s1Offset + Offset + .1 * (360 - counter)*leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 6 * lag));
300:     servo2.write(90 + s2Offset + Offset + .1 * (360 - counter)*leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 5 * lag));
301:     servo3.write(90 + s3Offset + Offset + .1 * (360 - counter)*leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 4 * lag));
302:     servo4.write(90 + s3Offset + Offset + .1 * (360 - counter)*leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 3 * lag));
303:     servo5.write(90 + s5Offset + Offset + .1 * (360 - counter)*leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 2 * lag));
304:     servo6.write(90 + s6Offset + Offset + .1 * (360 - counter)*leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 1 * lag));
305:     servo7.write(90 + s7Offset + Offset + .1 * (360 - counter)*leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 0 * lag));
306:     servo8.write(90 + s8Offset + Offset + .1 * (360 - counter)*leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 1 * lag));
307:     servo9.write(90 + s9Offset + Offset + .1 * (360 - counter)*leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 2 * lag));
308:     servo10.write(90 + s10Offset + Offset + .1 * (360 - counter)*leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 3 * lag));
309:     servo11.write(90 + s11Offset + Offset + .1 * (360 - counter)*leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 4 * lag));
310:     servo12.write(90 + s12Offset + Offset + .1 * (360 - counter)*leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 5 * lag));
311:     servo13.write(90 + s13Offset + Offset + .1 * (360 - counter)*leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 6 * lag));
312:     servo14.write(90 + s14Offset + Offset + .1 * (360 - counter)*leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 7 * lag));
313:   }
314: }
315: 
316: void Turnright() {
317:   for (counter = 0; counter < 12; counter += 1)  {
318:     delay(delayTime);
319:     servo1.write(90 + s1Offset + Offset + .1 * counter * rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 6 * lag));
320:     servo2.write(90 + s2Offset + Offset + .1 * counter * rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 5 * lag));
321:     servo3.write(90 + s3Offset + Offset + .1 * counter * rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 4 * lag));
322:     servo4.write(90 + s4Offset + Offset + .1 * counter * rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 3 * lag));
323:     servo5.write(90 + s5Offset + Offset + .1 * counter * rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 2 * lag));
324:     servo6.write(90 + s6Offset + Offset + .1 * counter * rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 1 * lag));
325:     servo7.write(90 + s7Offset + Offset + .1 * counter * rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 0 * lag));
326:     servo8.write(90 + s8Offset + Offset + .1 * counter * rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 1 * lag));
327:     servo9.write(90 + s9Offset + Offset + .1 * counter * rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 2 * lag));
328:     servo10.write(90 + s10Offset + Offset + .1 * counter * rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 3 * lag));
329:     servo11.write(90 + s11Offset + Offset + .1 * counter * rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 4 * lag));
330:     servo12.write(90 + s12Offset + Offset + .1 * counter * rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 5 * lag));
331:     servo13.write(90 + s13Offset + Offset + .1 * counter * rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 6 * lag));
332:     servo14.write(90 + s14Offset + Offset + .1 * counter * rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 7 * lag));
333:   }
334:   // Continue right turn
335:   for (counter = 13; counter < 350; counter += 1)  {
336:     delay(delayTime);
337:     servo1.write(90 + s1Offset + Offset + rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 6 * lag));
338:     servo2.write(90 + s2Offset + Offset + rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 5 * lag));
339:     servo3.write(90 + s3Offset + Offset + rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 4 * lag));
340:     servo4.write(90 + s3Offset + Offset + rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 3 * lag));
341:     servo5.write(90 + s5Offset + Offset + rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 2 * lag));
342:     servo6.write(90 + s6Offset + Offset + rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 1 * lag));
343:     servo7.write(90 + s7Offset + Offset + rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 0 * lag));
344:     servo8.write(90 + s8Offset + Offset + rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 1 * lag));
345:     servo9.write(90 + s9Offset + Offset + rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 2 * lag));
346:     servo10.write(90 + s10Offset + Offset + rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 3 * lag));
347:     servo11.write(90 + s11Offset + Offset + rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 4 * lag));
348:     servo12.write(90 + s12Offset + Offset + rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 5 * lag));
349:     servo13.write(90 + s13Offset + Offset + rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 6 * lag));
350:     servo14.write(90 + s14Offset + Offset + rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 7 * lag));
351:   }
352:   // Ramp down turn Offset
353:   for (counter = 350; counter < 360; counter += 1)  {
354:     delay(delayTime);
355:     servo1.write(90 + s1Offset + Offset + .1 * (360 - counter)*rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 6 * lag));
356:     servo2.write(90 + s2Offset + Offset + .1 * (360 - counter)*rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 5 * lag));
357:     servo3.write(90 + s3Offset + Offset + .1 * (360 - counter)*rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 4 * lag));
358:     servo4.write(90 + s3Offset + Offset + .1 * (360 - counter)*rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 3 * lag));
359:     servo5.write(90 + s5Offset + Offset + .1 * (360 - counter)*rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 2 * lag));
360:     servo6.write(90 + s6Offset + Offset + .1 * (360 - counter)*rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 1 * lag));
361:     servo7.write(90 + s7Offset + Offset + .1 * (360 - counter)*rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 0 * lag));
362:     servo8.write(90 + s8Offset + Offset + .1 * (360 - counter)*rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 1 * lag));
363:     servo9.write(90 + s9Offset + Offset + .1 * (360 - counter)*rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 2 * lag));
364:     servo10.write(90 + s10Offset + Offset + .1 * (360 - counter)*rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 3 * lag));
365:     servo11.write(90 + s11Offset + Offset + .1 * (360 - counter)*rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 4 * lag));
366:     servo12.write(90 + s12Offset + Offset + .1 * (360 - counter)*rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 5 * lag));
367:     servo13.write(90 + s13Offset + Offset + .1 * (360 - counter)*rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 6 * lag));
368:     servo14.write(90 + s14Offset + Offset + .1 * (360 - counter)*rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 7 * lag));
369:   }
370: }
371: 
372: void Calibrate() { // servo motor 90degree test.
373:   servo1.write(90 + s1Offset + Offset);
374:   servo2.write(90 + s2Offset + Offset);
375:   servo3.write(90 + s3Offset + Offset);
376:   servo4.write(90 + s3Offset + Offset);
377:   servo5.write(90 + s5Offset + Offset);
378:   servo6.write(90 + s6Offset + Offset);
379:   servo7.write(90 + s7Offset + Offset);
380:   servo8.write(90 + s8Offset + Offset);
381:   servo9.write(90 + s9Offset + Offset);
382:   servo10.write(90 + s10Offset + Offset);
383:   servo11.write(90 + s11Offset + Offset);
384:   servo12.write(90 + s12Offset + Offset);
385:   servo13.write(90 + s13Offset + Offset);
386:   servo14.write(90 + s14Offset + Offset);
387: }
388: 
389: void Autonomous_Mode() { // Autonomous mode
390:   servo_pan.write(170);
391:   Left_distance = 0;
392:   delay(500);
393:   // Read basic measurement
394:   Wire.beginTransmission(ADDRESS);
395:   Wire.write(byte(DISTANCE_ADDR));
396:   Wire.endTransmission();
397:   Wire.requestFrom(ADDRESS, 2);
398:   if (2 <= Wire.available()) {
399:     distance[0] = Wire.read();
400:     distance[1] = Wire.read();
401:     // Print distance in cm
402:     Left_distance = (distance[0] * 16 + distance[1]) / 16 / (int)pow(2, shift);
403:     sprintf(buf_L, "Left_distance %u cm", Left_distance);
404:     Serial.println(buf_L);
405:   }
406:   else {
407:     Serial.println("Read error");
408:   }
409:   delay(500);
410:   servo_pan.write(10);
411:   Right_distance = 0;
412:   delay(500);
413:   // Read basic measurement
414:   Wire.beginTransmission(ADDRESS);
415:   Wire.write(byte(DISTANCE_ADDR));
416:   Wire.endTransmission();
417:   Wire.requestFrom(ADDRESS, 2);
418:   if (2 <= Wire.available()) {
419:     distance[0] = Wire.read();
420:     distance[1] = Wire.read();
421:     // Print distance in cm
422:     Right_distance = (distance[0] * 16 + distance[1]) / 16 / (int)pow(2, shift);
423:     sprintf(buf_R, "Right_distance %u cm", Right_distance);
424:     Serial.println(buf_R);
425:   }
426:   else {
427:     Serial.println("Read error");
428:   }
429:   delay(500);
430:   servo_pan.write(90);
431:   if (Left_distance > Right_distance ) {
432:     Backward();
433:     Turnleft();
434:     Turnleft();
435:     Serial.println("Turnleft");
436:   }
437:   else if (Left_distance <= Right_distance ) {
438:     Backward();
439:     Turnright();
440:     Turnright();
441:     Serial.println("Turnright");
442:   }
443:   else {
444:     Forward();
445:     Serial.println("Forward");
446:   }
447:   delay(100) ;
448: }
449: 
450: void Received_Code() { // Demodulates the received BLE remote control code.
451:   if (doConnect == true) {
452:     if (connectToServer(*pServerAddress)) {
453:       connected = true;
454:     } else {
455:       connected = false;
456:     }
457:     doConnect = false;
458:   }
459:   if (connected) {
460:     std::string value = pRemoteCharacteristic->readValue();
461:     String strVal = value.c_str();
462:     char strNum = strVal.toInt();
463:     Serial.println(strNum);
464:     switch (strNum) {
465:       case 'F': // Forward
466:         Forward();
467:         break;
468:       case 'B': // Backward
469:         Backward();
470:         break;
471:       case 'L': // Turnleft
472:         Turnleft();
473:         break;
474:       case 'R': // Turnright
475:         Turnright();
476:         break;
477:       case '1': // Mode1:Autonomous Mode
478:         Mode = 1;
479:         digitalWrite(ModeLED, LOW);
480:         break;
481:       case '2': // Mode0:Remote Control Mode
482:         Mode = 0;
483:         digitalWrite(ModeLED, HIGH);
484:         break;
485:       default:
486:         break;
487:     }
488:   }
489:   else {
490:     Serial.println("Not connected");
491:     doConnect = true;
492:   }
493:   delay(10);
494: }
495: 
496: void loop() {
497:   Received_Code();
498:   State = digitalRead(CAL_SW);
499:   if (State == LOW) {
500:     Serial.println(State);
501:     Calibrate();
502:   }
503:   if (Mode == 1) { // Autonomous Mode
504:     Forward();
505:     servo_pan.write(90);
506:     delay(500);
507:     // Read basic measurement
508:     Wire.beginTransmission(ADDRESS);
509:     Wire.write(byte(DISTANCE_ADDR));
510:     Wire.endTransmission();
511:     Wire.requestFrom(ADDRESS, 2);
512:     if (2 <= Wire.available()) {
513:       distance[0] = Wire.read();
514:       distance[1] = Wire.read();
515:       // Print distance in cm
516:       Front_distance = (distance[0] * 16 + distance[1]) / 16 / (int)pow(2, shift);
517:       sprintf(buf_F, "Front_distance %u cm", Front_distance);
518:       Serial.println(buf_F);
519:     }
520:     else {
521:       Serial.println("Read error");
522:     }
523:     delay(500);
524:     if (Front_distance < wallDistanceTolerance) {
525:       Autonomous_Mode();
526:     }
527:   }
528: }