Untitled

// Include necessary libraries
#include <Wire.h>
#include <Adafruit_INA219.h>

Adafruit_INA219 ina219;

// Define the pins for haptic motors
#define HAPTIC_PIN_1 A0
#define HAPTIC_PIN_2 A1
// You can define more haptic pins here...

// Define maximum current draw (in mA) by the prosthetic motor
#define MAX_CURRENT_DRAW 1000

void setup(void) 
{
  Wire.begin();
  
  // Initialize the INA219.
  if (! ina219.begin()) {
    Serial.println("Failed to find INA219 chip");
    while (1) { delay(10); }
  }
  // Configure the INA219 chip
  ina219.setCalibration_16V_400mA();
  
  // Initialise the haptic motor pin as output
  pinMode(HAPTIC_PIN_1, OUTPUT);
  pinMode(HAPTIC_PIN_2, OUTPUT);
  // Initialise more haptic pins here if necessary...
}

void loop(void) 
{
  float current_mA = 0;
  
  // Measure the current
  current_mA = ina219.getCurrent_mA();
  
  // Convert the current to a haptic feedback level
  int hapticLevel = convertToHaptic(current_mA);
  
  // Output the haptic feedback
  analogWrite(HAPTIC_PIN_1, hapticLevel);
  analogWrite(HAPTIC_PIN_2, hapticLevel);
  // Output to more haptic pins here if necessary...
  
  delay(100);
}

int convertToHaptic(float current_mA) 
{
  // Linear mapping
  int hapticLevel = mapLinear(current_mA, 0, MAX_CURRENT_DRAW, 0, 255);
  
  // Uncomment the following line for a non-linear mapping
  // int hapticLevel = mapCurved(current_mA, 0, MAX_CURRENT_DRAW, 0, 255);
  
  // Constrain the haptic level to a value between 0 and 255
  hapticLevel = constrain(hapticLevel, 0, 255);
  
  return hapticLevel;
}

// Linear mapping
int mapLinear(float x, float in_min, float in_max, float out_min, float out_max) 
{
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

// Non-linear mapping (currently a simple piecewise linear approximation, replace with the correct function if available)
int mapCurved(float x, float in_min, float in_max, float out_min, float out_max) 
{
  // This is just a placeholder. Replace with the correct function if available.
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}