Control servo movement with gesture using ESP32 and Arduino

ESP32GestureServo.ino

/*
  Tutorial: Control servo movement with gesture using ESP32 and Arduino

  Board:
  - ESP32 Dev Module (Node32 Lite)
    https://my.cytron.io/p-node32-lite-wifi-and-bluetooth-development-kit

  Input:
  - PAJ7620U2 9 Gesture Recognition Sensor Module
    https://my.cytron.io/p-paj7620u2-9-gesture-recognition-sensor-module

  Connections   ESP32 | APDS9960
                  3V3 - VIN
                  GND - GND
                 IO22 - SCL
                 IO21 - SDA
                 IO18 - INT

  Output:
  - Analog Micro Servo 9g (3V-6V)
    https://my.cytron.io/p-analog-micro-servo-9g-3v-6v

  Connections   ESP32 | Servo
                 IO19 - Signal
                  RAW - VCC
                  GND - GND

  External libraries:
  - Gesture PAJ7620 by Seeed Studio Version 1.0.0
  - ESP32Servo by John K. Benneth, Kevin Harrington Version 0.6.0
*/

#include <Wire.h>
#include <paj7620.h>
#include <ESP32Servo.h>

Servo myservo;

#define BUILTIN_LED 2
#define SERVO   19

// You can adjust the reaction time according to the actual circumstance.
#define GES_REACTION_TIME 500

// When you want to recognize the forward or backward gestures,
// your gestures' reaction time must less than GES_ENTRY_TIME(0.8s).
#define GES_ENTRY_TIME  800
#define GES_QUIT_TIME   1000

boolean ledState = HIGH;

byte error = 0;
byte data1 = 0;
byte data2 = 0;;

long prevMillisSensor = 0;
byte intervalSensor = 100;
long prevMillisServo = 0;
byte intervalServo = 5;

int servoPos = 0;
int servoTargetPos = 80; // 90 degree + offset
int servoWaveCount = 0;

void setup()
{
  pinMode(BUILTIN_LED, OUTPUT);
  digitalWrite(BUILTIN_LED, HIGH);

  delay(2000);
  
  Serial.begin(115200);
  Serial.println(F("\nControl your servo movement with gesture using ESP32 and Arduino\n"));

  error = paj7620Init(); // initialize PAJ7620 registers
  if (error) {
    Serial.print(F("Initialize error, code:"));
    Serial.println(error);
    digitalWrite(BUILTIN_LED, LOW);
    while (1);
  }
  else {
    Serial.println("Initialize OK");
  }

  // Standard 50Hz servo
  myservo.setPeriodHertz(50);
  // You can adjust the number based on your servo behavior.
  myservo.attach(SERVO, 500, 2500);

  Serial.println(F("\nPlease input your gestures:"));
  Serial.println(F("- Gesture Up for 0 servo postion"));
  Serial.println(F("- Gesture Down for 180 servo postion"));
  Serial.println(F("- Gesture Left for 90 servo postion\n"));
}

void loop()
{
  if (millis() - prevMillisSensor > intervalSensor) {

    // Read Bank_0_Reg_0x43/0x44 for gesture result.
    error = paj7620ReadReg(0x43, 1, &data1);
    if (!error) {
      ledState = !ledState;
      digitalWrite(BUILTIN_LED, ledState);
      
      switch (data1) {
        delay(GES_ENTRY_TIME);
        
        case GES_UP_FLAG:
          paj7620ReadReg(0x43, 1, &data1);
          if (data1 == GES_FORWARD_FLAG) {
            Serial.println(F("Forward"));
          }
          else if (data1 == GES_BACKWARD_FLAG) {
            Serial.println(F("Backward"));
          }
          else {
            Serial.println(F("Up"));
            servoTargetPos = 5; // i0 degree + offset
          }
          delay(GES_QUIT_TIME);
          break;

        case GES_DOWN_FLAG:
          paj7620ReadReg(0x43, 1, &data1);
          if (data1 == GES_FORWARD_FLAG) {
            Serial.println(F("Forward"));
          }
          else if (data1 == GES_BACKWARD_FLAG) {
            Serial.println(F("Backward"));
          }
          else {
            Serial.println(F("Down"));
            servoTargetPos = 175; // 180 degree + offset
          }
          delay(GES_QUIT_TIME);
          break;

        case GES_RIGHT_FLAG:
          paj7620ReadReg(0x43, 1, &data1);
          if (data1 == GES_FORWARD_FLAG) {
            Serial.println(F("Forward"));
          }
          else if (data1 == GES_BACKWARD_FLAG) {
            Serial.println(F("Backward"));
          }
          else {
            Serial.println(F("Right"));
          }
          delay(GES_QUIT_TIME);
          break;

        case GES_LEFT_FLAG:
          paj7620ReadReg(0x43, 1, &data1);
          if (data1 == GES_FORWARD_FLAG) {
            Serial.println(F("Forward"));
          }
          else if (data1 == GES_BACKWARD_FLAG) {
            Serial.println(F("Backward"));
          }
          else {
            Serial.println(F("Left"));
            servoTargetPos = 80; // 90 degree + offset
          }
          delay(GES_QUIT_TIME);
          break;

        case GES_FORWARD_FLAG:
          Serial.println(F("Forward"));
          delay(GES_QUIT_TIME);
          break;

        case GES_BACKWARD_FLAG:
          Serial.println(F("Backward"));
          delay(GES_QUIT_TIME);
          break;

        case GES_CLOCKWISE_FLAG:
          Serial.println(F("Clockwise"));
          delay(GES_QUIT_TIME);
          break;

        case GES_COUNT_CLOCKWISE_FLAG:
          Serial.println(F("anti-clockwise"));
          delay(GES_QUIT_TIME);
          break;

        default:
          paj7620ReadReg(0x44, 1, &data2);
          if (data2 == GES_WAVE_FLAG) {
            Serial.println(F("wave"));
            servoWaveCount = 1;
            delay(GES_QUIT_TIME);
          }
          break;
      }
    }
    prevMillisSensor = millis();
  }

  if (millis() - prevMillisServo > intervalServo &&
      servoPos != servoTargetPos) {

    if (servoPos < servoTargetPos) {
      servoPos++;
    }
    else if (servoPos > servoTargetPos) {
      servoPos--;
    }
    myservo.write(servoPos);

    prevMillisServo = millis();
  }

  if (servoWaveCount) {
    if (servoWaveCount == 1 || servoWaveCount == 3) {
      servoTargetPos = 40; // 45 degree + offset
    }
    else if (servoWaveCount == 2 || servoWaveCount == 4) {
      servoTargetPos = 125; // 135 degree + offset
    }
    else if (servoWaveCount == 5) {
      servoTargetPos = 80; // 90 degree + offset
      servoWaveCount = 0;
    }
    
    if (servoPos == servoTargetPos) {
      servoWaveCount++;
    }
  }
}