Current Sensing with the ACS712 Hall Effect Sensor

Posted by

After a conversation with JD Warren, one of authors of the excellent Arduino Robotics book, I decided to try the ACS712 hall effect current sensor. I soldered one to a surface mount to breadboard adapters and plugged it in with some capacitor values recommended on the first page of the data sheet. I first just tried to sense the current used by the “shoulder” motor in the robotic arm I’ve posted about before. Here’s a video (it runs way smoother now, promise):

I also hooked up a quick RC low pass filter at about 7 Hz, then I ran the robotic arm (code below) and graphed shoulder angle vs. current, and it looks like this:

The graph shows several cycles of the shoulder angle increasing and decreasing while the end effector draws an imaginary vertical line. And although the current signal is periodic, it’s not exactly clear what’s causing the profile to look the way it does. Also, there is no way the current scaling is right (should be under an Amp the whole time) but I’ve read the data sheet and looked at my code and can’t find the error. Can you?

/*
Robot arm control using hacked servos and a master arm with pots as control in
master-slave system.
+y is vertical, +x is to the right
drawing line/circle:
http://www.circuitsathome.com/mcu/robotic-arm-inverse-kinematics-on-arduino
inverse kinematics:
http://www.micromegacorp.com/downloads/documentation/AN044-Robotic%20Arm.pdf
http://www.learnaboutrobots.com/inverseKinematics.htm
*/

#include <AFMotor.h>
#include <math.h>
#include <PID_v1.h>

// Instantiate both motors
AF_DCMotor shoulder(3);
AF_DCMotor elbow(4);

// DECLARE PINS
int ShoulderPin = A0; // to potentiometer on shoulder motor
int ElbowPin = A1; // to potentiometer on elbow motor
int ShoulderCurrentPin = A2; // to first ACS712 IC

// INITIALIZE CONSTANTS
double Kp_Elbow = 20; // this is the proportional gain
double Kp_Shoulder = 20;
double Ki_Elbow = 0.1; // this is the integral gain
double Ki_Shoulder = 0.1;
double Kd_Elbow = 1.0; // this is the derivative gain
double Kd_Shoulder = 0.75;

double Elbow_neg = 970; // joint limits of robotic arm using right hand rule for sign
double Elbow_pos = 31;
double Shoulder_neg = 210;
double Shoulder_pos = 793;

const double a1 = 198; // shoulder-to-elbow "bone" length from Solidworks (mm)
const double a2 = 220; // elbow-to-wrist "bone" length from Solidworks (mm) - longer c bracket

double highY = 350; // line drawing targets
double lowY = 250;
double constantX = 200;

boolean elbowup = false; // true=elbow up, false=elbow down

float Volt_per_amp = 0.185; // 185 mV/A resolution according to hardware page
 
// VARIABLES
double rawElbowAngle = 0.0; // initialize all angles to 0
double rawShoulderAngle = 0.0;

double elbowAngle = 0.0; // initialize all angles to 0
double shoulderAngle = 0.0;

double theta1 = 0.0; // target angles as determined through inverse kinematics
double theta2 = 0.0;

double actualX = 0.0;
double actualY = 0.0;
double predictedX = 0.0;
double predictedY = 0.0;
double y = 0.0;

double c2 = 0.0; // is btwn -1 and 1
double s2 = 0.0;

double pwmTemp = 0.0;

double pwmElbow = 50.0; // between 0 to 255
double pwmShoulder = 50.0;

float shoulderCurrentRaw; // the raw analogRead ranging from 0-1023
float shoulderCurrentVolts; // raw reading changed to Volts
float shoulderCurrentAmps; // Voltage reading changed to Amps

//Syntax: PID(&Input, &Output, &Setpoint, Kp, Ki, Kd, Direction)
PID elbowPID(&elbowAngle, &pwmElbow, &theta2, Kp_Elbow, Ki_Elbow, Kd_Elbow, DIRECT);
PID shoulderPID(&shoulderAngle, &pwmShoulder, &theta1, Kp_Shoulder, Ki_Shoulder, Kd_Shoulder, DIRECT);

void setup() { 
  Serial.begin(115200); // set up Serial library

  elbowPID.SetSampleTime(5); // (ms) how often new PID calc is performed, default is 1000
  shoulderPID.SetSampleTime(5);

  elbow.setSpeed(pwmElbow); // set the speed to pwmElbow
  shoulder.setSpeed(pwmShoulder); // set the speed to pwmElbow

  elbowPID.SetMode(AUTOMATIC); // modes in the PID library that have to be set
  shoulderPID.SetMode(AUTOMATIC);

  elbowPID.SetOutputLimits(-255,255); // we'll use the sign to tell us motor direction
  shoulderPID.SetOutputLimits(-255,255);

  move_to_start(); //get to starting position
}

void loop() { 
  line_y();
}

void move_to_start() {
  do {
    get_angles(constantX, lowY);
    drive_joints(); // drive joints until actual position equals expected position
  }
  while( abs(theta1 - shoulderAngle) > 2 && abs(theta2 - elbowAngle) > 2 ); // bail when it's close
}

void line_y() {
  for(y = lowY; y < highY; y += 0.5) { // draw straight line up
    get_angles(constantX,y);
    drive_joints();
  }
  for(y = highY; y > lowY; y -= 0.5) { // draw straight line down
    get_angles(constantX, y);
    drive_joints();
  }
}

// Given predicted x,y position at any time (forward kinematics)
void get_xy() {
  predictedX = a1*cos(radians(theta1)) + a2*cos(radians(theta1+theta2));
  predictedY = a1*sin(radians(theta1)) + a2*sin(radians(theta1+theta2));
  actualX = a1*cos(radians(shoulderAngle)) + a2*cos(radians(shoulderAngle+elbowAngle));
  actualY = a1*sin(radians(shoulderAngle)) + a2*sin(radians(shoulderAngle+elbowAngle));
}

// Given target(Px, Py) solve for theta1, theta2 (inverse kinematics)
void get_angles(double Px, double Py) {
  // first find theta2 where c2 = cos(theta2) and s2 = sin(theta2)
  c2 = (pow(Px,2) + pow(Py,2) - pow(a1,2) - pow(a2,2))/(2*a1*a2); // is btwn -1 and 1

  if (elbowup == false) {
    s2 = sqrt(1 - pow(c2,2)); // sqrt can be + or -, and each corresponds to a different orientation
  }
  else if (elbowup == true) {
    s2 = -sqrt(1 - pow(c2,2));
  }
  theta2 = degrees(atan2(s2,c2)); // solves for the angle in degrees and places in correct quadrant

  // now find theta1 where c1 = cos(theta1) and s1 = sin(theta1)
  theta1 = degrees(atan2(-a2*s2*Px + (a1 + a2*c2)*Py, (a1 + a2*c2)*Px + a2*s2*Py));
}

void drive_joints() {
  // read the actual values from all the pots
  rawElbowAngle = analogRead(ElbowPin);
  rawShoulderAngle = analogRead(ShoulderPin);

  // constrain robot arm to ignore out of range values
  elbowAngle = constrain(rawElbowAngle, Elbow_pos, Elbow_neg);
  shoulderAngle = constrain(rawShoulderAngle, Shoulder_neg, Shoulder_pos);

  // now map the angles so they correspond correctly
  elbowAngle = map_double(elbowAngle, Elbow_neg, Elbow_pos, -110.0, 85.0);
  shoulderAngle = map_double(shoulderAngle, Shoulder_neg, Shoulder_pos, 5.0, 135.0);

  elbowPID.Compute();
  shoulderPID.Compute();

  // DRIVE ELBOW: CW is forward, CCW is backward
  pwmTemp = abs(pwmElbow);
  elbow.setSpeed(pwmTemp); // now use the new PID output to set the speed

  if (pwmElbow < 0) {
    elbow.run(FORWARD); // turn it on
  }
  else if ( pwmElbow > 0) {
    elbow.run(BACKWARD); // turn it on
  }
  else elbow.run(RELEASE); // stopped

  // DRIVE SHOULDER: CCW is forward, CW is backward
  pwmTemp = abs(pwmShoulder);
  shoulder.setSpeed(pwmTemp); // set the speed

  if (pwmShoulder < 0) {
    shoulder.run(BACKWARD); // turn it on
  }
  else if (pwmShoulder > 0) {
    shoulder.run(FORWARD); // turn it on
  }
  else shoulder.run(RELEASE); // stopped
  
  shoulderCurrentRaw = analogRead(ShoulderCurrentPin);
  if (shoulderCurrentRaw > 512) {
   shoulderCurrentRaw = shoulderCurrentRaw - 512; 
  }
  else {
   shoulderCurrentRaw = 512 - shoulderCurrentRaw; 
  }
  shoulderCurrentVolts = shoulderCurrentRaw * (5.0/1024.0);
  shoulderCurrentAmps = shoulderCurrentVolts/Volt_per_amp;
 
  Serial.print(millis());
  Serial.print(',');
  
  //Serial.print(elbowAngle);
  //Serial.print(',');
  Serial.print(shoulderAngle);
  Serial.print(',');
  //Serial.print(pwmElbow);
  //Serial.print(',');
  //Serial.print(pwmShoulder);
  //Serial.print(',');

  Serial.println(shoulderCurrentAmps);
  //Serial.print(',');
  //Serial.print(shoulderCurrentAmps);
  //Serial.print(',');
  //Serial.print(shoulderCurrentAmps);
  //Serial.print(',');
  //Serial.println(shoulderCurrentAmps);
  
  //Serial.print(actualX);
  //Serial.print(',');
  //Serial.print(predictedX);
  //Serial.print(',');
  //Serial.print(actualY);
  //Serial.print(',');
  //Serial.println(predictedY);
}

double map_double(double x, double in_min, double in_max, double out_min, double out_max)
{
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

Leave a Reply