Published September 5, 2024 © Apache-2.0

Haptic Movement Guidance

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

IntermediateFull instructions provided20 hours20

Honorable Mention Visual Impairments

Build2gether 2.0 — Inclusive Innovation Challenge

Things used in this project

Hardware components

Seeed Studio XIAO BLE

Seeed Studio XIAO ESP32S3 Sense

Seeed Studio Grove Vision AI module v2 kit

General Purpose Transistor NPN

Resistor 221 ohm

Capacitor 100 nF

Resistor 221k ohm

Brown Dog Gadgets Solar Cockroach Vibrating Disc Motor

Li-Ion Battery 1000mAh

Hand tools and fabrication machines

Mini Side Cutter, 120mm Length with 25mm Jaw Capacity

Soldering iron (generic)

Plier, Side Cutting

Story

Around 2.2 billion people worldwide are struggling with some degree of visual impairment. This condition can deteriorate the quality of life to some extent. Mainly, it affects the ability to move due to the risk of losing body control caused by a lack of spatial information. This detriment leads to other damaging factors, such as social isolation and depression.

Different organizations have created programs to implement recreational activities, such as dancing and exercising, in squares and public parks. Their mission is to develop communities that can tackle social and health problems. These programs can open opportunities for visually impaired people to be integrated by providing spaces without discrimination and emotionally supportive networks. In most programs, the performed activities require participants to mimic the movements of an instructor, a situation that is complex for visually impaired people. Although instruments such as guide sticks exist to help them get spatial information, they cannot be included in activities like dancing or exercising.

This project aims to develop a prototype for outdoor community activities capable of detecting arm postures with a camera and activating vibro-motors to guide visually impaired people in accomplishing specific movements.

Design Posture Recognition System

The combination of computer vision and artificial intelligence algorithms have allowed tiny electronics devices, such as embedded devices, to detect, infer or predict behaviors. These capabilities arise from a series of techniques that use large computational extensive intelligence models to transfer the "knowledge" to small devices. This transfer reduces the performance of the inference, but increases the number of applications of such models.

This project used a large computational extensive intelligence model from Ultralytics (YOLOv8) to train a Grove Vision AI v2 module to detect keypoints of the Human Body in motion. It utilized the information provided by Seeed Studio on the following web page (YOLOV8 wiki) to get the YOLO v8 model and adapt it to the Grove module using Google Colabs. Once the model was compressed by TensorFlow lite, it was deployed using the tool SenseCraft by SeeedStudios, depicted on Image 1.

Image 1. Model deployment on Grove module.

From the image, it can be noticed that the keypoints (kp) are joined by lines of different colors for illustrative purposes. These kp were processed on a ESP32 module (XIAO ESP32S3). Thus, the ESP32S3 extracted the kp from Grove Module using the library SSCMA for Arduino Platform. According to the information provided by Ultralytics the arms occupied the positions on the array of keypoints as follows.

5: Shoulder Left
6: Shoulder Right
7: Elbow Left
8: Elbow Right
9: Wrist Left
10: Wrist Right

The kp were processed to determine the arms postures by analyzing the kinematic chain from shoulders to wrists of both arms. This analysis consisted of decomposing each individual chain, depicted on the Image 2. Then, it was calculated the distances between shoulders-elbows and shoulders-wrists and angles between these body sections. This decomposition was performed by setting origin-framework to the shoulders and calculated all values against Elbows and Wrists points with each independent arm, as observed in the Image 3.

Image 2. Kinematic chains of arms.

Image 3. Decomposition of arms chains.

The values calculated were analyzed to get the footprint of movements in order to classify them. This project centered the analysis on four possible movements (postures) with one standby position (Idle), observed on Image 4. The perspective to determine the name of the arms was the point of view of the executor of the movement.

Image 4. Postures classification.

In order to find the characteristics that the four movements comprises, it was conducted an experiment where angles and distances were captured by module Grove and ESP32S3. The both components (EyeNode) were set 2 meters apart from the target (person who performed the movement) and 1 meter from ground. Besides, the EyeNode was mounted on a tripod inside a cage, observed in the Image 5.

Image 5. EyeNode

The Eyenode was designed to allow the components to be battery powered using the ESP32S3 builtin circuits; the schematics of the EyeNode are attached. Additionally, It was programmed for the experiment to initiate capturing the information after 3 minutes passed after initialization. Then, values were sent to the computer to be analyzed statistically by Bluetooth Low Energy (BLE). The EyeNode was configured as a server, and it was programmed XIAO ESP32C3 module to function as a testNode (BLE client) to receive the data.

The information obtained by the experiment was condensed in the following tables where it is observed the average, the minimum value, maximum value, the standard deviation and the range.

Table 1. Distances at Idle.

Table 2. Distances at up arms horizontally.

Table 3. Distances at up arms vertically.

Table 4. Angles at Idle.

Table 5. Angles at arms up horizontally

Table 5. Angles at arms up vertically

From tables, it can be observed that there is variability in the angles for all the Postures/Stages selected that the postures can be classified. For instance, right arm shoulder-wrist angle is different at any posture at idle the range starts on 95° to 101°, at Up Horizontal starts on 162° to 170°, and at Up Vertical starts on -119° to -102°. Additionally, it is noticed the model struggles with segments regarding left arms. The values of this segment had bigger values for the standard deviation, which indicates a bigger dispersion of data. However, the ranges are well defined and do not overlap so that postures can be differentiated.

The ranges from tables were used to classified the postures on Image 4 by creating a state machine that depended on the angles ratter than distances. Finally, the EyeNode was programmed with the this state machine and BLE capabilities. Thus, the EyeNode would send information to the haptic devices to alert the visually impaired people which movement needs to be performed. To test it, an experiment was developed and conducted with a testingNode (BLE Client) to receive the posture observed through the EyeNode.

The code that EyeNode used is depicted in the section below. For this project, a tiny Log library was developed for debugging purposes (attached at the bottom). Besides, It was added an open source library for controlling the BLE module integrated in ESP32 chips.

Design Haptic Feedback

Haptics is a series of techniques which aims to stimulate the sense of touch by simulating a motor feedback. This feedback serves to guide people to perform activities or as an indicator that an activity was performed correctly; like when an icon on a touchscreen is selected and the device vibrates to inform the correct selection. Since haptic feedback helps to alert people about a situation, it can be used to guide visually impaired people who rely on touch and sound senses.

This project uses haptic feedback to indicate to visually impaired people the movement they need to execute with their arms to participate in outdoor community activities such as exercising or dancing. Thus, bracelets were developed to have vibro-motors attached and bring feedback to the wrist as indicators of movement.

The bracelets were designed based on the XIAO ESP32C3 module which would activate the vibro motors based on the receiving information from the EyeNode. Additionally, Inertial Measurement Unit MPU6050 was added to close the control loop of arms. The sensor provided spatial information about the movement, and the devices are battery powered. In the Image 6, it is observed the bracelet electronic prototype, where the battery is located under the MPU.

Image 6. Bracelet circuit

The MPU provides acceleration and gyroscoping information. This information can be used to classify movement more than detect the accurate position of the sensor. Considering that the arms are fixed kinematics chains, it is possible to use the same methodology as EyeNode to get values of angles that were reached with the postures. In order to get the sensor data related to angles, it was conducted an experiment where bracelet circuits were mounted on a structure to be attached to arms (see Image 7) to get values of angles (Pitch, Yaw, and Roll) characteristics of the postures on Image 4.

Image 7. Bracelet circuit attached to the arm

For the experiment, the bracelets were programmed to reset the offsets to have a local framework set on the MPU start position. This constrained the measurements and reduced the complexity of calculating the angles against an absolute framework. The information obtained by bracelets was sent to a testingNode through BLE for its statistical analysis. In the following tables a summary of the data obtained is shown.

Table 7. Angles MPU right arm at idle

Table 8. Angles MPU right arm at up horizontally

Table 9. Angles MPU right arm at up vectically

Table 10. Angles MPU left arm at idle

Table 11. Angles MPU left arm at up horizontally

Table 12. Angles MPU left arm at up vertically

As well as the data obtained in the experiment with the EyeNode, the angles varied on a range that could be clustered. Such a situation allowed postures classification; being the Yaw angle the most relevant. This angle is obtained by a rotation around the Y axis. According to the MPU indication and the configuration of the bracelet, the Y axis is perpendicular to the image shown on Image 8. Therefore, the Yaw angle had a strong relation to the desired postures on Image 4 so that it was utilized to classify the postures, using the ranges from Tables 7-12.

Image 8. Axis orientation of MPU.

In order to create the haptic feedback, four vibro-motors were utilized per wrist. Each motor was controlled by a transistor BJT (BC547), using the configuration on Image 9. Since the objective was to activate feedback until a person reaches the desired posture.

Image 9. Control unit of vibro-motor.

The vibro-motors were located at four directions around wrist in a soft estructure ( haptic feedback wrist belt) as shown in Image 10. They were named according to their position using the anatomical convention.

Dorsal - Outer Motors (OM)
Radial - Front Motors (FM)
Palmar- Inner Motor (IM)
Ulnar - Back Motor (BM)

Image 10 Haptic feedback wrist belt

The Image 11 shows how both bracelets were attached to the arms with the haptic feedback wrist belt.

Image 11.. Bracelet nodes both arms

Finally, the bracelets were programmed as BLE clients to receive the inference from EyeNode with respect to which pose needs to be addressed. This information activates one motor at a time from haptic feedback wrist belt to indicate user to move the arms until reaching the desired posture. In order to test it, it was conducted an experiment with a testingNode as a Server which would transmit randomly postures to bracelets. This experiment was conducted with one bracelet at the time.

The activation sequence of the bracelets started with a routine that trained the user how the feedback was feeling to be ready to play. During this sequence the bracelets were running a background process of calibrating the sensor and initiating the communication with the server.

Results

In the following short videos, it is observed the fucntionality of EyeNode and the Bracelets.

Video 1. EyeNode test

Video 2. Bracelet test right arm

Video 2. Bracelet test leftt arm

Special thanks

This project was possible due to the support of BUILD2GHETER 2.0 Admins and all sponsors that provided Hardware modules. Additonally, I will give a big thanks to all people form ETH Zurich that brought me feedback to complete this project succesfully

Code

#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);
}

#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;
      }
    }
  }
}

#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);
}

#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

#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;
      }
    }
  }
}

#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

#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

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

Haptic Movement Guidance

Things used in this project

Hardware components

Hand tools and fabrication machines

Story

Design Posture Recognition System

Design Haptic Feedback

Results

Special thanks

Schematics

AI Eye node

Haptic bracelet

AI Eye node Schematic Image

Haptic bracelet Schematic Image

Code

MiniLog_cpp

MiniLog_header

Right_Arm_Bracelet _cpp

EyeNode_cpp

EyeNode_header

Left_Arm_Bracelet _cpp

Right_Arm_Bracelet _header

Left_Arm_Bracelet _header

Credits

Uriel Cedeño Antunez

Comments

Awards

Embed the widget on your own site

Haptic Movement Guidance

Haptic Movement Guidance

Things used in this project

Hardware components

Hand tools and fabrication machines

Story

Design Posture Recognition System

Design Haptic Feedback

Results

Special thanks

Schematics

AI Eye node

Haptic bracelet

AI Eye node Schematic Image

Haptic bracelet Schematic Image

Code

MiniLog_cpp

MiniLog_header

Right_Arm_Bracelet _cpp

EyeNode_cpp

EyeNode_header

Left_Arm_Bracelet _cpp

Right_Arm_Bracelet _header

Left_Arm_Bracelet _header

Credits

Uriel Cedeño Antunez

Comments

Awards

Related channels and tags