 # 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
********************************************************/

#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()
{
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
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)