Hackster is hosting Hackster Holidays, Ep. 6: Livestream & Giveaway Drawing. Watch previous episodes or stream live on Monday!Stream Hackster Holidays, Ep. 6 on Monday!
Uriel Cedeño Antunez
Published © Apache-2.0

Haptic Movement Guidance

This projects is about create a prototype to guide people with haptics to dance/exercise.

IntermediateFull instructions provided20 hours22
Honorable Mention Visual Impairments
Build2gether 2.0 — Inclusive Innovation Challenge
Haptic Movement Guidance

Things used in this project

Hardware components

XIAO BLE
Seeed Studio XIAO BLE
×2
Seeed Studio XIAO ESP32S3 Sense
Seeed Studio XIAO ESP32S3 Sense
×1
Seeed Studio Grove Vision AI module v2 kit
×1
General Purpose Transistor NPN
General Purpose Transistor NPN
×8
Resistor 221 ohm
Resistor 221 ohm
×8
Capacitor 100 nF
Capacitor 100 nF
×8
Resistor 221k ohm
Resistor 221k ohm
×6
Solar Cockroach Vibrating Disc Motor
Brown Dog Gadgets Solar Cockroach Vibrating Disc Motor
×8
Li-Ion Battery 1000mAh
Li-Ion Battery 1000mAh
×1

Hand tools and fabrication machines

Mini Side Cutter, 120mm Length with 25mm Jaw Capacity
Mini Side Cutter, 120mm Length with 25mm Jaw Capacity
Soldering iron (generic)
Soldering iron (generic)
Plier, Side Cutting
Plier, Side Cutting

Story

Read more

Schematics

AI Eye node

The schematic for the Battery-powered AI camera that identify body movements. It has a jumper to debug the software by disconneting the battery.

Haptic bracelet

The schematic of a battery-powered haptic bracelet.

AI Eye node Schematic Image

For people that do not have eagle installed

Haptic bracelet Schematic Image

For people that do not have eagle installed

Code

MiniLog_cpp

C/C++
This is a minilog library for debugging purposes
#include "MiniLog.h"

MiniLog::MiniLog(int enable){
  this->enable = enable;
}

void MiniLog::error(const char * msg){
  unsigned long currentTime = millis();
  unsigned long seconds = currentTime / 1000;
  unsigned long milliseconds = currentTime % 1000;
  Serial.print("[");
  Serial.print(seconds);
  Serial.print(".");
  if (milliseconds < 100) Serial.print("0");
  if (milliseconds < 10) Serial.print("0");
  Serial.print(milliseconds);
  Serial.print("]");
  Serial.print(" ERROR - ");
  Serial.println(msg);
}

void MiniLog::debug(const char * msg){
  unsigned long currentTime = millis();
  unsigned long seconds = currentTime / 1000;
  unsigned long milliseconds = currentTime % 1000;
  Serial.print("[");
  Serial.print(seconds);
  Serial.print(".");
  if (milliseconds < 100) Serial.print("0");
  if (milliseconds < 10) Serial.print("0");
  Serial.print(milliseconds);
  Serial.print("]");
  Serial.print(" DEBUG - ");
  Serial.println(msg);
}

void MiniLog::info(const char * msg){
  unsigned long currentTime = millis();
  unsigned long seconds = currentTime / 1000;
  unsigned long milliseconds = currentTime % 1000;
  Serial.print("[");
  Serial.print(seconds);
  Serial.print(".");
  if (milliseconds < 100) Serial.print("0");
  if (milliseconds < 10) Serial.print("0");
  Serial.print(milliseconds);
  Serial.print("]");
  Serial.print(" INFO - ");
  Serial.println(msg);
}

MiniLog_header

C/C++
The header file for MiniLog lib
#ifndef MiniLog_H
#define MiniLog_H
#include <Arduino.h>

class MiniLog
{
  private:
    int enable;
  public:
    MiniLog(int enable);
    void error(const char * msg);
    void debug(const char * msg);
    void info(const char * msg);
};

#endif

Right_Arm_Bracelet _cpp

C/C++
This is the code for the right bracelet.
#include "BLEDevice.h"
#include "MiniLog.h"
#include "ArmNodeRight_Const.h"
#include <I2Cdev.h>
#include "MPU6050_6Axis_MotionApps20.h"
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    #include "Wire.h"
#endif

/*------------------------------------------------
// Objects and Interfaces
//-----------------------------------------------*/
MPU6050 sensor;
MiniLog Log(1);
BLEDevice mydevice;
static BLEScan *bleScanning_p;
static BLEAddress *bleAddress_p;
static BLEClient *bleClient_p;
static BLERemoteService *bleRemoteService_p;
static BLERemoteCharacteristic *bleRemoteChar_p;

/*------------------------------------------------
// Globals
//-----------------------------------------------*/
static bool newDataAck_B = false;
static uint8_t newBLEData_U08 = IDLE_R;
static uint8_t oldBLEData_U08 = IDLE_R;
static bool connectionAck_B = false;
static uint8_t fifoBuffer_aU8[64];               //FIFO Buffer for calculating angles base on MPU
static float outputVector_a32[3];                //vector from MPU that conatins angles
Quaternion q;           // quaternion 
VectorFloat gravity;    // gravity vector
/*------------------------------------------------
// Class for BLE to look if the device is connected to the correct server
//-----------------------------------------------*/
class clientCallback: public BLEAdvertisedDeviceCallbacks {
  void onResult(BLEAdvertisedDevice advertisedDevice) {
    if (advertisedDevice.getName() == SERVER_NAME) 
    { 
      advertisedDevice.getScan()->stop(); 
      bleAddress_p = new BLEAddress(advertisedDevice.getAddress()); 
      Log.info("Server Found");
    }
  }
};
/*------------------------------------------------
// Index of fucntions
//-----------------------------------------------*/
static void initMotorSequence();
static void initMotorSequenceBack();
static void serverDataCallback(BLERemoteCharacteristic *rCharacteristic_p, uint8_t* rData_p, size_t length, bool isNotify_B);
static bool connectServer();
static void startComms2Server();
static void getSensorData();
static void hapticActivation();
static void activateMotors(uint8_t motorNumber, int16_t minValue_S16, int16_t maxValue_S16);
/*------------------------------------------------
// Motor  
// *Description: Calculating the angles based on MPU data
//-----------------------------------------------*/
static void activateMotors(uint8_t motorNumber, int16_t minValue_S16, int16_t maxValue_S16){
  if (motorNumber == 0) {digitalWrite(INNER_M, HIGH);}
  if (motorNumber == 1) {digitalWrite(OUTTER_M, HIGH);}
  if (motorNumber == 2) {digitalWrite(FRONT_M, HIGH);}
  if (motorNumber == 3) {digitalWrite(BACK_M, HIGH);}
  while(1)
  {
    getSensorData();
    if (minValue_S16 <=outputVector_a32[1] && outputVector_a32[1]<= maxValue_S16) break;
    delay(200);
  }
  digitalWrite(BACK_M, LOW);
  digitalWrite(OUTTER_M, LOW);
  digitalWrite(FRONT_M, LOW);
  digitalWrite(INNER_M, LOW);

}
/*------------------------------------------------
// MPU Angles 
// *Description: Calculating the angles based on MPU data
//-----------------------------------------------*/
static void getSensorData (){
  float rawDatavector_a32[3];
  if (sensor.dmpGetCurrentFIFOPacket(fifoBuffer_aU8)){
    sensor.dmpGetQuaternion(&q, fifoBuffer_aU8);
    sensor.dmpGetGravity(&gravity, &q);
    sensor.dmpGetYawPitchRoll(rawDatavector_a32, &q, &gravity);
    outputVector_a32[0] = rawDatavector_a32[0] * 180/M_PI; // 180/PI to avoid sending decimals due to an issue on client
    outputVector_a32[1] = rawDatavector_a32[1] * 180/M_PI;
    outputVector_a32[2] = rawDatavector_a32[2] * 180/M_PI;
  }
  else{
    outputVector_a32[0] = 0.0;
    outputVector_a32[1] = 0.0;
    outputVector_a32[2] = 0.0;
    Log.error("Sensor Data Corrupted");
  }
}
/*------------------------------------------------
// State machine to actuivate haptic motor 
// *Description: State machine to select the correct activation of motors
//-----------------------------------------------*/
static void hapticActivation()
{
  if (oldBLEData_U08 == IDLE_R)
  {
    if (newBLEData_U08 == UPEXTHOR_R) {activateMotors(1,-25,-21);}
    if (newBLEData_U08 == UPEXTVER_R) {activateMotors(1,-53,-50);}
  }
  if (oldBLEData_U08 == UPEXTHOR_R)
  {
    if (newBLEData_U08 == IDLE_R) {activateMotors(0,0,5);}
    if (newBLEData_U08 == UPEXTVER_R) {activateMotors(1,-53,-50);}
  }
  if (oldBLEData_U08 == UPEXTVER_R)
  {
    if (newBLEData_U08 == IDLE_R) {activateMotors(0,0,5);}
    if (newBLEData_U08 == UPEXTHOR_R) {activateMotors(0,-25,-21);}
  }
}

/*------------------------------------------------
// Establish connections to BLE Server
// *Description: This will serve to scan BLE devices, find the server and connect to it.
//-----------------------------------------------*/
static void startComms2Server()
{
  bleScanning_p = mydevice.getScan();
  bleScanning_p->setAdvertisedDeviceCallbacks(new clientCallback());
  bleScanning_p->setActiveScan(true);
  bleScanning_p->start(30);
  connectionAck_B = connectServer();
  if (connectionAck_B == true)
  {
    Log.info("Connected to Server");
  }
  else
  {
    Log.error("Server offline");
  }
}
/*------------------------------------------------
// Handler to read BLE characteristic
// *Description: This will get the BLE characteristic value
//-----------------------------------------------*/
static void serverDataCallback(BLERemoteCharacteristic *rCharacteristic_p, uint8_t* rData_p, size_t length, bool isNotify_B)
{
    newDataAck_B = true;
    if(*rData_p == '0') newBLEData_U08 = IDLE_R;
    else if(*rData_p == '1') newBLEData_U08 = UPEXTHOR_R;
    else if(*rData_p == '2') newBLEData_U08 = UPEXTVER_R;
    else newBLEData_U08 = IDLE_R;                         //if any corrupted data go to safety position
}
/*------------------------------------------------
// Handler to connect to server 
// *Description: This function is charge to connect to server once it is found
//-----------------------------------------------*/
bool connectServer(){
  bleClient_p = mydevice.createClient();
  bleClient_p -> connect(*bleAddress_p);
  bleRemoteService_p = bleClient_p->getService(SERVICE_UUID); //Check references for service availability
  if(bleRemoteService_p == nullptr)
  {
    Log.error("Service unavailable");
    return false;
  }
  bleRemoteChar_p = bleRemoteService_p-> getCharacteristic(RIGHTARMSTATE_UUID);
  if(bleRemoteChar_p == nullptr)
  {
    Log.error("Service characteristics unavailable");
    return false;
  }
  bleRemoteChar_p->registerForNotify(serverDataCallback);
  return true;
}
/*------------------------------------------------
// Funny sequence to alert user that the program ends calibrations
// *Description: turn on sequentially all motor
//-----------------------------------------------*/
static void initMotorSequenceBack()
{
  digitalWrite(BACK_M, HIGH);
  delay(700);
  digitalWrite(BACK_M, LOW);
  delay(100);
  digitalWrite(OUTTER_M, HIGH);
  delay(700);
  digitalWrite(OUTTER_M, LOW);
  delay(100);
  digitalWrite(FRONT_M, HIGH);
  delay(700);
  digitalWrite(FRONT_M, LOW);
  delay(100);
  digitalWrite(INNER_M, HIGH);
  delay(700);
  digitalWrite(INNER_M, LOW);
}
/*------------------------------------------------
// Funny sequence to alert user that the program starts
// *Description: turn on sequentially all motor
//-----------------------------------------------*/
static void initMotorSequence()
{
  digitalWrite(INNER_M, HIGH);
  delay(700);
  digitalWrite(INNER_M, LOW);
  delay(100);
  digitalWrite(FRONT_M, HIGH);
  delay(700);
  digitalWrite(FRONT_M, LOW);
  delay(100);
  digitalWrite(OUTTER_M, HIGH);
  delay(700);
  digitalWrite(OUTTER_M, LOW);
  delay(100);
  digitalWrite(BACK_M, HIGH);
  delay(700);
  digitalWrite(BACK_M, LOW);
}
/*------------------------------------------------
// Setup Function
// Description: This setup all modules at init stage
//-----------------------------------------------*/
void setup() {
  uint8_t sensorStatus_U08;          // MPU6050 status after initilize
  Serial.begin(9600);                //Debugging purposes

  #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
        Wire.begin();
  #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
      Fastwire::setup(400, true);
  #endif

  //Config haptic motor I/O
  pinMode(INNER_M, OUTPUT);
  pinMode(OUTTER_M, OUTPUT);
  pinMode(FRONT_M, OUTPUT);
  pinMode(BACK_M, OUTPUT);
  initMotorSequence();           //init sequence for getting user to iddle stage
  delay(1000);
  //Init MPU6050 
  sensor.initialize();    //Iniciando el sensor
  if (!sensor.testConnection()){
    Log.error("Sensor is not responding, Execution stops, enter Safe Mode"); //TODO: routine for safe mode
    while(1); // Need to reset device
  }
  sensorStatus_U08 = sensor.dmpInitialize();
  //Setting offset values to move reference Frame
  //This values are for my chips, please calibrate yours once to see what values are good! Use example from MPU lib to do so.
  sensor.setXGyroOffset(63);
  sensor.setYGyroOffset(-1);
  sensor.setZGyroOffset(33);
  sensor.setZAccelOffset(1332); 

  if (sensorStatus_U08 == 0){
    sensor.CalibrateAccel(6);
    sensor.CalibrateGyro(6);
    sensor.setDMPEnabled(true);
    Log.info("Sensor ready to action!");
  }
  else {
    Log.error("Sensor is failing setting up offsets");
    while(1); // Need to reset the device  
  }
  initMotorSequenceBack();
  //BLE Sequence
  mydevice.init(DEVICE_NAME);
  if (!mydevice.getInitialized()){
    Log.error("Modem is not available. Execution stops, enter Safe Mode");
    while(1); // Need to reset the device 
  }
  startComms2Server();
}
/*------------------------------------------------
// Main function 
// Description: Main
//-----------------------------------------------*/
void loop() {
  if(connectionAck_B == true){
    if(newDataAck_B == true){
      if(newBLEData_U08 != oldBLEData_U08){
        Log.debug("Data received");
        Serial.printf("%d", newBLEData_U08);
        newDataAck_B = false;
        hapticActivation();
        oldBLEData_U08=newBLEData_U08;
      }
    }
  }
}

EyeNode_cpp

C/C++
This is the code for the EyeNode
#include <Seeed_Arduino_SSCMA.h>
#include "EyeNode_Const.h"
#include <math.h>
#include "MiniLog.h"
#include <BLEDevice.h>
#include <BLEUtils.h>
#include <BLEServer.h>
/*------------------------------------------------
// Objects and Interfaces
//-----------------------------------------------*/
static SSCMA EyeNode;   //Groove Sense EyeNode Interface
static BLEDevice mydevice;
static MiniLog Log(1);
static BLEServer *server_p;
static BLEService *service_p; 
static BLECharacteristic *bleRightArmChar_p,*bleLeftArmChar_p;
static BLEAdvertising *advertizing_p;
/*------------------------------------------------
// Globals
//-----------------------------------------------*/
static bool initPoseEvalStatus_B = false;

static uint16_t righArmShldrElbDis_rU16 = 0;          // Reference distance from Shoulder-elbow RA capture first frame
static uint16_t righArmShldrWrDis_rU16 = 0;           // Reference distance from Shoulder-Wrist RA capture first frame
static uint16_t leftArmShldrElbDis_rU16 = 0;          // Reference distance from Shoulder-elbow LA capture first frame
static uint16_t leftArmShldrWrDis_rU16 = 0;           // Reference distance from Shoulder-Wrist LA capture first frame

static uint16_t righArmShldrElbDis_U16 = 0;          // Ongoing distance from Shoulder-elbow RA capture any frame
static uint16_t righArmShldrWrDis_U16 = 0;           // Ongoing distance from Shoulder-Wrist RA capture any frame
static uint16_t leftArmShldrElbDis_U16 = 0;          // Ongoing distance from Shoulder-elbow LA capture any frame
static uint16_t leftArmShldrWrDis_U16 = 0;           // Ongoing distance from Shoulder-Wrist LA capture any frame

static int16_t righArmShldrElbAng_S16 = 0;          // Ongoing Angle Between Shoulder-elbow RA capture any frame
static int16_t righArmShldrWrAng_S16= 0;            // Ongoing Angle Between Shoulder-Wrist RA capture any frame
static int16_t leftArmShldrElbAng_S16 = 0;          // Ongoing Angle Between Shoulder-elbow LA capture any frame
static int16_t leftArmShldrWrAng_S16 = 0;           // Ongoing Angle Between Shoulder-Wrist LA capture any frame

static uint16_t rightArmShldrCoor_U16[2] = {0,0};          // Coordinates of Shoulder RA
static uint16_t rightArmElbCoor_U16[2] = {0,0};            // Coordinates of Elbow RA
static uint16_t rightArmWrCoor_U16[2] = {0,0};            // Coordinates of Wrist RA
static uint16_t leftArmShldrCoor_U16[2] = {0,0};          // Coordinates of Shoulder LA
static uint16_t leftArmElbCoor_U16[2] = {0,0};            // Coordinates of Elbow LA
static uint16_t leftArmWrCoor_U16[2] = {0,0};            // Coordinates of Wrist LA

static uint8_t rightArmStage = 0;
static uint8_t leftArmStage = 0;

static bool connectionAck_B = false;                     // Conection Acknowledge
static int8_t numOfDevices = 0;                          //Number of devices connected up to 2
String dataRA2Transmit, dataLA2Transmit;                 //Buffer for BLE Transmition.

/*------------------------------------------------
// Class for BLE to look if any incomming connections up to 2
//-----------------------------------------------*/
class serverCallback: public BLEServerCallbacks
{
    void onConnect(BLEServer *server_p){
        connectionAck_B = true;
        numOfDevices = numOfDevices + 1;
        if (numOfDevices >= 2) 
        {
          numOfDevices = 2;
        }
        else
        {
          mydevice.startAdvertising();
        }
        delay(100);
    }
    void onDisconnect(BLEServer *server_p){
        connectionAck_B = false;
        numOfDevices = numOfDevices - 1;
        delay(100);
        mydevice.startAdvertising();
        if(numOfDevices < 0) numOfDevices = 0;
        
    }
};

/*------------------------------------------------
// Index of fucntions
//-----------------------------------------------*/
static void captureBodyPoseData();
static void estimateBodyPoseData();
static void inferBodyPose();
static void updateBodyPose();
/*------------------------------------------------
// Update Body Gesture on BLE Service
// *Description: Update the characteristics of BLE service 
//-----------------------------------------------*/
static void updateBodyPose()
{
  switch(rightArmStage)
  {
    case IDDLE_R:
      dataRA2Transmit = String("0");
    break;
    case UPEXTHOR_R:
      dataRA2Transmit = String("1");
    break;
    case UPEXTVER_R:
      dataRA2Transmit = String("2");
    break;
    default:
    break;
  }
  switch(leftArmStage)
  {
    case IDDLE_L:
      dataLA2Transmit = String("0");
    break;
    case UPEXTHOR_L:
      dataLA2Transmit = String("1");
    break;
    case UPEXTVER_L:
      dataLA2Transmit = String("2");
    break;
    default:
    break;
  }
    bleRightArmChar_p->setValue(dataRA2Transmit);
    bleLeftArmChar_p->setValue(dataLA2Transmit);
    bleRightArmChar_p->notify();
    bleLeftArmChar_p->notify();
}
/*------------------------------------------------
// Infer Body Gesture
// *Description: Infer the gesture that is read 
//-----------------------------------------------*/
static void inferBodyPose()
{  
  if((23<= righArmShldrElbDis_U16 && righArmShldrElbDis_U16 <=39) && (52<= righArmShldrWrDis_U16 && righArmShldrWrDis_U16<=80) &&
     (23<= leftArmShldrElbDis_U16 && leftArmShldrElbDis_U16 <=39) && (52<= leftArmShldrWrDis_U16 && leftArmShldrWrDis_U16<=80)   )
  {
    //Iddle right
    if (101<= righArmShldrElbAng_S16 && righArmShldrElbAng_S16<=112) rightArmStage = IDDLE_R;
    //Iddle left
    if (69 <= leftArmShldrElbAng_S16 && leftArmShldrElbAng_S16<=78)  leftArmStage = IDDLE_L;
    //UP Right Arm Horizontally
    if (162<= righArmShldrElbAng_S16 && righArmShldrElbAng_S16<=174) rightArmStage = UPEXTHOR_R;
    //UP Left Arm Horizontally 
    if (-15<= leftArmShldrElbAng_S16 && leftArmShldrElbAng_S16<=12) leftArmStage = UPEXTHOR_L;
    //UP Right Arm Vertically
    if (-129<= righArmShldrElbAng_S16 && righArmShldrElbAng_S16<=-110) rightArmStage = UPEXTVER_R;
    //UP Left Arm Vertically
    if (-90<= leftArmShldrElbAng_S16 && leftArmShldrElbAng_S16<=-67) leftArmStage = UPEXTVER_L;
  }

}
/*------------------------------------------------
// Capture parameters
// *Description: Capture data from Inference
//-----------------------------------------------*/
static void captureBodyPoseData(){
  if (!EyeNode.invoke())
  {
      if(EyeNode.keypoints().size() > 0)
      {
        // Getting keypoints
        rightArmShldrCoor_U16[COORD_X] = EyeNode.keypoints()[0].points[RIGHT_SHLDR].x;        
        rightArmShldrCoor_U16[COORD_Y] = EyeNode.keypoints()[0].points[RIGHT_SHLDR].y; 
        rightArmElbCoor_U16[COORD_X] = EyeNode.keypoints()[0].points[RIGHT_ELB].x; 
        rightArmElbCoor_U16[COORD_Y] = EyeNode.keypoints()[0].points[RIGHT_ELB].y;          
        rightArmWrCoor_U16[COORD_X] = EyeNode.keypoints()[0].points[RIGHT_WR].x;
        rightArmWrCoor_U16[COORD_Y] = EyeNode.keypoints()[0].points[RIGHT_WR].y; 
        leftArmShldrCoor_U16[COORD_X] = EyeNode.keypoints()[0].points[LEFT_SHLDR].x;        
        leftArmShldrCoor_U16[COORD_Y] = EyeNode.keypoints()[0].points[LEFT_SHLDR].y; 
        leftArmElbCoor_U16[COORD_X] = EyeNode.keypoints()[0].points[LEFT_ELB].x; 
        leftArmElbCoor_U16[COORD_Y] = EyeNode.keypoints()[0].points[LEFT_ELB].y;          
        leftArmWrCoor_U16[COORD_X] = EyeNode.keypoints()[0].points[LEFT_WR].x;
        leftArmWrCoor_U16[COORD_Y] = EyeNode.keypoints()[0].points[LEFT_WR].y;
        estimateBodyPoseData();
      }
  }
}
/*------------------------------------------------
// Estimate distances and angles
// Description: Process data captured from sensor to get distances and angles
//-----------------------------------------------*/
static void estimateBodyPoseData(){ 
  double disRightShldrElb = 0.0;
  double disRightShldrWr = 0.0;
  double disLefttShldrElb = 0.0;
  double disLefttShldrWr = 0.0;
  double angRightShldrElb = 0.0;
  double angRightShldrWr = 0.0;
  double angLefttShldrElb = 0.0;
  double angLefttShldrWr = 0.0;
  int16_t tempOriginCompCoorX_S16 = 0;   // Auxiliar to move origin in X
  int16_t tempOrigiCompCoorY_S16 = 0;    // Auxiliar to move origin in Y
  //Calculation for Shoulder to Elbow RA
  tempOriginCompCoorX_S16 = rightArmElbCoor_U16[COORD_X] - rightArmShldrCoor_U16[COORD_X];
  tempOrigiCompCoorY_S16 = rightArmElbCoor_U16[COORD_Y] - rightArmShldrCoor_U16[COORD_Y];
  disRightShldrElb = sqrt((tempOriginCompCoorX_S16 * tempOriginCompCoorX_S16)+(tempOrigiCompCoorY_S16 * tempOrigiCompCoorY_S16));
  angRightShldrElb = atan2(tempOrigiCompCoorY_S16,tempOriginCompCoorX_S16) * (180/M_PI);
  //Calculation for Shoulder to Wrist RA
  tempOriginCompCoorX_S16 = rightArmWrCoor_U16[COORD_X] - rightArmShldrCoor_U16[COORD_X];
  tempOrigiCompCoorY_S16 = rightArmWrCoor_U16[COORD_Y] - rightArmShldrCoor_U16[COORD_Y];
  disRightShldrWr = sqrt((tempOriginCompCoorX_S16 * tempOriginCompCoorX_S16)+(tempOrigiCompCoorY_S16 * tempOrigiCompCoorY_S16));
  angRightShldrWr = atan2(tempOrigiCompCoorY_S16,tempOriginCompCoorX_S16)*(180/M_PI);
  //Calculation for Shoulder to Elbow LA
  tempOriginCompCoorX_S16 = leftArmElbCoor_U16[COORD_X] - leftArmShldrCoor_U16[COORD_X];
  tempOrigiCompCoorY_S16 = leftArmElbCoor_U16[COORD_Y] - leftArmShldrCoor_U16[COORD_Y];
  disLefttShldrElb = sqrt((tempOriginCompCoorX_S16 * tempOriginCompCoorX_S16)+(tempOrigiCompCoorY_S16 * tempOrigiCompCoorY_S16));
  angLefttShldrElb = atan2(tempOrigiCompCoorY_S16,tempOriginCompCoorX_S16) * (180/M_PI);
   //Calculation for Shoulder to Wrist LA
  tempOriginCompCoorX_S16 = leftArmWrCoor_U16[COORD_X] - leftArmShldrCoor_U16[COORD_X];
  tempOrigiCompCoorY_S16 = leftArmWrCoor_U16[COORD_Y] - leftArmShldrCoor_U16[COORD_Y];
  disLefttShldrWr = sqrt((tempOriginCompCoorX_S16 * tempOriginCompCoorX_S16)+(tempOrigiCompCoorY_S16 * tempOrigiCompCoorY_S16));
  angLefttShldrWr = atan2(tempOrigiCompCoorY_S16,tempOriginCompCoorX_S16) *(180/M_PI);

  if (initPoseEvalStatus_B == false){
    righArmShldrElbDis_rU16 = (int) disRightShldrElb;
    righArmShldrWrDis_rU16  = (int) disRightShldrWr;
    leftArmShldrElbDis_rU16 = (int) disLefttShldrElb;
    leftArmShldrWrDis_rU16 =  (int) disLefttShldrWr;
    initPoseEvalStatus_B = true; 
  }
  else
  {
    righArmShldrElbDis_U16 = (int) disRightShldrElb;
    righArmShldrWrDis_U16  = (int) disRightShldrWr;
    leftArmShldrElbDis_U16 = (int) disLefttShldrElb;
    leftArmShldrWrDis_U16 =  (int) disLefttShldrWr;

    righArmShldrElbAng_S16 = (int) angRightShldrElb;
    righArmShldrWrAng_S16 = (int) angRightShldrWr;
    leftArmShldrElbAng_S16 = (int) angLefttShldrElb;
    leftArmShldrWrAng_S16 = (int) angLefttShldrWr;
  }
}
/*------------------------------------------------
// Setup Function
// Description: This setup all modules at init stage
//-----------------------------------------------*/
void setup()
{
  Serial.begin(9600); // Setup Serial Bus Line
  EyeNode.begin(); // This starts EyeNode Module which has a preload model.
  Log.info("Init Program");
  //create BLE service and characteristic
  mydevice.init(DEVICE_NAME);
  if (!mydevice.getInitialized()){
    Log.error("Modem is not available. Execution stops, enter Safe Mode");
    while(1);// add a routine to go to sleep 
  }
  server_p = mydevice.createServer();
  server_p->setCallbacks(new serverCallback());
  service_p = server_p->createService(SERVICE_UUID); ///This create the service
  bleRightArmChar_p = service_p->createCharacteristic(RIGHTARMSTATE_UUID, BLECharacteristic::PROPERTY_NOTIFY | BLECharacteristic::PROPERTY_READ);
  bleLeftArmChar_p = service_p->createCharacteristic(LEFTARMSTATE_UUID, BLECharacteristic::PROPERTY_NOTIFY | BLECharacteristic::PROPERTY_READ);
  service_p->start();
  advertizing_p = mydevice.getAdvertising();
  advertizing_p->addServiceUUID(SERVICE_UUID);
  advertizing_p->setScanResponse(true);
  advertizing_p->setMinPreferred(0x06);  
  advertizing_p->setMinPreferred(0x12);
  mydevice.startAdvertising();                                  //BLE is ready to be advertized
  Log.info("Waiting connection..");

}
/*------------------------------------------------
// Main function 
// Description: Main
//-----------------------------------------------*/
void loop()
{
  captureBodyPoseData();
  if (initPoseEvalStatus_B ==  true)
  {
     inferBodyPose();
     updateBodyPose();
  }
  delay(300);
}

EyeNode_header

C/C++
The header of EyeNode
#ifndef EYENODE_CONST_H
#define EYENODE_CONST_H

/*------------------------------------------------
// Definitions for BLE services
//-----------------------------------------------*/

#define DEVICE_NAME                "ServNode"
#define SERVICE_UUID               "31e000e0-dfd0-4696-a5e2-3677cec108b4"
#define RIGHTARMSTATE_UUID         "4f367bef-84c9-4f4d-b9e8-7d59f957a47b" //BLE Characteristic for RA State 
#define LEFTARMSTATE_UUID          "e9e9f222-db79-441a-962d-3053daf1842a" //BLE Characteristic for LA State

/*------------------------------------------------
// ENUM for Right Arm Pose Estimation
//-----------------------------------------------*/
enum poseRighArMStage_E{
    IDDLE_R,            // 0: IDLLE Stage arms down
    UPEXTHOR_R,         // 1: Up arm to be extended Horizontally
    UPEXTVER_R,         // 2: 1: Up arm to be extended all way up (Vertically)
};

/*------------------------------------------------
// ENUM for Left Arm Pose Estimation
//-----------------------------------------------*/
enum poseLeftArMStage_E{
    IDDLE_L,            // 0: IDLLE Stage arms down
    UPEXTHOR_L,         // 1: Up arm to be extended Horizontally
    UPEXTVER_L,         // 2: 1: Up arm to be extended all way up (Vertically)
};
/*------------------------------------------------
// ENUM for Body Posture Keypoints
//-----------------------------------------------*/
enum bodyKeypoints_E{
    NOSE,               // 0: Nose
    LEFT_EYE,           // 1: Eye Left 
    RIGHT_EYE,          // 2: Eye Right 
    LEFT_EAR,           // 3: Ear Left
    RIGHT_EAR,          // 4: Ear Right
    LEFT_SHLDR,         // 5: Shoulder Left
    RIGHT_SHLDR,        // 6: Shoulder Right
    LEFT_ELB,           // 7: Elbow Left
    RIGHT_ELB,          // 8: Elbow Right
    LEFT_WR,            // 9: Wrist Left
    RIGHT_WR,           // 10: Wrist Right
    LEFT_HIP,           // 11: Hip Left
    RIGHT_HIP,          // 12: Hip Right
    LEFT_KNEE,          // 13: Knee Left
    RIGHT_KNEE,         // 14: Knee Right
    LEFT_ANKLE,         // 15: Ankle Left
    RIGHT_ANKLE,        // 16: Ankle Right
};
/*------------------------------------------------
// ENUM for accessing coordinates X and Y
//-----------------------------------------------*/
enum coordinates_E{
    COORD_X,               // 0: Coordinate in X
    COORD_Y,               // 1: Coordinate in y
};

#endif

Left_Arm_Bracelet _cpp

C/C++
This is the code for the Left Bracelet.
#include "BLEDevice.h"
#include "MiniLog.h"
#include "ArmNodeLeft_Const.h"
#include <I2Cdev.h>
#include "MPU6050_6Axis_MotionApps20.h"
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    #include "Wire.h"
#endif

/*------------------------------------------------
// Objects and Interfaces
//-----------------------------------------------*/
MPU6050 sensor;
MiniLog Log(1);
BLEDevice mydevice;
static BLEScan *bleScanning_p;
static BLEAddress *bleAddress_p;
static BLEClient *bleClient_p;
static BLERemoteService *bleRemoteService_p;
static BLERemoteCharacteristic *bleRemoteChar_p;

/*------------------------------------------------
// Globals
//-----------------------------------------------*/
static bool newDataAck_B = false;
static uint8_t newBLEData_U08 = IDLE_L;
static uint8_t oldBLEData_U08 = IDLE_L;
static bool connectionAck_B = false;
static uint8_t fifoBuffer_aU8[64];               //FIFO Buffer for calculating angles base on MPU
static float outputVector_a32[3];                //vector from MPU that conatins angles
Quaternion q;           // quaternion 
VectorFloat gravity;    // gravity vector
/*------------------------------------------------
// Class for BLE to look if the device is connected to the correct server
//-----------------------------------------------*/
class clientCallback: public BLEAdvertisedDeviceCallbacks {
  void onResult(BLEAdvertisedDevice advertisedDevice) {
    if (advertisedDevice.getName() == SERVER_NAME) 
    { 
      advertisedDevice.getScan()->stop(); 
      bleAddress_p = new BLEAddress(advertisedDevice.getAddress()); 
      Log.info("Server Found");
    }
  }
};
/*------------------------------------------------
// Index of fucntions
//-----------------------------------------------*/
static void initMotorSequence();
static void initMotorSequenceBack();
static void serverDataCallback(BLERemoteCharacteristic *rCharacteristic_p, uint8_t* rData_p, size_t length, bool isNotify_B);
static bool connectServer();
static void startComms2Server();
static void getSensorData();
static void hapticActivation();
static void activateMotors(uint8_t motorNumber, int16_t minValue_S16, int16_t maxValue_S16);
/*------------------------------------------------
// Motor  
// *Description: Calculating the angles based on MPU data
//-----------------------------------------------*/
static void activateMotors(uint8_t motorNumber, int16_t minValue_S16, int16_t maxValue_S16){
  if (motorNumber == 0) {digitalWrite(INNER_M, HIGH);}
  if (motorNumber == 1) {digitalWrite(OUTTER_M, HIGH);}
  if (motorNumber == 2) {digitalWrite(FRONT_M, HIGH);}
  if (motorNumber == 3) {digitalWrite(BACK_M, HIGH);}
  while(1)
  {
    getSensorData();
    if (minValue_S16 <=outputVector_a32[1] && outputVector_a32[1]<= maxValue_S16) break;
    delay(200);
  }
  digitalWrite(BACK_M, LOW);
  digitalWrite(OUTTER_M, LOW);
  digitalWrite(FRONT_M, LOW);
  digitalWrite(INNER_M, LOW);

}
/*------------------------------------------------
// MPU Angles 
// *Description: Calculating the angles based on MPU data
//-----------------------------------------------*/
static void getSensorData (){
  float rawDatavector_a32[3];
  if (sensor.dmpGetCurrentFIFOPacket(fifoBuffer_aU8)){
    sensor.dmpGetQuaternion(&q, fifoBuffer_aU8);
    sensor.dmpGetGravity(&gravity, &q);
    sensor.dmpGetYawPitchRoll(rawDatavector_a32, &q, &gravity);
    outputVector_a32[0] = rawDatavector_a32[0] * 180/M_PI; // 180/PI to avoid sending decimals due to an issue on client
    outputVector_a32[1] = rawDatavector_a32[1] * 180/M_PI;
    outputVector_a32[2] = rawDatavector_a32[2] * 180/M_PI;
  }
  else{
    outputVector_a32[0] = 0.0;
    outputVector_a32[1] = 0.0;
    outputVector_a32[2] = 0.0;
    Log.error("Sensor Data Corrupted");
  }
}
/*------------------------------------------------
// State machine to actuivate haptic motor 
// *Description: State machine to select the correct activation of motors
//-----------------------------------------------*/
static void hapticActivation()
{
  if (oldBLEData_U08 == IDLE_L)
  {
    if (newBLEData_U08 == UPEXTHOR_L) {activateMotors(1,-25,-21);}
    if (newBLEData_U08 == UPEXTVER_L) {activateMotors(1,-53,-50);}
  }
  if (oldBLEData_U08 == UPEXTHOR_L)
  {
    if (newBLEData_U08 == IDLE_L) {activateMotors(0,0,5);}
    if (newBLEData_U08 == UPEXTVER_L) {activateMotors(1,-54,-50);}
  }
  if (oldBLEData_U08 == UPEXTVER_L)
  {
    if (newBLEData_U08 == IDLE_L) {activateMotors(0,0,5);}
    if (newBLEData_U08 == UPEXTHOR_L) {activateMotors(0,-25,-21);}
  }
}

/*------------------------------------------------
// Establish connections to BLE Server
// *Description: This will serve to scan BLE devices, find the server and connect to it.
//-----------------------------------------------*/
static void startComms2Server()
{
  bleScanning_p = mydevice.getScan();
  bleScanning_p->setAdvertisedDeviceCallbacks(new clientCallback());
  bleScanning_p->setActiveScan(true);
  bleScanning_p->start(30);
  connectionAck_B = connectServer();
  if (connectionAck_B == true)
  {
    Log.info("Connected to Server");
  }
  else
  {
    Log.error("Server offline");
  }
}
/*------------------------------------------------
// Handler to read BLE characteristic
// *Description: This will get the BLE characteristic value
//-----------------------------------------------*/
static void serverDataCallback(BLERemoteCharacteristic *rCharacteristic_p, uint8_t* rData_p, size_t length, bool isNotify_B)
{
    newDataAck_B = true;
    if(*rData_p == '0') newBLEData_U08 = IDLE_L;
    else if(*rData_p == '1') newBLEData_U08 = UPEXTHOR_L;
    else if(*rData_p == '2') newBLEData_U08 = UPEXTVER_L;
    else newBLEData_U08 = IDLE_L;                         //if any corrupted data go to safety position
}
/*------------------------------------------------
// Handler to connect to server 
// *Description: This function is charge to connect to server once it is found
//-----------------------------------------------*/
bool connectServer(){
  bleClient_p = mydevice.createClient();
  bleClient_p -> connect(*bleAddress_p);
  bleRemoteService_p = bleClient_p->getService(SERVICE_UUID); //Check references for service availability
  if(bleRemoteService_p == nullptr)
  {
    Log.error("Service unavailable");
    return false;
  }
  bleRemoteChar_p = bleRemoteService_p-> getCharacteristic(LEFTARMSTATE_UUID);
  if(bleRemoteChar_p == nullptr)
  {
    Log.error("Service characteristics unavailable");
    return false;
  }
  bleRemoteChar_p->registerForNotify(serverDataCallback);
  return true;
}
/*------------------------------------------------
// Funny sequence to alert user that the program ends calibrations
// *Description: turn on sequentially all motor
//-----------------------------------------------*/
static void initMotorSequenceBack()
{
  digitalWrite(BACK_M, HIGH);
  delay(700);
  digitalWrite(BACK_M, LOW);
  delay(100);
  digitalWrite(OUTTER_M, HIGH);
  delay(700);
  digitalWrite(OUTTER_M, LOW);
  delay(100);
  digitalWrite(FRONT_M, HIGH);
  delay(700);
  digitalWrite(FRONT_M, LOW);
  delay(100);
  digitalWrite(INNER_M, HIGH);
  delay(700);
  digitalWrite(INNER_M, LOW);
}
/*------------------------------------------------
// Funny sequence to alert user that the program starts
// *Description: turn on sequentially all motor
//-----------------------------------------------*/
static void initMotorSequence()
{
  digitalWrite(INNER_M, HIGH);
  delay(700);
  digitalWrite(INNER_M, LOW);
  delay(100);
  digitalWrite(FRONT_M, HIGH);
  delay(700);
  digitalWrite(FRONT_M, LOW);
  delay(100);
  digitalWrite(OUTTER_M, HIGH);
  delay(700);
  digitalWrite(OUTTER_M, LOW);
  delay(100);
  digitalWrite(BACK_M, HIGH);
  delay(700);
  digitalWrite(BACK_M, LOW);
}
/*------------------------------------------------
// Setup Function
// Description: This setup all modules at init stage
//-----------------------------------------------*/
void setup() {
  uint8_t sensorStatus_U08;          // MPU6050 status after initilize
  Serial.begin(9600);                //Debugging purposes

  #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
        Wire.begin();
  #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
      Fastwire::setup(400, true);
  #endif

  //Config haptic motor I/O
  pinMode(INNER_M, OUTPUT);
  pinMode(OUTTER_M, OUTPUT);
  pinMode(FRONT_M, OUTPUT);
  pinMode(BACK_M, OUTPUT);
  initMotorSequence();           //init sequence for getting user to iddle stage
  delay(1000);
  //Init MPU6050 
  sensor.initialize();    //Iniciando el sensor
  if (!sensor.testConnection()){
    Log.error("Sensor is not responding, Execution stops, enter Safe Mode"); //TODO: routine for safe mode
    while(1); // Need to reset device
  }
  sensorStatus_U08 = sensor.dmpInitialize();
  //Setting offset values to move reference Frame
  //This values are for my chips, please calibrate yours once to see what values are good! Use example from MPU lib to do so.
  sensor.setXGyroOffset(63);
  sensor.setYGyroOffset(-1);
  sensor.setZGyroOffset(33);
  sensor.setZAccelOffset(1332); 

  if (sensorStatus_U08 == 0){
    sensor.CalibrateAccel(6);
    sensor.CalibrateGyro(6);
    sensor.setDMPEnabled(true);
    Log.info("Sensor ready to action!");
  }
  else {
    Log.error("Sensor is failing setting up offsets");
    while(1); // Need to reset the device  
  }
  initMotorSequenceBack();
  //BLE Sequence
  mydevice.init(DEVICE_NAME);
  if (!mydevice.getInitialized()){
    Log.error("Modem is not available. Execution stops, enter Safe Mode");
    while(1); // Need to reset the device 
  }
  startComms2Server();
}
/*------------------------------------------------
// Main function 
// Description: Main
//-----------------------------------------------*/
void loop() {
  if(connectionAck_B == true){
    if(newDataAck_B == true){
      if(newBLEData_U08 != oldBLEData_U08){
        Log.debug("Data received");
        Serial.printf("%d", newBLEData_U08);
        newDataAck_B = false;
        hapticActivation();
        oldBLEData_U08=newBLEData_U08;
      }
    }
  }
}

Right_Arm_Bracelet _header

C/C++
The header of right bracelet
#ifndef ARMNODERIGHT_CONST_H
#define ARMNODERIGHT_CONST_H

/*------------------------------------------------
// PIN ALLOCATION FOR MOTOR ON WRIST 
//-----------------------------------------------*/
#define INNER_M D2         //INNER MOTOR CONNECTED TO D2
#define OUTTER_M D1       //OUTTER MOTOR CONNECTED TO D10
#define FRONT_M D10         //FRONT MOTOR CONNECTED TO D1
#define BACK_M D3          //BACK MOTOR CONNECTED TO D3
/*------------------------------------------------
// BLE Service/Characteristic/Names
//-----------------------------------------------*/
#define SERVER_NAME                "ServNode"
#define DEVICE_NAME                "RightArmNode"
#define SERVICE_UUID               "31e000e0-dfd0-4696-a5e2-3677cec108b4"
#define RIGHTARMSTATE_UUID         "e9e9f222-db79-441a-962d-3053daf1842a" 

/*------------------------------------------------
// ENUM for Right Arm Pose Estimation
//-----------------------------------------------*/
enum poseRighArMStage_E{
    IDLE_R,             // 0: IDLE Stage arms down
    UPEXTHOR_R,         // 1: Up arm to be extended Horizontally
    UPEXTVER_R,         // 2: Up arm to be extended all way up (Vertically)
};


#endif

Left_Arm_Bracelet _header

C/C++
The header of Left bracelet
#ifndef ARMNODELEFT_CONST_H
#define ARMNODELEFT_CONST_H

/*------------------------------------------------
// PIN ALLOCATION FOR MOTOR ON WRIST 
//-----------------------------------------------*/
#define INNER_M D2         //INNER MOTOR CONNECTED TO D2
#define OUTTER_M D1       //OUTTER MOTOR CONNECTED TO D10
#define FRONT_M D3         //FRONT MOTOR CONNECTED TO D1
#define BACK_M D10          //BACK MOTOR CONNECTED TO D3
/*------------------------------------------------
// BLE Service/Characteristic/Names
//-----------------------------------------------*/
#define SERVER_NAME                "ServNode"
#define DEVICE_NAME                "LeftArmNode"
#define SERVICE_UUID               "31e000e0-dfd0-4696-a5e2-3677cec108b4"
#define LEFTARMSTATE_UUID          "4f367bef-84c9-4f4d-b9e8-7d59f957a47b" 

/*------------------------------------------------
// ENUM for Right Arm Pose Estimation
//-----------------------------------------------*/
enum poseLeftArMStage_E{
    IDLE_L,             // 0: IDLE Stage arms down
    UPEXTHOR_L,         // 1: Up arm to be extended Horizontally
    UPEXTVER_L,         // 2: Up arm to be extended all way up (Vertically)
};


#endif

Credits

Uriel Cedeño Antunez

Uriel Cedeño Antunez

1 project • 1 follower
I am an embedded enginner who has worked on education, automotive and medical applications designing and developing HW and SW.

Comments

Awards
Honorable Mention Visual Impairments
Build2gether 2.0 — Inclusive Innovation Challenge
Related channels and tags