Grip force control of a robotic gripper driven by a DC motor

I designed a simple two-finger robotic gripper using a stepper motor. Now I want to attach a three-axial force sensor on one gripper finger, and control the grip force when an object is grasped.

The measurement range of the force sensor is: 0-80N for Fx and Fy, and 0-240N for Fz, which are all converted to 0-5V voltage as analog output. Since the outputs are usually small (around 0.01V) in the case of my experiment, amplifiers are used to amplify three channels of force signals. I am considering using PID control. The Arduino continuously reads the force signal and calculates the target force based on tangential force and coefficient of friction.

However, as I am new to all the stuff, especially programming, I do need help on how to write the code. I have learned a bit from some example codes, and written a (wrong) script myself. Can someone help me? Thanks a lot.

/********************************************************
 * PID control of a DC motor based on force feedback from ATI sensor
 * Reading analog input Fx,Fy,Fz to control analog PWM output ENA
 ********************************************************/

#include <PID_v1.h>

// Set pin numbers
#define PIN_Fx A0
#define PIN_Fy A1
#define PIN_Fz A2
#define IN1 7
#define IN2 8
#define ENA 6

// Define variables
double Setpoint, Input, Output;
double Fx, Fy, Fz, Ft, Fn;
int Analog_Fx, Analog_Fy, Analog_Fz;

// Amplification factors
const int Amp_Fx = 10;
const int Amp_Fy = 10;
const int Amp_Fz = 20;

const int precontactSpeed = 150;

// ATI force sensor maps 0-80N to 0-5V for Fx&Fy, and 0-240N to 0-5V for Fz
const double Fx2Analog = 1024*Amp_Fx/80;
const double Fy2Analog = 1024*Amp_Fy/80;
const double Fz2Analog = 1024*Amp_Fz/240;
const double COF = 0.25; // coefficient of friction
const double SMF = 1.1; // safety margin factor

// Specify the links and initial tuning parameters
double Kp = 2, Ki = 5, Kd = 1;
PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);

void setup()
{   
  pinMode(ENA, OUTPUT);
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  
  // Set initial rotation direction to be forward
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, HIGH);
  
  // Turn the PID on
  myPID.SetMode(AUTOMATIC);
  myPID.SetSampleTime(1);
  myPID.SetOutputLimits(-255, 255);
  Input = 0; Output = 0; Setpoint = 0;
  
  // Initialize the serial port
  Serial.begin(9600);
}

void loop()
{
  Analog_Fz = analogRead(PIN_Fz);
  Fz = Analog_Fz/Fz2Analog; // convert analog value to force in Newtons
  
  // Close the gripper when grip force is smaller than 0.5N
  if (Fz < 0.5) {
    analogWrite(ENA, precontactSpeed);
  }
  else {
    // Stop the motor
    digitalWrite(IN1, LOW);
    digitalWrite(IN2, LOW);
    
    // Read forces and set target 
    Analog_Fx = analogRead(PIN_Fx);
    Analog_Fy = analogRead(PIN_Fy);
    Fx = Analog_Fx/Fx2Analog;
    Fy = Analog_Fy/Fy2Analog;
    Input = Fz;
    Ft = sqrt(sq(Fx)+sq(Fy)); // calculate tangential force
    Fn = Ft/COF*SMF;    
    Setpoint = Fn;
    
    // PID control will not be activated when setpoint is smaller than 0.5N
    if (Setpoint > 0.5) {
      myPID.Compute();
      if (Output > 0){
        digitalWrite(IN1, LOW);
        digitalWrite(IN2, HIGH);
        analogWrite(ENA, Output);
      }
      else {
        digitalWrite(IN1, HIGH);
        digitalWrite(IN2, LOW);
        analogWrite(ENA, abs(Output));
      }
    }     
  }
}

Gripper_Force_Control.ino (2.46 KB)