/*This source code is based on the snake-shaped robot announced by joes on April 23, 2017 at https://www.instructables.com/Snake-Robot/. I replaced the servo motor with a small SG90, changed the microcomputer to ESP32, and controlled it remotely with Bluetooth LE. March 7, 2021 By vsr_Paradise*/ #include "BLEDevice.h" // Ver.1.01 https://www.arduinolibraries.info/libraries/esp32-ble-arduino #include // Ver.0.90 https://www.arduinolibraries.info/libraries/esp32-servo #include // I2c #define ADDRESS 0x40 // SHARP GP2Y0E03 is a distance measuring sensor unit,I2C address. #define SHIFT_ADDR 0x35 #define DISTANCE_ADDR 0x5E Servo servo1; Servo servo2; Servo servo3; Servo servo4; Servo servo5; Servo servo6; Servo servo7; Servo servo8; Servo servo9; Servo servo10; Servo servo11; Servo servo12; Servo servo13; Servo servo14; Servo servo_pan; int servo1Pin = 2; int servo2Pin = 4; int servo3Pin = 12; int servo4Pin = 13; int servo5Pin = 14; int servo6Pin = 15; int servo7Pin = 16; int servo8Pin = 17; int servo9Pin = 18; int servo10Pin = 19; int servo11Pin = 23; int servo12Pin = 25; int servo13Pin = 26; int servo14Pin = 27; int servo_panPin = 32; //Distance measurement sensor on Servo. // Published values for SG90 servos; adjust if needed int minUs = 500; int maxUs = 2400; int counter = 0; // Loop counter variable float lag = .5712; // Phase lag between segments int frequency = 1; // Oscillation frequency of segments. int amplitude = 40; // Amplitude of the serpentine motion of the snake int rightOffset = -10; // Right turn Offset //5 int leftOffset = 10; // Left turn Offset //-5 int Offset = 0; // Correction value when 90 degree PWM signal is input. int s1Offset = 2; int s2Offset = 11; int s3Offset = 9; int s4Offset = 12; int s5Offset = 8; int s6Offset = 0; int s7Offset = 11; int s8Offset = 6; int s9Offset = 6; int s10Offset = 13; int s11Offset = 1; int s12Offset = -1; int s13Offset = 10; int s14Offset = -2; int delayTime = 5; // Delay between limb movements //7 int startPause = 5000; // Delay time to position robot int ModeLED = 33; // Mode Indicator LED int Mode = 0; // Mode initial value int CAL_SW = 35; // 90 degree servo test switch. int State = 0; int wallDistanceTolerance = 30; // Obstacle detection set distance. uint8_t distance[2] = { 0 }; uint8_t shift = 0; uint8_t Front_distance = 0; uint8_t Left_distance = 0; uint8_t Right_distance = 0; char buf_F[100]; char buf_L[100]; char buf_R[100]; //Get your UUID from Online UUID Generator. //https://www.uuidgenerator.net/version4 #define SERVICE_UUID "********-****-****-****-************" #define CHARACTERISTIC_UUID "********-****-****-****-************" #define DEVICE_NAME "esp32_BLE" static BLEUUID serviceUUID(SERVICE_UUID); static BLEUUID charUUID(CHARACTERISTIC_UUID); static BLEAddress *pServerAddress; static boolean doConnect = false; static boolean connected = false; static BLERemoteCharacteristic* pRemoteCharacteristic; static void notifyCallback( BLERemoteCharacteristic* pBLERemoteCharacteristic, uint8_t* pData, size_t length, bool isNotify) { } bool connectToServer(BLEAddress pAddress) { Serial.print("Forming a connection to "); Serial.println(pAddress.toString().c_str()); BLEClient* pClient = BLEDevice::createClient(); pClient->connect(pAddress); BLERemoteService* pRemoteService = pClient->getService(serviceUUID); Serial.print(" - Connected to server :"); Serial.println(serviceUUID.toString().c_str()); if (pRemoteService == nullptr) { Serial.print("Failed to find our service UUID: "); Serial.println(serviceUUID.toString().c_str()); return false; } pRemoteCharacteristic = pRemoteService->getCharacteristic(charUUID); Serial.print(" - Found our characteristic UUID: "); Serial.println(charUUID.toString().c_str()); if (pRemoteCharacteristic == nullptr) { Serial.print("Failed to find our characteristic UUID: "); return false; } Serial.println(" - Found our characteristic"); pRemoteCharacteristic->registerForNotify(notifyCallback); } class MyAdvertisedDeviceCallbacks: public BLEAdvertisedDeviceCallbacks { void onResult(BLEAdvertisedDevice advertisedDevice) { Serial.print("BLE Advertised Device found: "); Serial.println(advertisedDevice.toString().c_str()); Serial.println(advertisedDevice.getName().c_str()); if (advertisedDevice.getName() == DEVICE_NAME) { // Serial.println(advertisedDevice.getAddress().toString().c_str()); advertisedDevice.getScan()->stop(); pServerAddress = new BLEAddress(advertisedDevice.getAddress()); doConnect = true; } } }; void setup() { Serial.begin(115200) ; // USB Serial Wire.begin() ; // I2c pinMode(ModeLED, OUTPUT); digitalWrite(ModeLED, HIGH); // Mode LED default settings. pinMode(CAL_SW, INPUT); digitalWrite(CAL_SW, HIGH); // Initial setting of 90 degree correction test switch. // Attach segments to Pins servo1.setPeriodHertz(50); servo2.setPeriodHertz(50); servo3.setPeriodHertz(50); servo4.setPeriodHertz(50); servo5.setPeriodHertz(50); servo6.setPeriodHertz(50); servo7.setPeriodHertz(50); servo8.setPeriodHertz(50); servo9.setPeriodHertz(50); servo10.setPeriodHertz(50); servo11.setPeriodHertz(50); servo12.setPeriodHertz(50); servo13.setPeriodHertz(50); servo14.setPeriodHertz(50); servo_pan.setPeriodHertz(50); servo1.attach(servo1Pin, minUs, maxUs); servo2.attach(servo2Pin, minUs, maxUs); servo3.attach(servo3Pin, minUs, maxUs); servo4.attach(servo4Pin, minUs, maxUs); servo5.attach(servo5Pin, minUs, maxUs); servo6.attach(servo6Pin, minUs, maxUs); servo7.attach(servo7Pin, minUs, maxUs); servo8.attach(servo8Pin, minUs, maxUs); servo9.attach(servo9Pin, minUs, maxUs); servo10.attach(servo10Pin, minUs, maxUs); servo11.attach(servo11Pin, minUs, maxUs); servo12.attach(servo12Pin, minUs, maxUs); servo13.attach(servo13Pin, minUs, maxUs); servo14.attach(servo14Pin, minUs, maxUs); servo_pan.attach(servo_panPin, minUs, maxUs); // Put snake in starting position servo1.write(90 + s1Offset + Offset + amplitude * cos(6 * lag)); servo2.write(90 + s2Offset + Offset + amplitude * cos(5 * lag)); servo3.write(90 + s3Offset + Offset + amplitude * cos(4 * lag)); servo4.write(90 + s4Offset + Offset + amplitude * cos(3 * lag)); servo5.write(90 + s5Offset + Offset + amplitude * cos(2 * lag)); servo6.write(90 + s6Offset + Offset + amplitude * cos(1 * lag)); servo7.write(90 + s7Offset + Offset + amplitude * cos(0 * lag)); servo8.write(90 + s8Offset + Offset + amplitude * cos(-1 * lag)); servo9.write(90 + s9Offset + Offset + amplitude * cos(-2 * lag)); servo10.write(90 + s10Offset + Offset + amplitude * cos(-3 * lag)); servo11.write(90 + s11Offset + Offset + amplitude * cos(-4 * lag)); servo12.write(90 + s12Offset + Offset + amplitude * cos(-5 * lag)); servo13.write(90 + s13Offset + Offset + amplitude * cos(-6 * lag)); servo14.write(90 + s14Offset + Offset + amplitude * cos(-7 * lag)); servo_pan.write(90); Serial.println("Initializing"); Wire.beginTransmission(ADDRESS); Wire.write(byte(SHIFT_ADDR)); Wire.endTransmission(); Wire.requestFrom(ADDRESS, 1); if (1 <= Wire.available()) { shift = Wire.read(); } Serial.print("Read shift bit: "); Serial.println(shift, HEX); delay(startPause); // Pause to position robot*/ BLEDevice::init(DEVICE_NAME); BLEScan* pBLEScan = BLEDevice::getScan(); Serial.println("getScan"); pBLEScan->setAdvertisedDeviceCallbacks(new MyAdvertisedDeviceCallbacks()); Serial.println("setAdvertisedDeviceCallbacks"); pBLEScan->setActiveScan(true); pBLEScan->start(10); Serial.println(""); Serial.println("End of setup"); } void Forward() { for (counter = 0; counter < 360; counter += 1) { delay(delayTime); servo1.write(90 + s1Offset + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 + 6 * lag)); servo2.write(90 + s2Offset + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 + 5 * lag)); servo3.write(90 + s3Offset + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 + 4 * lag)); servo4.write(90 + s4Offset + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 + 3 * lag)); servo5.write(90 + s5Offset + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 + 2 * lag)); servo6.write(90 + s6Offset + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 + 1 * lag)); servo7.write(90 + s7Offset + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 + 0 * lag)); servo8.write(90 + s8Offset + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 - 1 * lag)); servo9.write(90 + s9Offset + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 - 2 * lag)); servo10.write(90 + s10Offset + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 - 3 * lag)); servo11.write(90 + s11Offset + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 - 4 * lag)); servo12.write(90 + s12Offset + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 - 5 * lag)); servo13.write(90 + s13Offset + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 - 6 * lag)); servo14.write(90 + s14Offset + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 - 7 * lag)); } } void Backward() { for (counter = 360; counter > 0; counter -= 1) { delay(delayTime); servo1.write(90 + s1Offset + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 + 6 * lag)); servo2.write(90 + s2Offset + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 + 5 * lag)); servo3.write(90 + s3Offset + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 + 4 * lag)); servo4.write(90 + s4Offset + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 + 3 * lag)); servo5.write(90 + s5Offset + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 + 2 * lag)); servo6.write(90 + s6Offset + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 + 1 * lag)); servo7.write(90 + s7Offset + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 + 0 * lag)); servo8.write(90 + s8Offset + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 - 1 * lag)); servo9.write(90 + s9Offset + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 - 2 * lag)); servo10.write(90 + s10Offset + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 - 3 * lag)); servo11.write(90 + s11Offset + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 - 4 * lag)); servo12.write(90 + s12Offset + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 - 5 * lag)); servo13.write(90 + s13Offset + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 - 6 * lag)); servo14.write(90 + s14Offset + Offset + amplitude * cos(frequency * counter * 3.14159 / 180 - 7 * lag)); } } void Turnleft() { for (counter = 0; counter < 12; counter += 1) { delay(delayTime); servo1.write(90 + s1Offset + Offset + .1 * counter * leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 6 * lag)); servo2.write(90 + s2Offset + Offset + .1 * counter * leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 5 * lag)); servo3.write(90 + s3Offset + Offset + .1 * counter * leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 4 * lag)); servo4.write(90 + s4Offset + Offset + .1 * counter * leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 3 * lag)); servo5.write(90 + s5Offset + Offset + .1 * counter * leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 2 * lag)); servo6.write(90 + s6Offset + Offset + .1 * counter * leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 1 * lag)); servo7.write(90 + s7Offset + Offset + .1 * counter * leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 0 * lag)); servo8.write(90 + s8Offset + Offset + .1 * counter * leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 1 * lag)); servo9.write(90 + s9Offset + Offset + .1 * counter * leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 2 * lag)); servo10.write(90 + s10Offset + Offset + .1 * counter * leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 3 * lag)); servo11.write(90 + s11Offset + Offset + .1 * counter * leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 4 * lag)); servo12.write(90 + s12Offset + Offset + .1 * counter * leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 5 * lag)); servo13.write(90 + s13Offset + Offset + .1 * counter * leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 6 * lag)); servo14.write(90 + s14Offset + Offset + .1 * counter * leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 7 * lag)); } // Continue left turn for (counter = 13; counter < 350; counter += 1) { delay(delayTime); servo1.write(90 + s1Offset + Offset + leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 6 * lag)); servo2.write(90 + s2Offset + Offset + leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 5 * lag)); servo3.write(90 + s3Offset + Offset + leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 4 * lag)); servo4.write(90 + s4Offset + Offset + leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 3 * lag)); servo5.write(90 + s5Offset + Offset + leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 2 * lag)); servo6.write(90 + s6Offset + Offset + leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 1 * lag)); servo7.write(90 + s7Offset + Offset + leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 0 * lag)); servo8.write(90 + s8Offset + Offset + leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 1 * lag)); servo9.write(90 + s9Offset + Offset + leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 2 * lag)); servo10.write(90 + s10Offset + Offset + leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 3 * lag)); servo11.write(90 + s11Offset + Offset + leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 4 * lag)); servo12.write(90 + s12Offset + Offset + leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 5 * lag)); servo13.write(90 + s13Offset + Offset + leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 6 * lag)); servo14.write(90 + s14Offset + Offset + leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 7 * lag)); } // Ramp down turn Offset for (counter = 350; counter < 360; counter += 1) { delay(delayTime); servo1.write(90 + s1Offset + Offset + .1 * (360 - counter)*leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 6 * lag)); servo2.write(90 + s2Offset + Offset + .1 * (360 - counter)*leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 5 * lag)); servo3.write(90 + s3Offset + Offset + .1 * (360 - counter)*leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 4 * lag)); servo4.write(90 + s4Offset + Offset + .1 * (360 - counter)*leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 3 * lag)); servo5.write(90 + s5Offset + Offset + .1 * (360 - counter)*leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 2 * lag)); servo6.write(90 + s6Offset + Offset + .1 * (360 - counter)*leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 1 * lag)); servo7.write(90 + s7Offset + Offset + .1 * (360 - counter)*leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 0 * lag)); servo8.write(90 + s8Offset + Offset + .1 * (360 - counter)*leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 1 * lag)); servo9.write(90 + s9Offset + Offset + .1 * (360 - counter)*leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 2 * lag)); servo10.write(90 + s10Offset + Offset + .1 * (360 - counter)*leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 3 * lag)); servo11.write(90 + s11Offset + Offset + .1 * (360 - counter)*leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 4 * lag)); servo12.write(90 + s12Offset + Offset + .1 * (360 - counter)*leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 5 * lag)); servo13.write(90 + s13Offset + Offset + .1 * (360 - counter)*leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 6 * lag)); servo14.write(90 + s14Offset + Offset + .1 * (360 - counter)*leftOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 7 * lag)); } } void Turnright() { for (counter = 0; counter < 12; counter += 1) { delay(delayTime); servo1.write(90 + s1Offset + Offset + .1 * counter * rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 6 * lag)); servo2.write(90 + s2Offset + Offset + .1 * counter * rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 5 * lag)); servo3.write(90 + s3Offset + Offset + .1 * counter * rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 4 * lag)); servo4.write(90 + s4Offset + Offset + .1 * counter * rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 3 * lag)); servo5.write(90 + s5Offset + Offset + .1 * counter * rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 2 * lag)); servo6.write(90 + s6Offset + Offset + .1 * counter * rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 1 * lag)); servo7.write(90 + s7Offset + Offset + .1 * counter * rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 0 * lag)); servo8.write(90 + s8Offset + Offset + .1 * counter * rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 1 * lag)); servo9.write(90 + s9Offset + Offset + .1 * counter * rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 2 * lag)); servo10.write(90 + s10Offset + Offset + .1 * counter * rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 3 * lag)); servo11.write(90 + s11Offset + Offset + .1 * counter * rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 4 * lag)); servo12.write(90 + s12Offset + Offset + .1 * counter * rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 5 * lag)); servo13.write(90 + s13Offset + Offset + .1 * counter * rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 6 * lag)); servo14.write(90 + s14Offset + Offset + .1 * counter * rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 7 * lag)); } // Continue right turn for (counter = 13; counter < 350; counter += 1) { delay(delayTime); servo1.write(90 + s1Offset + Offset + rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 6 * lag)); servo2.write(90 + s2Offset + Offset + rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 5 * lag)); servo3.write(90 + s3Offset + Offset + rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 4 * lag)); servo4.write(90 + s4Offset + Offset + rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 3 * lag)); servo5.write(90 + s5Offset + Offset + rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 2 * lag)); servo6.write(90 + s6Offset + Offset + rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 1 * lag)); servo7.write(90 + s7Offset + Offset + rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 0 * lag)); servo8.write(90 + s8Offset + Offset + rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 1 * lag)); servo9.write(90 + s9Offset + Offset + rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 2 * lag)); servo10.write(90 + s10Offset + Offset + rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 3 * lag)); servo11.write(90 + s11Offset + Offset + rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 4 * lag)); servo12.write(90 + s12Offset + Offset + rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 5 * lag)); servo13.write(90 + s13Offset + Offset + rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 6 * lag)); servo14.write(90 + s14Offset + Offset + rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 7 * lag)); } // Ramp down turn Offset for (counter = 350; counter < 360; counter += 1) { delay(delayTime); servo1.write(90 + s1Offset + Offset + .1 * (360 - counter)*rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 6 * lag)); servo2.write(90 + s2Offset + Offset + .1 * (360 - counter)*rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 5 * lag)); servo3.write(90 + s3Offset + Offset + .1 * (360 - counter)*rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 4 * lag)); servo4.write(90 + s4Offset + Offset + .1 * (360 - counter)*rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 3 * lag)); servo5.write(90 + s5Offset + Offset + .1 * (360 - counter)*rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 2 * lag)); servo6.write(90 + s6Offset + Offset + .1 * (360 - counter)*rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 1 * lag)); servo7.write(90 + s7Offset + Offset + .1 * (360 - counter)*rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 + 0 * lag)); servo8.write(90 + s8Offset + Offset + .1 * (360 - counter)*rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 1 * lag)); servo9.write(90 + s9Offset + Offset + .1 * (360 - counter)*rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 2 * lag)); servo10.write(90 + s10Offset + Offset + .1 * (360 - counter)*rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 3 * lag)); servo11.write(90 + s11Offset + Offset + .1 * (360 - counter)*rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 4 * lag)); servo12.write(90 + s12Offset + Offset + .1 * (360 - counter)*rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 5 * lag)); servo13.write(90 + s13Offset + Offset + .1 * (360 - counter)*rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 6 * lag)); servo14.write(90 + s14Offset + Offset + .1 * (360 - counter)*rightOffset + amplitude * cos(frequency * counter * 3.14159 / 180 - 7 * lag)); } } void Calibrate() { // servo motor 90degree test. servo1.write(90 + s1Offset + Offset); servo2.write(90 + s2Offset + Offset); servo3.write(90 + s3Offset + Offset); servo4.write(90 + s4Offset + Offset); servo5.write(90 + s5Offset + Offset); servo6.write(90 + s6Offset + Offset); servo7.write(90 + s7Offset + Offset); servo8.write(90 + s8Offset + Offset); servo9.write(90 + s9Offset + Offset); servo10.write(90 + s10Offset + Offset); servo11.write(90 + s11Offset + Offset); servo12.write(90 + s12Offset + Offset); servo13.write(90 + s13Offset + Offset); servo14.write(90 + s14Offset + Offset); } void Autonomous_Mode() { // Autonomous mode servo_pan.write(170); Left_distance = 0; delay(500); // Read basic measurement Wire.beginTransmission(ADDRESS); Wire.write(byte(DISTANCE_ADDR)); Wire.endTransmission(); Wire.requestFrom(ADDRESS, 2); if (2 <= Wire.available()) { distance[0] = Wire.read(); distance[1] = Wire.read(); // Print distance in cm Left_distance = (distance[0] * 16 + distance[1]) / 16 / (int)pow(2, shift); sprintf(buf_L, "Left_distance %u cm", Left_distance); Serial.println(buf_L); } else { Serial.println("Read error"); } delay(500); servo_pan.write(10); Right_distance = 0; delay(500); // Read basic measurement Wire.beginTransmission(ADDRESS); Wire.write(byte(DISTANCE_ADDR)); Wire.endTransmission(); Wire.requestFrom(ADDRESS, 2); if (2 <= Wire.available()) { distance[0] = Wire.read(); distance[1] = Wire.read(); // Print distance in cm Right_distance = (distance[0] * 16 + distance[1]) / 16 / (int)pow(2, shift); sprintf(buf_R, "Right_distance %u cm", Right_distance); Serial.println(buf_R); } else { Serial.println("Read error"); } delay(500); servo_pan.write(90); if (Left_distance > Right_distance ) { Backward(); Turnleft(); Turnleft(); Serial.println("Turnleft"); } else if (Left_distance <= Right_distance ) { Backward(); Turnright(); Turnright(); Serial.println("Turnright"); } else { Forward(); Serial.println("Forward"); } delay(100) ; } void Received_Code() { // Demodulates the received BLE remote control code. if (doConnect == true) { if (connectToServer(*pServerAddress)) { connected = true; } else { connected = false; } doConnect = false; } if (connected) { std::string value = pRemoteCharacteristic->readValue(); String strVal = value.c_str(); char strNum = strVal.toInt(); Serial.println(strNum); switch (strNum) { case 'F': // Forward Forward(); break; case 'B': // Backward Backward(); break; case 'L': // Turnleft Turnleft(); break; case 'R': // Turnright Turnright(); break; case '1': // Mode1:Autonomous Mode Mode = 1; digitalWrite(ModeLED, LOW); break; case '2': // Mode0:Remote Control Mode Mode = 0; digitalWrite(ModeLED, HIGH); break; default: break; } } else { Serial.println("Not connected"); doConnect = true; } delay(10); } void loop() { Received_Code(); State = digitalRead(CAL_SW); if (State == LOW) { Serial.println(State); Calibrate(); } if (Mode == 1) { // Autonomous Mode Forward(); servo_pan.write(90); delay(500); // Read basic measurement Wire.beginTransmission(ADDRESS); Wire.write(byte(DISTANCE_ADDR)); Wire.endTransmission(); Wire.requestFrom(ADDRESS, 2); if (2 <= Wire.available()) { distance[0] = Wire.read(); distance[1] = Wire.read(); // Print distance in cm Front_distance = (distance[0] * 16 + distance[1]) / 16 / (int)pow(2, shift); sprintf(buf_F, "Front_distance %u cm", Front_distance); Serial.println(buf_F); } else { Serial.println("Read error"); } delay(500); if (Front_distance < wallDistanceTolerance) { Autonomous_Mode(); } } }