#include <ESP32Servo.h>

Servo myservo;  // create servo object to control a servo

int pos = 0;    // variable to store the servo position

int servoPin = 10;
const int buttonPin = 21;

int buttonState;            // the current reading from the input pin
int lastButtonState = LOW;  // the previous reading from the input pin

// the following variables are unsigned longs because the time, measured in
// milliseconds, will quickly become a bigger number than can be stored in an int.
unsigned long lastDebounceTime = 0;  // the last time the output pin was toggled
unsigned long debounceDelay = 50;    // the debounce time; increase if the output flickers


void setup() {
  Serial.begin(9600);
  while (!Serial) {
    ; // wait for serial port to connect. Needed for native USB port only
  }
  pinMode(buttonPin, INPUT_PULLUP);

  myservo.attach(servoPin, 400, 2500);
  myservo.write(140);
}

void loop() {
  int reading = digitalRead(buttonPin);

  // If the switch changed, due to noise or pressing:
  if (reading != lastButtonState) {
    // reset the debouncing timer
    lastDebounceTime = millis();
  }
  if ((millis() - lastDebounceTime) > debounceDelay) {

    // if the button state has changed:
    if (reading != buttonState) {
      buttonState = reading;

      if (buttonState == LOW) {
        Serial.println("buttonpressed");


        for (pos = 140; pos >= 0; pos -= 1) { // goes from 180 degrees to 0 degrees
          myservo.write(pos);    // tell servo to go to position in variable 'pos'
          delay(15);             // waits 15ms for the servo to reach the position
        }

        for (pos = 0; pos <= 140; pos += 1) { // goes from 0 degrees to 180 degrees
          // in steps of 1 degree
          myservo.write(pos);    // tell servo to go to position in variable 'pos'
          delay(3);             // waits 15ms for the servo to reach the position
        }
      }

    }
  }
  lastButtonState = reading;

}

// Include the AccelStepper Library
#include <AccelStepper.h>

// Define pin connections
const int dirPin1 = 27;
const int stepPin1 = 26;
const int sleepResetPin1 = 25;

const int dirPin2 = 17;
const int stepPin2 = 2;
const int sleepResetPin2 = 15;


const int dirPin3 = 13;
const int stepPin3 = 12;
const int sleepResetPin3 = 14;

// Define motor interface type
#define motorInterfaceType 1

// Creates an instance
AccelStepper myStepper1(motorInterfaceType, stepPin1, dirPin1);
AccelStepper myStepper2(motorInterfaceType, stepPin2, dirPin2);
AccelStepper myStepper3(motorInterfaceType, stepPin3, dirPin3);


void setup() {
  // set the maximum speed, acceleration factor,
  // initial speed and the target position
  myStepper1.setMaxSpeed(1000);
  myStepper1.setAcceleration(50);
  myStepper1.setSpeed(200);
  myStepper1.moveTo(200);

  myStepper2.setMaxSpeed(1000);
  myStepper2.setAcceleration(50);
  myStepper2.setSpeed(200);
  myStepper2.moveTo(200);

  myStepper3.setMaxSpeed(1000);
  myStepper3.setAcceleration(50);
  myStepper3.setSpeed(200);
  myStepper3.moveTo(200);


  pinMode(sleepResetPin1, HIGH);
  pinMode(sleepResetPin2, HIGH);
  pinMode(sleepResetPin2, HIGH);

  delay(1);
}

void loop() {
  // Change direction once the motor reaches target position
  if (myStepper1.distanceToGo() == 0)
    myStepper1.moveTo(-myStepper1.currentPosition());

  // Move the motor one step
  myStepper1.run();

  if (myStepper2.distanceToGo() == 0)
    myStepper2.moveTo(-myStepper2.currentPosition());

  // Move the motor one step
  myStepper2.run();

    if (myStepper3.distanceToGo() == 0)
    myStepper3.moveTo(-myStepper3.currentPosition());

  // Move the motor one step
  myStepper3.run();
}

moveTo((long) absolute_position)
  Argument: absolute_position in steps. The desired absolute position of type long. May be positive or negative.
Set the target position. The run() function will try to move the motor (at most one step per call) from the current position to the target position set by the most recent call to this function. Caution: moveTo() also recalculates the speed for the next step.  If you are trying to use constant speed movements, you should call setSpeed() after calling moveTo(). If moveTo() is called while the motor is moving,the target position is changed immediately and the acceleration algorithm is used to calculate the new speed. If the motor is running at a high speed in a particular direction and the new target position is in the opposite direction, the motor will continue to run in the same direction, decelerate to stop, then accelerate in the new direction until it approaches the new target and slows to a stop.

  myStepper1.setMaxSpeed(1000);
  //  myStepper1.setAcceleration(50);
  myStepper1.setSpeed(1000);
  myStepper1.moveTo(0);

  myStepper2.setMaxSpeed(1000);
  //  myStepper2.setAcceleration(50);
  myStepper2.setSpeed(1000);
  myStepper2.moveTo(0);


      myStepper1.setSpeed(1000);
      myStepper1.moveTo(sensorValue1);
      myStepper1.runSpeedToPosition();

      myStepper2.setSpeed(1000);
      myStepper2.moveTo(sensorValue2);
      myStepper2.runSpeedToPosition();

For example using Full steps you might be able to reach close to 4 rotation a second at max speed, well if using 1/4 steps that maximum speed might be reduced to only 1 rotation a second.
This has nothing to do with the Stepper Motor itself, it’s a combination of the stepper driver and the micro controller maximum speed at which it can turn on and off the motor coils.
One other compromise might be about the maximum torque that can be achieve.
Although this is debatable, using Full steps normally will result in higher torque compared to micro stepping.
So in conclusion, if you’re project requires maximum speed, maybe using Full steps is the way to go.
On the other hand if you need more precision, then using micro stepping is a better choice.
Keep in mind that using a faster micro controller and better stepper driver will affect these choices.

  myStepper1.setMaxSpeed(4000);
  myStepper1.setAcceleration(4000);
  myStepper1.setSpeed(4000);
  myStepper1.moveTo(2400);

    sensorValue1 = map(sensorValue1, sensorMin1, sensorMax1, 0, 600);

/**
   A BLE client example that is rich in capabilities.
   There is a lot new capabilities implemented.
   author unknown
   updated by chegewara
   Modified by Michelle Vossen
*/

#include "BLEDevice.h"
//#include "BLEScan.h"
#include <AccelStepper.h>
#include <UniversalTimer.h>

UniversalTimer timer(500, true);

// Define pin connections for steppers

const int dirPin1 = 27;
const int stepPin1 = 26;
const int sleepResetPin1 = 25;

const int dirPin2 = 17;
const int stepPin2 = 2;
const int sleepResetPin2 = 15;

//const int dirPin3 = 13;
//const int stepPin3 = 12;
//const int sleepResetPin3 = 14;

// Define motor interface type
#define motorInterfaceType 1

// Creates an instance
AccelStepper myStepper1(motorInterfaceType, stepPin1, dirPin1);
AccelStepper myStepper2(motorInterfaceType, stepPin2, dirPin2);
//AccelStepper myStepper3(motorInterfaceType, stepPin3, dirPin3);

// To convert the incoming string into ints
int i;
char delimiter[] = ",";
char *p;
char string[128];
String sensorValuesString[2];
int sensorValue1;
int sensorValue2;
String valueString;

BLERemoteService* pRemoteService;
BLEClient*  pClient;

// The remote service we wish to connect to.
static BLEUUID serviceUUID("4fafc201-1fb5-459e-8fcc-c5c9c331914b");
// The characteristic of the remote service we are interested in.
static BLEUUID    charUUID("beb5483e-36e1-4688-b7f5-ea07361b26a8");

static boolean doConnect = false;
static boolean connected = false;
static boolean doScan = false;
static BLERemoteCharacteristic* pRemoteCharacteristic;
static BLEAdvertisedDevice* myDevice;

static void notifyCallback(
  BLERemoteCharacteristic* pBLERemoteCharacteristic,
  uint8_t* pData,
  size_t length,
  bool isNotify) {
  Serial.print("Notify callback for characteristic ");
  Serial.print(pBLERemoteCharacteristic->getUUID().toString().c_str());
  Serial.print(" of data length ");
  Serial.println(length);
  Serial.print("data: ");
  Serial.write(pData, length);
  Serial.println();
}

class MyClientCallback : public BLEClientCallbacks {
    void onConnect(BLEClient* pclient) {
    }

    void onDisconnect(BLEClient* pclient) {
      connected = false;
      Serial.println("onDisconnect");
    }
};

bool connectToServer() {
  Serial.print("Forming a connection to ");
  Serial.println(myDevice->getAddress().toString().c_str());

  pClient  = BLEDevice::createClient();
  Serial.println(" - Created client");

  pClient->setClientCallbacks(new MyClientCallback());

  // Connect to the remote BLE Server.
  pClient->connect(myDevice);  // if you pass BLEAdvertisedDevice instead of address, it will be recognized type of peer device address (public or private)
  Serial.println(" - Connected to server");
  pClient->setMTU(517); //set client to request maximum MTU from server (default is 23 otherwise)

  // Obtain a reference to the service we are after in the remote BLE server.
  pRemoteService = pClient->getService(serviceUUID);
  if (pRemoteService == nullptr) {
    Serial.print("Failed to find our service UUID: ");
    Serial.println(serviceUUID.toString().c_str());
    pClient->disconnect();
    return false;
  }
  Serial.println(" - Found our service");


  // Obtain a reference to the characteristic in the service of the remote BLE server.
  pRemoteCharacteristic = pRemoteService->getCharacteristic(charUUID);
  if (pRemoteCharacteristic == nullptr) {
    Serial.print("Failed to find our characteristic UUID: ");
    Serial.println(charUUID.toString().c_str());
    pClient->disconnect();
    return false;
  }
  Serial.println(" - Found our characteristic");

  // Read the value of the characteristic.
  if (pRemoteCharacteristic->canRead()) {
    std::string value = pRemoteCharacteristic->readValue();
    Serial.print("The characteristic value was: ");
    Serial.println(value.c_str());
  }

  if (pRemoteCharacteristic->canNotify())
    pRemoteCharacteristic->registerForNotify(notifyCallback);

  connected = true;
  return true;
}
/**
   Scan for BLE servers and find the first one that advertises the service we are looking for.
*/
class MyAdvertisedDeviceCallbacks: public BLEAdvertisedDeviceCallbacks {
    /**
        Called for each advertising BLE server.
    */
    void onResult(BLEAdvertisedDevice advertisedDevice) {
      Serial.print("BLE Advertised Device found: ");
      Serial.println(advertisedDevice.toString().c_str());

      // We have found a device, let us now see if it contains the service we are looking for.
      if (advertisedDevice.haveServiceUUID() && advertisedDevice.isAdvertisingService(serviceUUID)) {

        BLEDevice::getScan()->stop();
        myDevice = new BLEAdvertisedDevice(advertisedDevice);
        doConnect = true;
        doScan = true;

      } // Found our server
    } // onResult
}; // MyAdvertisedDeviceCallbacks


void setup() {
  Serial.begin(115200);
  while (!Serial) {
    ; // wait for serial port to connect. Needed for native USB port only
  }
  Serial.println("Starting Arduino BLE Client application...");
  BLEDevice::init("");

  // Retrieve a Scanner and set the callback we want to use to be informed when we
  // have detected a new device.  Specify that we want active scanning and start the
  // scan to run for 5 seconds.
  BLEScan* pBLEScan = BLEDevice::getScan();
  pBLEScan->setAdvertisedDeviceCallbacks(new MyAdvertisedDeviceCallbacks());
  pBLEScan->setInterval(1349);
  pBLEScan->setWindow(449);
  pBLEScan->setActiveScan(true);
  pBLEScan->start(5, false);

  myStepper1.setMaxSpeed(4000);
  myStepper1.setAcceleration(4000);
  myStepper1.setSpeed(4000);
  myStepper1.moveTo(0);

  myStepper2.setMaxSpeed(4000);
  myStepper2.setAcceleration(4000);
  myStepper2.setSpeed(4000);
  myStepper2.moveTo(0);

  timer.start();
} // End of setup.


// This is the Arduino main loop function.
void loop() {
  //      Serial.println(sensorValue1);
  //      Serial.println(sensorValue2);

  if (myStepper1.distanceToGo() == 0)
    myStepper1.moveTo(sensorValue1);

  // Move the motor one step
  myStepper1.run();

  if (myStepper2.distanceToGo() == 0)
    myStepper2.moveTo(-sensorValue2);

  // Move the motor one step
  myStepper2.run();

  if (timer.check()) {

    // If the flag "doConnect" is true then we have scanned for and found the desired
    //z BLE Server with which we wish to connect.  Now we connect to it.  Once we are
    // connected we set the connected flag to be true.
    if (doConnect == true) {
      if (connectToServer()) {
        Serial.println("We are now connected to the XIAO ESP BLE Server.");
      } else {
        Serial.println("We have failed to connect to the server; there is nothing more we will do.");
      }
      doConnect = false;
    }

    if (connected) {
      // Read the value of the characteristic.
      if (pRemoteCharacteristic->canRead()) {
        std::string value = pRemoteCharacteristic->readValue();
        //      Serial.print("The characteristic value was: ");
        //      Serial.println(value.c_str());

        // Convert received data to two separate integers
        char *cstr = &value[0];
        i = 0;
        p = strtok(cstr, delimiter);

        while (p && i < 2)
        {
          sensorValuesString[i] = p;
          p = strtok(NULL, delimiter);
          ++i;
        }

        sensorValue1 = sensorValuesString[0].toInt();
        sensorValue2 = sensorValuesString[1].toInt();
        return;
      }
    } else if (doScan) {
      BLEDevice::getScan()->start(0);
      return;
    }
  }
}

/**
   A BLE client example that is rich in capabilities.
   There is a lot new capabilities implemented.
   author unknown
   updated by chegewara
   Modified by Michelle Vossen
*/

#include "BLEDevice.h"
//#include "BLEScan.h"


BLERemoteService* pRemoteService;
BLEClient*  pClient;

// The remote service we wish to connect to.
static BLEUUID serviceUUID("4fafc201-1fb5-459e-8fcc-c5c9c331914b");
// The characteristic of the remote service we are interested in.
static BLEUUID    charUUID("beb5483e-36e1-4688-b7f5-ea07361b26a8");

static boolean doConnect = false;
static boolean connected = false;
static boolean doScan = false;
static BLERemoteCharacteristic* pRemoteCharacteristic;
static BLEAdvertisedDevice* myDevice;

static void notifyCallback(
  BLERemoteCharacteristic* pBLERemoteCharacteristic,
  uint8_t* pData,
  size_t length,
  bool isNotify) {
  Serial.print("Notify callback for characteristic ");
  Serial.print(pBLERemoteCharacteristic->getUUID().toString().c_str());
  Serial.print(" of data length ");
  Serial.println(length);
  Serial.print("data: ");
  Serial.write(pData, length);
  Serial.println();
}

class MyClientCallback : public BLEClientCallbacks {
    void onConnect(BLEClient* pclient) {
    }

    void onDisconnect(BLEClient* pclient) {
      connected = false;
      Serial.println("onDisconnect");
    }
};

bool connectToServer() {
  Serial.print("Forming a connection to ");
  Serial.println(myDevice->getAddress().toString().c_str());

  pClient  = BLEDevice::createClient();
  Serial.println(" - Created client");

  pClient->setClientCallbacks(new MyClientCallback());

  // Connect to the remove BLE Server.
  pClient->connect(myDevice);  // if you pass BLEAdvertisedDevice instead of address, it will be recognized type of peer device address (public or private)
  Serial.println(" - Connected to server");
  pClient->setMTU(517); //set client to request maximum MTU from server (default is 23 otherwise)

  // Obtain a reference to the service we are after in the remote BLE server.
  pRemoteService = pClient->getService(serviceUUID);
  if (pRemoteService == nullptr) {
    Serial.print("Failed to find our service UUID: ");
    Serial.println(serviceUUID.toString().c_str());
    pClient->disconnect();
    return false;
  }
  Serial.println(" - Found our service");


  // Obtain a reference to the characteristic in the service of the remote BLE server.
  pRemoteCharacteristic = pRemoteService->getCharacteristic(charUUID);
  if (pRemoteCharacteristic == nullptr) {
    Serial.print("Failed to find our characteristic UUID: ");
    Serial.println(charUUID.toString().c_str());
    pClient->disconnect();
    return false;
  }
  Serial.println(" - Found our characteristic");

  // Read the value of the characteristic.
  if (pRemoteCharacteristic->canRead()) {
    std::string value = pRemoteCharacteristic->readValue();
    Serial.print("The characteristic value was: ");
    Serial.println(value.c_str());
  }

  if (pRemoteCharacteristic->canNotify())
    pRemoteCharacteristic->registerForNotify(notifyCallback);

  connected = true;
  return true;
}
/**
   Scan for BLE servers and find the first one that advertises the service we are looking for.
*/
class MyAdvertisedDeviceCallbacks: public BLEAdvertisedDeviceCallbacks {
    /**
        Called for each advertising BLE server.
    */
    void onResult(BLEAdvertisedDevice advertisedDevice) {
      Serial.print("BLE Advertised Device found: ");
      Serial.println(advertisedDevice.toString().c_str());

      // We have found a device, let us now see if it contains the service we are looking for.
      if (advertisedDevice.haveServiceUUID() && advertisedDevice.isAdvertisingService(serviceUUID)) {

        BLEDevice::getScan()->stop();
        myDevice = new BLEAdvertisedDevice(advertisedDevice);
        doConnect = true;
        doScan = true;

      } // Found our server
    } // onResult
}; // MyAdvertisedDeviceCallbacks


void setup() {
  Serial.begin(115200);
  while (!Serial) {
    ; // wait for serial port to connect. Needed for native USB port only
  }

  Serial.println("Starting Arduino BLE Client application...");
  BLEDevice::init("");

  // Retrieve a Scanner and set the callback we want to use to be informed when we
  // have detected a new device.  Specify that we want active scanning and start the
  // scan to run for 5 seconds.
  BLEScan* pBLEScan = BLEDevice::getScan();
  pBLEScan->setAdvertisedDeviceCallbacks(new MyAdvertisedDeviceCallbacks());
  pBLEScan->setInterval(1349);
  pBLEScan->setWindow(449);
  pBLEScan->setActiveScan(true);
  pBLEScan->start(5, false);
} // End of setup.


// This is the Arduino main loop function.
void loop() {

  // If the flag "doConnect" is true then we have scanned for and found the desired
  // BLE Server with which we wish to connect.  Now we connect to it.  Once we are
  // connected we set the connected flag to be true.
  if (doConnect == true) {
    if (connectToServer()) {
      Serial.println("We are now connected to the XIAO ESP BLE Server.");
    } else {
      Serial.println("We have failed to connect to the server; there is nothing more we will do.");
    }
    doConnect = false;
  }

  // If we are connected to a peer BLE Server, update the characteristic each time we are reached
  // with the current time since boot.
  if (connected) {
    // Read the value of the characteristic.
    if (pRemoteCharacteristic->canRead()) {
      std::string value = pRemoteCharacteristic->readValue();
      Serial.print("The characteristic value was: ");
      Serial.println(value.c_str());
    }
//    pRemoteCharacteristic->writeValue("Hello");
  } else if (doScan) {
    BLEDevice::getScan()->start(0);
  }

  delay(10); // Delay a second between loops.
} // End of loop

#include <BLEDevice.h>
#include <BLEUtils.h>
#include <BLEServer.h>

BLEServer *pServer;
BLEService *pService;
BLECharacteristic *pCharacteristic;

#define SERVICE_UUID        "4fafc201-1fb5-459e-8fcc-c5c9c331914b"
#define CHARACTERISTIC_UUID "beb5483e-36e1-4688-b7f5-ea07361b26a8"


const int sensorPin1 = 2;
const int sensorPin2 = 3;

const int ledPin1 = 4;
const int ledPin2 = 5;

const int buttonPin = 21;

int sensorValue1 = 0;   // the sensor value
int sensorMin1 = 4095;  // minimum sensor value
int sensorMax1 = 0;     // maximum sensor value

int sensorValue2 = 0;   // the sensor value
int sensorMin2 = 4095;  // minimum sensor value
int sensorMax2 = 0;     // maximum sensor value

int ledState1 = LOW;        // the current state of the output pin
int ledState2 = LOW;        // the current state of the output pin
int buttonState;            // the current reading from the input pin
int lastButtonState = LOW;  // the previous reading from the input pin

// the following variables are unsigned longs because the time, measured in
// milliseconds, will quickly become a bigger number than can be stored in an int.
unsigned long lastDebounceTime = 0;  // the last time the output pin was toggled
unsigned long debounceDelay = 50;    // the debounce time; increase if the output flickers


void setup() {
  // turn on LED to signal the start of the calibration period:
  Serial.begin(115200);
  while (!Serial) {
    ; // wait for serial port to connect. Needed for native USB port only
  }
  pinMode(ledPin1, OUTPUT);
  pinMode(ledPin2, OUTPUT);
  pinMode(sensorPin1, INPUT);
  pinMode(sensorPin2, INPUT);
  pinMode(buttonPin, INPUT_PULLUP);

  digitalWrite(ledPin1, ledState1);
  digitalWrite(ledPin2, ledState2);

  Serial.println("Starting BLE work!");

  BLEDevice::init("XIAO input device board");

  // C++ code: in class BLEDevice we call createServer(); this method returns a pointer (=address)
  // of type BLEServer and we store that in a variable named pServer of type pointer to BLEServer
  // (BLEServer *).
  pServer = BLEDevice::createServer();

  pService = pServer->createService(SERVICE_UUID);
  pCharacteristic = pService->createCharacteristic(
                       CHARACTERISTIC_UUID,
                       BLECharacteristic::PROPERTY_READ |
                       BLECharacteristic::PROPERTY_WRITE
                     );

  pService->start();
  // BLEAdvertising *pAdvertising = pServer->getAdvertising();  // this still is working for backward compatibility
  BLEAdvertising *pAdvertising = BLEDevice::getAdvertising();
  pAdvertising->addServiceUUID(SERVICE_UUID);
  pAdvertising->setScanResponse(true);
  pAdvertising->setMinPreferred(0x06);  // functions that help with iPhone connections issue
  pAdvertising->setMinPreferred(0x12);
  BLEDevice::startAdvertising();
  Serial.println("Characteristic defined");
}

void loop() {
  int reading = digitalRead(buttonPin);

  // If the switch changed, due to noise or pressing:
  if (reading != lastButtonState) {
    // reset the debouncing timer
    lastDebounceTime = millis();
  }

  if ((millis() - lastDebounceTime) > debounceDelay) {

    // if the button state has changed:
    if (reading != buttonState) {
      buttonState = reading;

      // only toggle the LED if the new button state is HIGH
      if (buttonState == LOW) {
        Serial.println("start");
        digitalWrite(ledPin1, HIGH);
        digitalWrite(ledPin2, HIGH);

        // determine min and max values of both sensors for 5 seconds
        for (int timems = 0; timems < 5000; timems++) {
          sensorValue1 = analogRead(sensorPin1);
          sensorValue2 = analogRead(sensorPin2);

          // record the maximum sensor value
          if (sensorValue1 > sensorMax1) {
            sensorMax1 = sensorValue1;
          }
          // record the maximum sensor value
          if (sensorValue2 > sensorMax2) {
            sensorMax2 = sensorValue2;
          }

          // record the minimum sensor value
          if (sensorValue1 < sensorMin1) {
            sensorMin1 = sensorValue1;
          }
          // record the minimum sensor value
          if (sensorValue2 < sensorMin2) {
            sensorMin2 = sensorValue2;
          }
          delay(1);
        }
        // signal the end of the calibration period
        digitalWrite(ledPin1, LOW);
        digitalWrite(ledPin2, LOW);

        Serial.print("Results for sensor 1: ");

        Serial.print(sensorMin1);
        Serial.print("\t");
        Serial.println(sensorMax1);

        Serial.print("Results for sensor 2: ");

        Serial.print(sensorMin2);
        Serial.print("\t");
        Serial.println(sensorMax2);
      }
    }

    sensorValue1 = analogRead(sensorPin1);
    sensorValue2 = analogRead(sensorPin2);

    Serial.print(sensorValue1);
    Serial.print("\t");
    Serial.print(sensorValue2);
    Serial.print("\t");

    sensorValue1 = constrain(sensorValue1, sensorMin1, sensorMax1);
    sensorValue1 = map(sensorValue1, sensorMin1, sensorMax1, 0, 255);
    Serial.print(sensorValue1);
    Serial.print("\t");

    sensorValue2 = constrain(sensorValue2, sensorMin2, sensorMax2);
    sensorValue2 = map(sensorValue2, sensorMin2, sensorMax2, 0, 255);
    Serial.println(sensorValue2);

    // fade the LED using the calibrated value:
    analogWrite(ledPin1, sensorValue1);
    analogWrite(ledPin2, sensorValue2);

    String valueString1 = String(sensorValue1);
    String valueString2 = String(sensorValue2);
    String bothValues = String(valueString1 + "," + valueString2);
    pCharacteristic->setValue(bothValues.c_str()); // expects 0 terminated string, converts String to c_str which is a character array
    pCharacteristic->notify();
    delay(1);
  }

  // save the reading. Next time through the loop, it'll be the lastButtonState:
  lastButtonState = reading;
}

Guru Meditation Error: Core  1 panic'ed (LoadProhibited). Exception was unhandled.

Core  1 register dump:
PC      : 0x400d7be9  PS      : 0x00060830  A0      : 0x800d7e24  A1      : 0x3ffc9690  
A2      : 0x3ffc39c4  A3      : 0x011f3576  A4      : 0x00000001  A5      : 0x0000019a  
A6      : 0x00000000  A7      : 0xff000000  A8      : 0x00000001  A9      : 0x00000001  
A10     : 0x011f3576  A11     : 0x00000001  A12     : 0xfffffc02  A13     : 0x000003fe  
A14     : 0x7ff00000  A15     : 0x40bf4000  SAR     : 0x00000001  EXCCAUSE: 0x0000001c  
EXCVADDR: 0x0000000d  LBEG    : 0x4008443d  LEND    : 0x40084445  LCOUNT  : 0x00000027  


Backtrace: 0x400d7be6:0x3ffc9690 0x400d7e21:0x3ffc96b0 0x400d2543:0x3ffc96d0 0x400d9585:0x3ffc9730

/**
   A BLE client example that is rich in capabilities.
   There is a lot new capabilities implemented.
   author unknown
   updated by chegewara
   Modified by Michelle Vossen
*/

#include "BLEDevice.h"
//#include "BLEScan.h"
#include <AccelStepper.h>
#include <UniversalTimer.h>
#include <ESP32Servo.h>

Servo myservo;  // create servo object to control a servo

int pos = 140;    // variable to store the servo position

// Define servo motor pin
const int servoPin = 32;

UniversalTimer timer(500, true);

// Define stepper pins

const int dirPin1 = 27;
const int stepPin1 = 26;
const int sleepResetPin1 = 25;

const int dirPin2 = 17;
const int stepPin2 = 2;
const int sleepResetPin2 = 15;

//const int dirPin3 = 13;
//const int stepPin3 = 12;
//const int sleepResetPin3 = 14;

// Define motor interface type
#define motorInterfaceType 1

// Creates an instance
AccelStepper myStepper1(motorInterfaceType, stepPin1, dirPin1);
AccelStepper myStepper2(motorInterfaceType, stepPin2, dirPin2);
//AccelStepper myStepper3(motorInterfaceType, stepPin3, dirPin3);

// To convert the incoming string into ints
int i;
char delimiter[] = ",";
char *p;
char string[128];
String sensorValuesString[3];
int sensorValue1;
int sensorValue2;
String valueString;

BLERemoteService* pRemoteService;
BLEClient*  pClient;

// The remote service we wish to connect to.
static BLEUUID serviceUUID("d881bfae-0213-11ee-be56-0242ac120002");
// The characteristic of the remote service we are interested in.
static BLEUUID    charUUID("0b85454c-0214-11ee-be56-0242ac120002");

static boolean doConnect = false;
static boolean connected = false;
static boolean doScan = false;
static BLERemoteCharacteristic* pRemoteCharacteristic;
static BLEAdvertisedDevice* myDevice;

static void notifyCallback(
  BLERemoteCharacteristic* pBLERemoteCharacteristic,
  uint8_t* pData,
  size_t length,
  bool isNotify) {
  Serial.print("Notify callback for characteristic ");
  Serial.print(pBLERemoteCharacteristic->getUUID().toString().c_str());
  Serial.print(" of data length ");
  Serial.println(length);
  Serial.print("data: ");
  Serial.write(pData, length);
  Serial.println();
}

class MyClientCallback : public BLEClientCallbacks {
    void onConnect(BLEClient* pclient) {
    }

    void onDisconnect(BLEClient* pclient) {
      connected = false;
      Serial.println("onDisconnect");
    }
};

bool connectToServer() {
  Serial.print("Forming a connection to ");
  Serial.println(myDevice->getAddress().toString().c_str());

  pClient  = BLEDevice::createClient();
  Serial.println(" - Created client");

  pClient->setClientCallbacks(new MyClientCallback());

  // Connect to the remote BLE Server.
  pClient->connect(myDevice);  // if you pass BLEAdvertisedDevice instead of address, it will be recognized type of peer device address (public or private)
  Serial.println(" - Connected to server");
  pClient->setMTU(517); //set client to request maximum MTU from server (default is 23 otherwise)

  // Obtain a reference to the service we are after in the remote BLE server.
  pRemoteService = pClient->getService(serviceUUID);
  if (pRemoteService == nullptr) {
    Serial.print("Failed to find our service UUID: ");
    Serial.println(serviceUUID.toString().c_str());
    pClient->disconnect();
    return false;
  }
  Serial.println(" - Found our service");


  // Obtain a reference to the characteristic in the service of the remote BLE server.
  pRemoteCharacteristic = pRemoteService->getCharacteristic(charUUID);
  if (pRemoteCharacteristic == nullptr) {
    Serial.print("Failed to find our characteristic UUID: ");
    Serial.println(charUUID.toString().c_str());
    pClient->disconnect();
    return false;
  }
  Serial.println(" - Found our characteristic");

  // Read the value of the characteristic.
  if (pRemoteCharacteristic->canRead()) {
    std::string value = pRemoteCharacteristic->readValue();
    Serial.print("The characteristic value was: ");
    Serial.println(value.c_str());
  }

  if (pRemoteCharacteristic->canNotify())
    pRemoteCharacteristic->registerForNotify(notifyCallback);

  connected = true;
  return true;
}


/**
   Scan for BLE servers and find the first one that advertises the service we are looking for.
*/
class MyAdvertisedDeviceCallbacks: public BLEAdvertisedDeviceCallbacks {
    /**
        Called for each advertising BLE server.
    */
    void onResult(BLEAdvertisedDevice advertisedDevice) {
      Serial.print("BLE Advertised Device found: ");
      Serial.println(advertisedDevice.toString().c_str());

      // We have found a device, let us now see if it contains the service we are looking for.
      if (advertisedDevice.haveServiceUUID() && advertisedDevice.isAdvertisingService(serviceUUID)) {

        BLEDevice::getScan()->stop();
        myDevice = new BLEAdvertisedDevice(advertisedDevice);
        doConnect = true;
        doScan = true;

      } // Found our server
    } // onResult
}; // MyAdvertisedDeviceCallbacks

//--------------------------------------------------




void setup() {
  Serial.begin(115200);
  while (!Serial) {
    ; // wait for serial port to connect. Needed for native USB port only
  }
  Serial.println("Starting Arduino BLE Client application...");
  BLEDevice::init("");

  // Retrieve a Scanner and set the callback we want to use to be informed when we
  // have detected a new device.  Specify that we want active scanning and start the
  // scan to run for 5 seconds.
  BLEScan* pBLEScan = BLEDevice::getScan();
  pBLEScan->setAdvertisedDeviceCallbacks(new MyAdvertisedDeviceCallbacks());
  pBLEScan->setInterval(1349);
  pBLEScan->setWindow(449);
  pBLEScan->setActiveScan(true);
  pBLEScan->start(5, false);

  myStepper1.setMaxSpeed(4000);
  myStepper1.setAcceleration(4000);
  myStepper1.setSpeed(4000);
  myStepper1.moveTo(0);

  myStepper2.setMaxSpeed(4000);
  myStepper2.setAcceleration(4000);
  myStepper2.setSpeed(4000);
  myStepper2.moveTo(0);

  timer.start();

  myservo.attach(servoPin, 400, 2500);
  myservo.write(pos);

} // End of setup.





// This is the Arduino main loop function.
void loop() {
  //      Serial.println(sensorValue1);
  //      Serial.println(sensorValue2);

  if (myStepper1.distanceToGo() == 0)
    myStepper1.moveTo(sensorValue1);

  // Move the motor one step
  myStepper1.run();

  if (myStepper2.distanceToGo() == 0)
    myStepper2.moveTo(-sensorValue2);

  // Move the motor one step
  myStepper2.run();


  if (timer.check()) {

    // If the flag "doConnect" is true then we have scanned for and found the desired
    //z BLE Server with which we wish to connect.  Now we connect to it.  Once we are
    // connected we set the connected flag to be true.
    if (doConnect == true) {
      if (connectToServer()) {
        Serial.println("We are now connected to the XIAO ESP BLE Server.");
      } else {
        Serial.println("We have failed to connect to the server; there is nothing more we will do.");
      }
      doConnect = false;
    }

    if (connected) {
      // Read the value of the characteristic.
      if (pRemoteCharacteristic->canRead()) {
        std::string value = pRemoteCharacteristic->readValue();
        Serial.print("The characteristic value was: ");
        Serial.println(value.c_str());

        // Convert received data to 3 separate integers
        char *cstr = &value[0];
        i = 0;
        p = strtok(cstr, delimiter);

        while (p && i < 3)
        {
          sensorValuesString[i] = p;
          p = strtok(NULL, delimiter);
          ++i;
        }

        sensorValue1 = sensorValuesString[0].toInt();
        sensorValue2 = sensorValuesString[1].toInt();
        pos = sensorValuesString[2].toInt();
        //        myservo.write(pos);

        return;
      }
    } else if (doScan) {
      BLEDevice::getScan()->start(0); // restart scanning if the connection is lost
      return;
    }

  }
}

#include <BLEDevice.h>
#include <BLEUtils.h>
#include <BLEServer.h>

BLEServer *pServer;
BLEService *pService;
BLECharacteristic *pCharacteristic;

#define SERVICE_UUID        "d881bfae-0213-11ee-be56-0242ac120002"
#define CHARACTERISTIC_UUID "0b85454c-0214-11ee-be56-0242ac120002"


const int sensorPin1 = 2;
const int sensorPin2 = 3;

const int ledPin1 = 4;
const int ledPin2 = 5;

const int buttonPin = 21;
const int servoButtonPin = 9;

// Brush lifting with a button
int servoPos = 140;
int servoButtonPushCounter = 0;  // counter for the number of button presses
int servoButtonState = 0;        // current state of the button
int servoLastButtonState = 0;    // previous state of the button

int sensorValue1 = 0;   // the sensor value
int sensorMin1 = 4095;  // minimum sensor value
int sensorMax1 = 0;     // maximum sensor value

int sensorValue2 = 0;   // the sensor value
int sensorMin2 = 4095;  // minimum sensor value
int sensorMax2 = 0;     // maximum sensor value

int ledState1 = LOW;        // the current state of the output pin
int ledState2 = LOW;        // the current state of the output pin
int buttonState;            // the current reading from the input pin
int lastButtonState = LOW;  // the previous reading from the input pin

// the following variables are unsigned longs because the time, measured in
// milliseconds, will quickly become a bigger number than can be stored in an int.
unsigned long lastDebounceTime = 0;  // the last time the output pin was toggled
unsigned long debounceDelay = 50;    // the debounce time; increase if the output flickers


void setup() {
  Serial.begin(115200);
  while (!Serial) {
    ; // wait for serial port to connect. Needed for native USB port only
  }
  pinMode(ledPin1, OUTPUT);
  pinMode(ledPin2, OUTPUT);
  pinMode(sensorPin1, INPUT);
  pinMode(sensorPin2, INPUT);
  pinMode(buttonPin, INPUT_PULLUP);
  pinMode(servoButtonPin, INPUT_PULLUP);

  digitalWrite(ledPin1, ledState1);
  digitalWrite(ledPin2, ledState2);

  Serial.println("Starting BLE work!");

  BLEDevice::init("XIAO input device board");

  // C++ code: in class BLEDevice we call createServer(); this method returns a pointer (=address)
  // of type BLEServer and we store that in a variable named pServer of type pointer to BLEServer
  // (BLEServer *).
  pServer = BLEDevice::createServer();

  pService = pServer->createService(SERVICE_UUID);
  pCharacteristic = pService->createCharacteristic(
                      CHARACTERISTIC_UUID,
                      BLECharacteristic::PROPERTY_READ |
                      BLECharacteristic::PROPERTY_WRITE
                    );

  pService->start();
  // BLEAdvertising *pAdvertising = pServer->getAdvertising();  // this still is working for backward compatibility
  BLEAdvertising *pAdvertising = BLEDevice::getAdvertising();
  pAdvertising->addServiceUUID(SERVICE_UUID);
  pAdvertising->setScanResponse(true);
  pAdvertising->setMinPreferred(0x06);  // functions that help with iPhone connections issue
  pAdvertising->setMinPreferred(0x12);
  BLEDevice::startAdvertising();
  Serial.println("Characteristic defined");
}

void loop() {
  int reading = digitalRead(buttonPin);
  servoButtonState = digitalRead(servoButtonPin);
  Serial.println(servoButtonState);

  // If the switch changed, due to noise or pressing:
  if (reading != lastButtonState) {
    // reset the debouncing timer
    lastDebounceTime = millis();
  }

  if ((millis() - lastDebounceTime) > debounceDelay) {

    // if the button state has changed:
    if (reading != buttonState) {
      buttonState = reading;

      // only toggle the LED if the new button state is HIGH
      if (buttonState == LOW) {
        Serial.println("start");
        digitalWrite(ledPin1, HIGH);
        digitalWrite(ledPin2, HIGH);

        // determine min and max values of both sensors for 5 seconds
        for (int timems = 0; timems < 5000; timems++) {
          sensorValue1 = analogRead(sensorPin1);
          sensorValue2 = analogRead(sensorPin2);

          // record the maximum sensor value
          if (sensorValue1 > sensorMax1) {
            sensorMax1 = sensorValue1;
          }
          // record the maximum sensor value
          if (sensorValue2 > sensorMax2) {
            sensorMax2 = sensorValue2;
          }

          // record the minimum sensor value
          if (sensorValue1 < sensorMin1) {
            sensorMin1 = sensorValue1;
          }
          // record the minimum sensor value
          if (sensorValue2 < sensorMin2) {
            sensorMin2 = sensorValue2;
          }
          delay(1);
        }
        // signal the end of the calibration period
        digitalWrite(ledPin1, LOW);
        digitalWrite(ledPin2, LOW);

        Serial.print("Results for sensor 1: ");

        Serial.print(sensorMin1);
        Serial.print("\t");
        Serial.println(sensorMax1);

        Serial.print("Results for sensor 2: ");

        Serial.print(sensorMin2);
        Serial.print("\t");
        Serial.println(sensorMax2);
      }
    }

    sensorValue1 = analogRead(sensorPin1);
    sensorValue2 = analogRead(sensorPin2);

    Serial.print(sensorValue1);
    Serial.print("\t");
    Serial.print(sensorValue2);
    Serial.print("\t");

    sensorValue1 = constrain(sensorValue1, sensorMin1, sensorMax1);
    sensorValue1 = map(sensorValue1, sensorMin1, sensorMax1, 0, 2400);
    Serial.print("constrained and mapped:");
    Serial.print("\t");
    Serial.print(sensorValue1);
    Serial.print("\t");

    sensorValue2 = constrain(sensorValue2, sensorMin2, sensorMax2);
    sensorValue2 = map(sensorValue2, sensorMin2, sensorMax2, 0, 2400);

    Serial.print(sensorValue2);
    Serial.print("\t");

    // fade the LED using the calibrated value:
    analogWrite(ledPin1, sensorValue1 / 10);
    analogWrite(ledPin2, sensorValue2 / 10);


    // Filtering the values by dividing by 10 then multiplying by 10
    
    Serial.print("filtered:");
    Serial.print("\t");
    sensorValue1 = sensorValue1 / 10;
    sensorValue1 = sensorValue1 * 10;
    Serial.print(sensorValue1);
    Serial.print("\t");

    sensorValue2 = sensorValue2 / 10;
    sensorValue2 = sensorValue2 * 10;
    Serial.println(sensorValue2);



    // compare the buttonState to its previous state
    if (servoButtonState != servoLastButtonState) {
      // if the state has changed, increment the counter
      if (servoButtonState == LOW) {
        // if the current state is HIGH then the button went from off to on:
        servoButtonPushCounter++;
        Serial.println("on");
        Serial.print("number of button pushes: ");
        Serial.println(servoButtonPushCounter);
      } else {
        // if the current state is LOW then the button went from on to off:
        Serial.println("on to off");
      }
      // Delay a little bit to avoid bouncing
      delay(50);
    }
    // save the current state as the last state, for next time through the loop
    servoLastButtonState = servoButtonState;

    if (servoButtonPushCounter % 2 == 0) {
      Serial.println("modulo 2 is 0");
      servoPos = 0;

    } else {
      servoPos = 140;
    }




    String valueString1 = String(sensorValue1);
    String valueString2 = String(sensorValue2);
    String valueString3 = String(servoPos);
    String bothValues = String(valueString1 + "," + valueString2 + "," + valueString3);

    Serial.println(bothValues);
    pCharacteristic->setValue(bothValues.c_str()); // expects 0 terminated string, converts String to c_str which is a character array
    pCharacteristic->notify();
    delay(50);
  }

  // save the reading. Next time through the loop, it'll be the lastButtonState:
  lastButtonState = reading;
}

ets Jul 29 2019 12:21:46

rst:0x1 (POWERON_RESET),boot:0x13 (SPI_FAST_FLASH_BOOT)
configsip: 0, SPIWP:0xee
clk_drv:0x00,q_drv:0x00,d_drv:0x00,cs0_drv:0x00,hd_drv:0x00,wp_drv:0x00
mode:DIO, clock div:1
load:0x3fff0030,len:1344
load:0x40078000,len:13924
ho 0 tail 12 room 4
load:0x40080400,len:3600
entry 0x400805f0
Starting Arduino BLE Client application...
BLE Advertised Device found: Name: XIAO input device board, Address: 58:cf:79:f2:0b:32, serviceUUID: d881bfae-0213-11ee-be56-0242ac120002, txPower: 9, rssi: -45
Forming a connection to 58:cf:79:f2:0b:32
 - Created client
 - Connected to server
 - Found our service
 - Found our characteristic
The characteristic value was: 0,-1820,140
We are now connected to the XIAO ESP BLE Server.
Notify callback for characteristic 0b85454c-0214-11ee-be56-0242ac120002 of data length 13
data: 100,-1800,140
CORRUPT HEAP: Bad head at 0x3ffc96fe. Expected 0xabba1234 got 0x312d0030

assert failed: multi_heap_free multi_heap_poisoning.c:253 (head != NULL)


Backtrace: 0x40083755:0x3ffc9460 0x40092a61:0x3ffc9480 0x400983bd:0x3ffc94a0 0x4009804b:0x3ffc95d0 0x40083af5:0x3ffc95f0 0x400983ed:0x3ffc9610 0x400d9221:0x3ffc9630 0x400d92f8:0x3ffc9670 0x400d932d:0x3ffc9690 0x400d938a:0x3ffc96b0 0x400d25c5:0x3ffc96d0 0x400d9569:0x3ffc9730

ELF file SHA256: 07981d55a9af21da

Rebooting...
ets Jul 29 2019 12:21:46

rst:0xc (SW_CPU_RESET),boot:0x13 (SPI_FAST_FLASH_BOOT)
configsip: 0, SPIWP:0xee
clk_drv:0x00,q_drv:0x00,d_drv:0x00,cs0_drv:0x00,hd_drv:0x00,wp_drv:0x00
mode:DIO, clock div:1
load:0x3fff0030,len:1344
load:0x40078000,len:13924
ho 0 tail 12 room 4
load:0x40080400,len:3600
entry 0x400805f0
Starting Arduino BLE Client application...
BLE Advertised Device found: Name: , Address: 50:6d:e1:26:13:23, manufacturer data: 4c00100501980c0907, txPower: 8, rssi: -97
BLE Advertised Device found: Name: , Address: 04:2e:b2:d2:cb:f5, manufacturer data: 4c000906035d0a0087a9, rssi: -65
(... and more devices found)

        // Convert received data to 3 separate integers
        char *cstr = &value[0];
        i = 0;
        p = strtok(cstr, delimiter);

        while (p && i < 3)
        {
          sensorValuesString[i] = p;
          p = strtok(NULL, delimiter);
          ++i;
        }

        sensorValue1 = sensorValuesString[0].toInt();
        sensorValue2 = sensorValuesString[1].toInt();
        pos = sensorValuesString[2].toInt();
        myservo.write(pos);

        return;

eps = 0.5 # filter time constant
filter = 0.0 # filtered value

   filter = (1-eps)*filter + eps*T

    // Filtering the values by dividing by 10 then multiplying by 10
    
    Serial.print("filtered:");
    Serial.print("\t");
    sensorValue1 = sensorValue1 / 10;
    sensorValue1 = sensorValue1 * 10;
    Serial.print(sensorValue1);
    Serial.print("\t");

    sensorValue2 = sensorValue2 / 10;
    sensorValue2 = sensorValue2 * 10;
    Serial.println(sensorValue2);

#include <BLEDevice.h>
#include <BLEUtils.h>
#include <BLEServer.h>

BLEServer *pServer;
BLEService *pService;
BLECharacteristic *pCharacteristic;

#define SERVICE_UUID        "d881bfae-0213-11ee-be56-0242ac120002"
#define CHARACTERISTIC_UUID "0b85454c-0214-11ee-be56-0242ac120002"


const int sensorPin1 = 2;
const int sensorPin2 = 3;

const int ledPin1 = 4;
const int ledPin2 = 5;

const int buttonPin = 8;
const int servoButtonPin = 9;

// Brush lifting with a button
int servoPos = 140;
int servoButtonPushCounter = 0;  // counter for the number of button presses
int servoButtonState = 0;        // current state of the button
int servoLastButtonState = 0;    // previous state of the button

int sensorValue1 = 0;   // the sensor value
int sensorMin1 = 4095;  // minimum sensor value
int sensorMax1 = 0;     // maximum sensor value

int sensorValue2 = 0;   // the sensor value
int sensorMin2 = 4095;  // minimum sensor value
int sensorMax2 = 0;     // maximum sensor value

int ledState1 = LOW;        // the current state of the output pin
int ledState2 = LOW;        // the current state of the output pin
int buttonState;            // the current reading from the input pin
int lastButtonState = LOW;  // the previous reading from the input pin

// the following variables are unsigned longs because the time, measured in
// milliseconds, will quickly become a bigger number than can be stored in an int.
unsigned long lastDebounceTime = 0;  // the last time the output pin was toggled
unsigned long debounceDelay = 50;    // the debounce time; increase if the output flickers


void setup() {
//  Serial.begin(115200);
//  while (!Serial) {
//    ; // wait for serial port to connect. Needed for native USB port only
//  }
  pinMode(ledPin1, OUTPUT);
  pinMode(ledPin2, OUTPUT);
  pinMode(sensorPin1, INPUT);
  pinMode(sensorPin2, INPUT);
  pinMode(buttonPin, INPUT_PULLUP);
  pinMode(servoButtonPin, INPUT_PULLUP);

  digitalWrite(ledPin1, ledState1);
  digitalWrite(ledPin2, ledState2);

//  Serial.println("Starting BLE work!");

  BLEDevice::init("XIAO input device board");

  // C++ code: in class BLEDevice we call createServer(); this method returns a pointer (=address)
  // of type BLEServer and we store that in a variable named pServer of type pointer to BLEServer
  // (BLEServer *).
  pServer = BLEDevice::createServer();

  pService = pServer->createService(SERVICE_UUID);
  pCharacteristic = pService->createCharacteristic(
                      CHARACTERISTIC_UUID,
                      BLECharacteristic::PROPERTY_READ |
                      BLECharacteristic::PROPERTY_WRITE
                    );

  pService->start();
  // BLEAdvertising *pAdvertising = pServer->getAdvertising();  // this still is working for backward compatibility
  BLEAdvertising *pAdvertising = BLEDevice::getAdvertising();
  pAdvertising->addServiceUUID(SERVICE_UUID);
  pAdvertising->setScanResponse(true);
  pAdvertising->setMinPreferred(0x06);  // functions that help with iPhone connections issue
  pAdvertising->setMinPreferred(0x12);
  BLEDevice::startAdvertising();
//  Serial.println("Characteristic defined");
}

void loop() {
  int reading = digitalRead(buttonPin);
  servoButtonState = digitalRead(servoButtonPin);
//  Serial.println(servoButtonState);

  // If the switch changed, due to noise or pressing:
  if (reading != lastButtonState) {
    // reset the debouncing timer
    lastDebounceTime = millis();
  }

  if ((millis() - lastDebounceTime) > debounceDelay) {

    // if the button state has changed:
    if (reading != buttonState) {
      buttonState = reading;

      // only toggle the LED if the new button state is HIGH
      if (buttonState == LOW) {
//        Serial.println("start");
        digitalWrite(ledPin1, HIGH);
        digitalWrite(ledPin2, HIGH);

        // determine min and max values of both sensors for 5 seconds
        for (int timems = 0; timems < 5000; timems++) {
          sensorValue1 = analogRead(sensorPin1);
          sensorValue2 = analogRead(sensorPin2);

          // record the maximum sensor value
          if (sensorValue1 > sensorMax1) {
            sensorMax1 = sensorValue1;
          }
          // record the maximum sensor value
          if (sensorValue2 > sensorMax2) {
            sensorMax2 = sensorValue2;
          }

          // record the minimum sensor value
          if (sensorValue1 < sensorMin1) {
            sensorMin1 = sensorValue1;
          }
          // record the minimum sensor value
          if (sensorValue2 < sensorMin2) {
            sensorMin2 = sensorValue2;
          }
          delay(1);
        }
        // signal the end of the calibration period
        digitalWrite(ledPin1, LOW);
        digitalWrite(ledPin2, LOW);

//        Serial.print("Results for sensor 1: ");
//
//        Serial.print(sensorMin1);
//        Serial.print("\t");
//        Serial.println(sensorMax1);
//
//        Serial.print("Results for sensor 2: ");
//
//        Serial.print(sensorMin2);
//        Serial.print("\t");
//        Serial.println(sensorMax2);
      }
    }

    sensorValue1 = analogRead(sensorPin1);
    sensorValue2 = analogRead(sensorPin2);

//    Serial.print(sensorValue1);
//    Serial.print("\t");
//    Serial.print(sensorValue2);
//    Serial.print("\t");

    sensorValue1 = constrain(sensorValue1, sensorMin1, sensorMax1);
    sensorValue1 = map(sensorValue1, sensorMin1, sensorMax1, 0, 300);
//    Serial.print("constrained and mapped:");
//    Serial.print("\t");
//    Serial.print(sensorValue1);
//    Serial.print("\t");

    sensorValue2 = constrain(sensorValue2, sensorMin2, sensorMax2);
    sensorValue2 = map(sensorValue2, sensorMin2, sensorMax2, 0, 300);

//    Serial.print(sensorValue2);
//    Serial.print("\t");

    // fade the LED using the calibrated value:
    analogWrite(ledPin1, sensorValue1);
    analogWrite(ledPin2, sensorValue2);


    // Filtering the values by dividing by 10 then multiplying by 10
    
//    Serial.print("filtered:");
//    Serial.print("\t");
    sensorValue1 = sensorValue1 / 5;
    sensorValue1 = sensorValue1 * 5;
//    Serial.print(sensorValue1);
//    Serial.print("\t");

    sensorValue2 = sensorValue2 / 5;
    sensorValue2 = sensorValue2 * 5;
//    Serial.println(sensorValue2);

    // compare the buttonState to its previous state
    if (servoButtonState != servoLastButtonState) {
      // if the state has changed, increment the counter
      if (servoButtonState == LOW) {
        // if the current state is HIGH then the button went from off to on:
        servoButtonPushCounter++;
//        Serial.println("on");
//        Serial.print("number of button pushes: ");
//        Serial.println(servoButtonPushCounter);
      } //else {
        // if the current state is LOW then the button went from on to off:
//        Serial.println("on to off");
  //    }
      // Delay a little bit to avoid bouncing
      delay(50);
    }
    // save the current state as the last state, for next time through the loop
    servoLastButtonState = servoButtonState;

    if (servoButtonPushCounter % 2 == 0) {
//      Serial.println("modulo 2 is 0");
      servoPos = 140;

    } else {
      servoPos = 0;
    }

    String valueString1 = String(sensorValue1);
    String valueString2 = String(sensorValue2);
    String valueString3 = String(servoPos);
    String bothValues = String(valueString1 + "," + valueString2 + "," + valueString3);

//    Serial.println(bothValues);
    pCharacteristic->setValue(bothValues.c_str()); // expects 0 terminated string, converts String to c_str which is a character array
    pCharacteristic->notify();
    delay(50);
  }

  // save the reading. Next time through the loop, it'll be the lastButtonState:
  lastButtonState = reading;
}

/**
   A BLE client example that is rich in capabilities.
   There is a lot new capabilities implemented.
   author unknown
   updated by chegewara
   Modified by Michelle Vossen
*/

#include "BLEDevice.h"
//#include "BLEScan.h"
#include <AccelStepper.h>
#include <UniversalTimer.h>
#include <ESP32Servo.h>

Servo myservo;  // create servo object to control a servo

int pos = 140;    // variable to store the servo position

// Define servo motor pin
const int servoPin = 32;

UniversalTimer timer(500, true);

// Define stepper pins

const int dirPin1 = 27;
const int stepPin1 = 26;
const int sleepResetPin1 = 25;

const int dirPin2 = 17;
const int stepPin2 = 2;
const int sleepResetPin2 = 15;

const int spd = 100;
const int acc = 100;

// Define motor interface type
#define motorInterfaceType 1 //AccelStepper::DRIVER

// Creates an instance
AccelStepper myStepper1(motorInterfaceType, stepPin1, dirPin1);
AccelStepper myStepper2(motorInterfaceType, stepPin2, dirPin2);
//AccelStepper myStepper3(motorInterfaceType, stepPin3, dirPin3);

// To convert the incoming string into ints
int i;
char delimiter[] = ",";
char *p;
char string[128];
String sensorValuesString[3];
int sensorValue1;
int sensorValue2;
String valueString;

BLERemoteService* pRemoteService;
BLEClient*  pClient;

// The remote service we wish to connect to.
static BLEUUID serviceUUID("d881bfae-0213-11ee-be56-0242ac120002");
// The characteristic of the remote service we are interested in.
static BLEUUID    charUUID("0b85454c-0214-11ee-be56-0242ac120002");

static boolean doConnect = false;
static boolean connected = false;
static boolean doScan = false;
static BLERemoteCharacteristic* pRemoteCharacteristic;
static BLEAdvertisedDevice* myDevice;

static void notifyCallback(
  BLERemoteCharacteristic* pBLERemoteCharacteristic,
  uint8_t* pData,
  size_t length,
  bool isNotify) {
  Serial.print("Notify callback for characteristic ");
  Serial.print(pBLERemoteCharacteristic->getUUID().toString().c_str());
  Serial.print(" of data length ");
  Serial.println(length);
  Serial.print("data: ");
  Serial.write(pData, length);
  Serial.println();
}

class MyClientCallback : public BLEClientCallbacks {
    void onConnect(BLEClient* pclient) {
    }

    void onDisconnect(BLEClient* pclient) {
      connected = false;
      Serial.println("onDisconnect");
    }
};

bool connectToServer() {
  Serial.print("Forming a connection to ");
  Serial.println(myDevice->getAddress().toString().c_str());

  pClient  = BLEDevice::createClient();
  Serial.println(" - Created client");

  pClient->setClientCallbacks(new MyClientCallback());

  // Connect to the remote BLE Server.
  pClient->connect(myDevice);  // if you pass BLEAdvertisedDevice instead of address, it will be recognized type of peer device address (public or private)
  Serial.println(" - Connected to server");
  pClient->setMTU(517); //set client to request maximum MTU from server (default is 23 otherwise)

  // Obtain a reference to the service we are after in the remote BLE server.
  pRemoteService = pClient->getService(serviceUUID);
  if (pRemoteService == nullptr) {
    Serial.print("Failed to find our service UUID: ");
    Serial.println(serviceUUID.toString().c_str());
    pClient->disconnect();
    return false;
  }
  Serial.println(" - Found our service");


  // Obtain a reference to the characteristic in the service of the remote BLE server.
  pRemoteCharacteristic = pRemoteService->getCharacteristic(charUUID);
  if (pRemoteCharacteristic == nullptr) {
    Serial.print("Failed to find our characteristic UUID: ");
    Serial.println(charUUID.toString().c_str());
    pClient->disconnect();
    return false;
  }
  Serial.println(" - Found our characteristic");

  // Read the value of the characteristic.
  if (pRemoteCharacteristic->canRead()) {
    std::string value = pRemoteCharacteristic->readValue();
    Serial.print("The characteristic value was: ");
    Serial.println(value.c_str());
  }

  if (pRemoteCharacteristic->canNotify())
    pRemoteCharacteristic->registerForNotify(notifyCallback);

  connected = true;
  return true;
}


/**
   Scan for BLE servers and find the first one that advertises the service we are looking for.
*/
class MyAdvertisedDeviceCallbacks: public BLEAdvertisedDeviceCallbacks {
    /**
        Called for each advertising BLE server.
    */
    void onResult(BLEAdvertisedDevice advertisedDevice) {
      Serial.print("BLE Advertised Device found: ");
      Serial.println(advertisedDevice.toString().c_str());

      // We have found a device, let us now see if it contains the service we are looking for.
      if (advertisedDevice.haveServiceUUID() && advertisedDevice.isAdvertisingService(serviceUUID)) {

        BLEDevice::getScan()->stop();
        myDevice = new BLEAdvertisedDevice(advertisedDevice);
        doConnect = true;
        doScan = true;

      } // Found our server
    } // onResult
}; // MyAdvertisedDeviceCallbacks

//--------------------------------------------------

void setup() {
  Serial.begin(115200);
  while (!Serial) {
    ; // wait for serial port to connect. Needed for native USB port only
  }
  Serial.println("Starting Arduino BLE Client application...");
  BLEDevice::init("");

  // Retrieve a Scanner and set the callback we want to use to be informed when we
  // have detected a new device.  Specify that we want active scanning and start the
  // scan to run for 5 seconds.
  BLEScan* pBLEScan = BLEDevice::getScan();
  pBLEScan->setAdvertisedDeviceCallbacks(new MyAdvertisedDeviceCallbacks());
  pBLEScan->setInterval(1349);
  pBLEScan->setWindow(449);
  pBLEScan->setActiveScan(true);
  pBLEScan->start(5, false);

  pinMode(sleepResetPin1, OUTPUT);
  pinMode(sleepResetPin2, OUTPUT);
  digitalWrite(sleepResetPin1, HIGH);
  digitalWrite(sleepResetPin2, HIGH);

  myStepper1.setMaxSpeed(spd);
  myStepper1.setAcceleration(acc);
  myStepper1.setSpeed(spd);
  myStepper1.moveTo(-100);          // - is CW, for right stepper
  myStepper1.setCurrentPosition(0);  // turn previous position into new 0 position
  myStepper1.setSpeed(spd);         // setCurrentPosition() has a side effect of setting the current motor speed to 0

  myStepper2.setMaxSpeed(spd);
  myStepper2.setAcceleration(acc);
  myStepper2.setSpeed(spd);
  myStepper2.moveTo(100); // + is CCW, for left stepper
  myStepper2.setCurrentPosition(0);  // turn previous position into new 0 position
  myStepper2.setSpeed(spd);         // setCurrentPosition() has a side effect of setting the current motor speed to 0  

  timer.start();

  myservo.attach(servoPin, 400, 2500);
  myservo.write(pos);

} // End of setup.


// This is the Arduino main loop function.
void loop() {
  //      Serial.println(sensorValue1);
  //      Serial.println(sensorValue2);

//  if (myStepper1.distanceToGo() == 0)
  myStepper1.moveTo(-sensorValue1);

  // Move the motor one step
  myStepper1.run();

//  if (myStepper2.distanceToGo() == 0)
  myStepper2.moveTo(sensorValue2);

  // Move the motor one step
  myStepper2.run();


  if (timer.check()) {

    // If the flag "doConnect" is true then we have scanned for and found the desired
    //z BLE Server with which we wish to connect.  Now we connect to it.  Once we are
    // connected we set the connected flag to be true.
    if (doConnect == true) {
      if (connectToServer()) {
        Serial.println("We are now connected to the XIAO ESP BLE Server.");
      } else {
        Serial.println("We have failed to connect to the server; there is nothing more we will do.");
      }
      doConnect = false;
    }

    if (connected) {
      // Read the value of the characteristic.
      if (pRemoteCharacteristic->canRead()) {
        std::string value = pRemoteCharacteristic->readValue();
        Serial.print("The characteristic value was: ");
        Serial.println(value.c_str());

        // Convert received data to 3 separate integers
        char *cstr = &value[0];
        i = 0;
        p = strtok(cstr, delimiter);

        while (p && i < 3)
        {
          sensorValuesString[i] = p;
          p = strtok(NULL, delimiter);
          ++i;
        }

        sensorValue1 = sensorValuesString[0].toInt();
        sensorValue2 = sensorValuesString[1].toInt();
        pos = sensorValuesString[2].toInt();
        myservo.write(pos);

        return;
      }
    } else if (doScan) {
      BLEDevice::getScan()->start(0); // restart scanning if the connection is lost
      myservo.write(140);

      return;
    }
  }
}