Difference between revisions of "Using motion recognition on the Arduino"

Latest revision as of 11:42, 6 March 2023

Explanation coming soon ...

Sender (Central)

Swap out the first line with your trained model library

#include <Interfaces-Sense_inferencing.h> // Change to your model library
#include <ArduinoBLE.h>
#include <Arduino_APDS9960.h>
#include <Arduino_LSM9DS1.h>

#define CONVERT_G_TO_MS2    9.80665f
#define MAX_ACCEPTED_RANGE  2.0f

static bool debug_nn = false;

const char* deviceServiceUuid = "19b10000-e8f2-537e-4f6c-d104768a1214";
const char* deviceServiceCharacteristicUuid = "19b10001-e8f2-537e-4f6c-d104768a1214";

int gesture = -1;
int oldGestureValue = -1;   

void setup() {
  Serial.begin(9600);

  pinMode(LEDR, OUTPUT);
  pinMode(LEDG, OUTPUT);
  pinMode(LEDB, OUTPUT);
  pinMode(LED_BUILTIN, OUTPUT);

  digitalWrite(LEDR, LOW);
  digitalWrite(LEDG, HIGH);
  digitalWrite(LEDB, HIGH);
  digitalWrite(LED_BUILTIN, LOW);
  
  if (!IMU.begin()) {
    Serial.println("- Failed to initialize accelerometer!");
  }
  else {
    Serial.println("- Accelerometer initialized!");
  }

  if (EI_CLASSIFIER_RAW_SAMPLES_PER_FRAME != 3) {
      Serial.println("EI_CLASSIFIER_RAW_SAMPLES_PER_FRAME should be equal to 3 (x,y,z)");
      return;
  }
  
  if (!BLE.begin()) {
    Serial.println("- Starting BLE module failed!");
    while (1);
  }
  
  BLE.setLocalName("Nano 33 BLE Sense (Central)"); 
  BLE.advertise();
}

void loop() {
  connectToPeripheral();
}

void connectToPeripheral(){
  BLEDevice peripheral;
  
  Serial.println("- Discovering peripheral device...");

  do
  {
    BLE.scanForUuid(deviceServiceUuid);
    peripheral = BLE.available();
  } while (!peripheral);
  
  if (peripheral) {
    Serial.println("* Peripheral device found!");
    Serial.print("* Device MAC address: ");
    Serial.println(peripheral.address());
    Serial.print("* Device name: ");
    Serial.println(peripheral.localName());
    Serial.print("* Advertised service UUID: ");
    Serial.println(peripheral.advertisedServiceUuid());
    Serial.println(" ");
    BLE.stopScan();
    controlPeripheral(peripheral);
  }
}

void controlPeripheral(BLEDevice peripheral) {
  Serial.println("- Connecting to peripheral device...");

  if (peripheral.connect()) {
    Serial.println("* Connected to peripheral device!");
    Serial.println(" ");
    // Change LED to green
    digitalWrite(LEDR, HIGH);
    digitalWrite(LEDG, LOW);
    digitalWrite(LEDB, HIGH);
    digitalWrite(LED_BUILTIN, LOW);
  } else {
    Serial.println("* Connection to peripheral device failed!");
    Serial.println(" ");
    return;
  }

  Serial.println("- Discovering peripheral device attributes...");
  if (peripheral.discoverAttributes()) {
    Serial.println("* Peripheral device attributes discovered!");
    Serial.println(" ");
  } else {
    Serial.println("* Peripheral device attributes discovery failed!");
    Serial.println(" ");
    peripheral.disconnect();
    return;
  }

  BLECharacteristic gestureCharacteristic = peripheral.characteristic(deviceServiceCharacteristicUuid);
    
  if (!gestureCharacteristic) {
    Serial.println("* Peripheral device does not have gesture_type characteristic!");
    peripheral.disconnect();
    return;
  } else if (!gestureCharacteristic.canWrite()) {
    Serial.println("* Peripheral does not have a writable gesture_type characteristic!");
    peripheral.disconnect();
    return;
  }
  
  while (peripheral.connected()) {
    gesture = gestureDetection();

    if (oldGestureValue != gesture) {  
      oldGestureValue = gesture;
      Serial.print("- Writing value to gesture characteristic: ");
      Serial.println(gesture);
      gestureCharacteristic.writeValue((byte)gesture);
      Serial.println("- Writing value to gesture characteristic done!");
      Serial.println(" ");
    }
  
  }
  Serial.println("- Peripheral device disconnected!");
  // Change LED to red
  digitalWrite(LEDR, LOW);
  digitalWrite(LEDG, HIGH);
  digitalWrite(LEDB, HIGH);
  digitalWrite(LED_BUILTIN, LOW);
}


int gestureDetection() 
{
    int gesture_index;
    Serial.println("- Start inferencing in 2 second...");
    delay(2000);
    Serial.println("- Getting accelorometer sample...");

    // Allocate a buffer here for the values we'll read from the IMU
    float buffer[EI_CLASSIFIER_DSP_INPUT_FRAME_SIZE] = { 0 };

    for (size_t ix = 0; ix < EI_CLASSIFIER_DSP_INPUT_FRAME_SIZE; ix += 3) {
        // Determine the next tick (and then sleep later)
        uint64_t next_tick = micros() + (EI_CLASSIFIER_INTERVAL_MS * 1000);

        IMU.readAcceleration(buffer[ix], buffer[ix + 1], buffer[ix + 2]);

        for (int i = 0; i < 3; i++) {
            if (fabs(buffer[ix + i]) > MAX_ACCEPTED_RANGE) {
                buffer[ix + i] = ei_get_sign(buffer[ix + i]) * MAX_ACCEPTED_RANGE;
            }
        }

        buffer[ix + 0] *= CONVERT_G_TO_MS2;
        buffer[ix + 1] *= CONVERT_G_TO_MS2;
        buffer[ix + 2] *= CONVERT_G_TO_MS2;

        delayMicroseconds(next_tick - micros());
    }

    // Turn the raw buffer in a signal which we can the classify
    signal_t signal;
    int err = numpy::signal_from_buffer(buffer, EI_CLASSIFIER_DSP_INPUT_FRAME_SIZE, &signal);
    if (err != 0) {
        Serial.println("Failed to create a signal!");
        return -1;
    }

    // Run the classifier
    ei_impulse_result_t result = { 0 };

    err = run_classifier(&signal, &result, debug_nn);
    if (err != EI_IMPULSE_OK) {
        Serial.println("Failed to run classifier!");
        return -1;
    }

    for (int ix = 0; ix < EI_CLASSIFIER_LABEL_COUNT; ix++) {
      if (result.classification[ix].value > 0.8) {
        gesture_index = ix;
        Serial.println(" ");
        Serial.print("* Detected gesture: ");
        Serial.print(result.classification[ix].label);
        Serial.print(" with index number: ");
        Serial.println(ix);
        Serial.println(" ");
      }
    }
    return gesture_index; 
}
  
float ei_get_sign(float number) {
  return (number >= 0.0) ? 1.0 : -1.0;
}

Receiver (Peripheral)

#include <ArduinoBLE.h>
      
enum {
  GESTURE_NONE  = -1,
  GESTURE_SLEEP    = 0,
  GESTURE_UPDOWN  = 1,
  GESTURE_WAVE  = 2,
};

const char* deviceServiceUuid = "19b10000-e8f2-537e-4f6c-d104768a1214";
const char* deviceServiceCharacteristicUuid = "19b10001-e8f2-537e-4f6c-d104768a1214";

int gesture = -1;

BLEService gestureService(deviceServiceUuid); 
BLEByteCharacteristic gestureCharacteristic(deviceServiceCharacteristicUuid, BLERead | BLEWrite);


void setup() {
  Serial.begin(9600);
  
  pinMode(LEDR, OUTPUT);
  pinMode(LEDG, OUTPUT);
  pinMode(LEDB, OUTPUT);
  pinMode(LED_BUILTIN, OUTPUT);
  
  digitalWrite(LEDR, HIGH);
  digitalWrite(LEDG, HIGH);
  digitalWrite(LEDB, HIGH);
  digitalWrite(LED_BUILTIN, LOW);

  
  if (!BLE.begin()) {
    Serial.println("- Starting BLE module failed!");
    while (1);
  }

  BLE.setLocalName("Nano 33 BLE Sense (Peripheral)");
  BLE.setAdvertisedService(gestureService);
  gestureService.addCharacteristic(gestureCharacteristic);
  BLE.addService(gestureService);
  gestureCharacteristic.writeValue(-1);
  BLE.advertise();
}

void loop() {
  BLEDevice central = BLE.central();
  Serial.println("- Discovering central device...");
  delay(500);

  if (central) {
    Serial.println("* Connected to central device!");
    Serial.print("* Device MAC address: ");
    Serial.println(central.address());
    Serial.println(" ");

    while (central.connected()) {
      if (gestureCharacteristic.written()) {
         gesture = gestureCharacteristic.value();
         writeGesture(gesture);
       }
    }
    
    Serial.println("* Disconnected to central device!");
  }
}

void writeGesture(int gesture) {
  Serial.println("- Gesture characteristic has changed!");
  
   switch (gesture) {
      case GESTURE_SLEEP:
        Serial.println("* Actual value: SLEEP (0) (red LED on)");
        Serial.println(" ");
        digitalWrite(LEDR, LOW);
        digitalWrite(LEDG, HIGH);
        digitalWrite(LEDB, HIGH);
        digitalWrite(LED_BUILTIN, LOW);
        break;
      case GESTURE_UPDOWN:
        Serial.println("* Actual value: UPDOWN (1) (green LED on)");
        Serial.println(" ");
        digitalWrite(LEDR, HIGH);
        digitalWrite(LEDG, LOW);
        digitalWrite(LEDB, HIGH);
        digitalWrite(LED_BUILTIN, LOW);
        break;
      case GESTURE_WAVE:
        Serial.println("* Actual value: WAVE (2) (blue LED on)");
        Serial.println(" ");
        digitalWrite(LEDR, HIGH);
        digitalWrite(LEDG, HIGH);
        digitalWrite(LEDB, LOW);
        digitalWrite(LED_BUILTIN, LOW);
        break;
      default:
        digitalWrite(LEDR, HIGH);
        digitalWrite(LEDG, HIGH);
        digitalWrite(LEDB, HIGH);
        digitalWrite(LED_BUILTIN, LOW);
        break;
    }      
}

Connect some Outputs

Arduino BLE 33 sense pinout

Servo

The servo motor can connect directly to a PWM digital pin, check pinout above.
On the image below, the signal line (yellow wire) is connected to D2.
Black is GND and red is connected to +5V (beware need to solder a pad on the bottom of the Arduino in order to activate the 5V pin)

DC motor + other higher voltage load

Receiver (Peripheral) with Servo motor /DC motor, etc.

#include <ArduinoBLE.h>
#include <Servo.h>

 
enum {
  GESTURE_NONE  = -1,
  GESTURE_SLEEP    = 0,
  GESTURE_UPDOWN  = 1,
  GESTURE_WAVE  = 2,
};
 
const char* deviceServiceUuid = "19b10000-e8f2-537e-4f6c-d104768a1214";
const char* deviceServiceCharacteristicUuid = "19b10001-e8f2-537e-4f6c-d104768a1214";
 
int gesture = -1;
 
BLEService gestureService(deviceServiceUuid); 
BLEByteCharacteristic gestureCharacteristic(deviceServiceCharacteristicUuid, BLERead | BLEWrite);

Servo myservo;  // create servo object to control a servo
// twelve servo objects can be created on most boards


 
void setup() {
  Serial.begin(9600);
 
  pinMode(LEDR, OUTPUT);
  pinMode(LEDG, OUTPUT);
  pinMode(LEDB, OUTPUT);
  pinMode(LED_BUILTIN, OUTPUT);

  myservo.attach(2);  // attaches the servo on pin 2 to the servo object
  pinMode(3, OUTPUT); //pin number 3 defined as output, can connect fan + various actuators, lights(fan, solenoid,vibration motor, led strip, relay, etc.)
  pinMode(4, OUTPUT);  //pin number 4 defined as output, can connect DC motor + various actuators, lights(fan, solenoid,vibration motor, led strip, relay, etc.)
 
  digitalWrite(LEDR, HIGH);
  digitalWrite(LEDG, HIGH);
  digitalWrite(LEDB, HIGH);
  digitalWrite(LED_BUILTIN, LOW);
 
 
  if (!BLE.begin()) {
    Serial.println("- Starting BLE module failed!");
    while (1);
  }
 
  BLE.setLocalName("Nano 33 BLE Sense (Peripheral)");
  BLE.setAdvertisedService(gestureService);
  gestureService.addCharacteristic(gestureCharacteristic);
  BLE.addService(gestureService);
  gestureCharacteristic.writeValue(-1);
  BLE.advertise();
  
}
 
void loop() {
  BLEDevice central = BLE.central();
  Serial.println("- Discovering central device...");
  delay(500);
 
  if (central) {
    Serial.println("* Connected to central device!");
    Serial.print("* Device MAC address: ");
    Serial.println(central.address());
    Serial.println(" ");
 
    while (central.connected()) {
      if (gestureCharacteristic.written()) {
         gesture = gestureCharacteristic.value();
         writeGesture(gesture);
       }
    }
 
    Serial.println("* Disconnected to central device!");
  }
}
 
void writeGesture(int gesture) {
  Serial.println("- Gesture characteristic has changed!");
 
   switch (gesture) {
      case GESTURE_SLEEP:
        Serial.println("* Actual value: SLEEP (0) (red LED on)");
        Serial.println(" ");
        digitalWrite(LEDR, LOW);
        digitalWrite(LEDG, HIGH);
        digitalWrite(LEDB, HIGH);
        digitalWrite(LED_BUILTIN, LOW);
        myservo.write(10);              // tell servo to go to position 10 degrees
        digitalWrite(3, LOW);           
        digitalWrite(4, LOW);



        break;
      case GESTURE_UPDOWN:
        Serial.println("* Actual value: UPDOWN (1) (green LED on)");
        Serial.println(" ");
        digitalWrite(LEDR, HIGH);
        digitalWrite(LEDG, LOW);
        digitalWrite(LEDB, HIGH);
        digitalWrite(LED_BUILTIN, LOW);
        myservo.write(50);               // tell servo to go to position 50 degrees
        digitalWrite(3, HIGH);          // start DC motor
        digitalWrite(4, LOW);          // fan is off

        

        break;
      case GESTURE_WAVE:
        Serial.println("* Actual value: WAVE (2) (blue LED on)");
        Serial.println(" ");
        digitalWrite(LEDR, HIGH);
        digitalWrite(LEDG, HIGH);
        digitalWrite(LEDB, LOW);
        digitalWrite(LED_BUILTIN, LOW);
        myservo.write(180);               // tell servo to go to position 180 degrees
        digitalWrite(3, LOW);            // DC motor is off 
        digitalWrite(4, HIGH);          // run fan 


        break;
      default:
        digitalWrite(LEDR, HIGH);
        digitalWrite(LEDG, HIGH);
        digitalWrite(LEDB, HIGH);
        digitalWrite(LED_BUILTIN, LOW);
        myservo.write(10);              // tell servo to go to position 10 degrees
        digitalWrite(3, LOW);          // DC motor is off
        digitalWrite(4, LOW);         // fan is off


        break;
        
    }      
}

@@ Line 323: / Line 323: @@
 ===Arduino BLE 33 sense pinout===
 [[File:SRL-image-0.png | 900 px]]
-===Serso===
+===Servo===
 The servo motor can connect directly to a PWM digital pin, check pinout above. <br>
 On the image below, the signal line (yellow wire) is connected to D2. <br>
@@ Line 484: / Line 484: @@
 <br>
 <br>
+[[Category:Motion Tracking]][[Category:Arduino]][[Category:Classification Models]]