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: }