Current sensing problem with DC motors

So I’m trying to do low side current sensing on a small robotic arm driven by 2 DC gearmotors. With no current sensing, everything is working fine, and I’m printing a bunch of debug values to the serial monitor. Then if I add the 1 ohm current sensing resistor in series with the motor ground, everything is mostly fine - just a little bit twitchy. However, when I actually try to analogRead each side of the current sensing resistor and print those values, the robot gets super slow and no longer does what it’s supposed to at all. All I did was add this code:

  lowV = analogRead(LowPin);
  highV = analogRead(HighPin);

Serial.print("Voltage:");
        Serial.print(" , ");
  Serial.print(highV);
      Serial.print(" , ");
      Serial.println(lowV);

There are no delays in the entire program, analogRead() only takes 0.0001 seconds according to the http://arduino.cc/en/Reference/AnalogRead, and I can’t imagine that serial printing a few more values when I was already printing 10 others would slow the loop down so much as to mess up the motor control, especially because I’m already printing at 115200. Any thoughts?

Full code here for reference:

/* 
 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 ElbowPin = A1;  // to potentiometer on elbow motor
int ShoulderPin = A0; // to potentiometer on shoulder motor

int LowPin = A3; // to ground of power supply
int HighPin = A2; // to high side of current resistor
//AREF tied to 3.3V

// INITIALIZE CONSTANTS
double Kp_Elbow = 28;    // this is the proportional gain
double Kp_Shoulder = 30; 
double Ki_Elbow = 0.2;    // this is the integral gain
double Ki_Shoulder = 0.2; 
double Kd_Elbow = 2.0;  // this is the derivative gain
double Kd_Shoulder = 1.5;

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 = 200;     // shoulder-to-elbow "bone" length (mm)
const double a2 = 220;     // elbow-to-wrist "bone" length (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

// VARIABLES
double rawElbowAngle = 0;      // initialize all angles to 0 
double rawShoulderAngle = 0; 

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

double theta1 = 0;  // target angles as determined through IK
double theta2 = 0;

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

int pwmTemp = 0;

double pwmElbow = 100;  
double pwmShoulder = 100;

//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);

int lowV = 0;
int highV = 0;

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

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

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

  elbowPID.SetMode(AUTOMATIC);
  shoulderPID.SetMode(AUTOMATIC);

  elbowPID.SetOutputLimits(-255,255);
  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 equals expected 
  }  
  while( abs(theta1 - shoulderAngle) > 2 && abs(theta2 - elbowAngle) > 2 ); // bail when it's close
}

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

// 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(elbowAngle, Elbow_neg, Elbow_pos, -110, 85);  
  shoulderAngle = map(shoulderAngle, Shoulder_neg, Shoulder_pos, 5, 135);  

  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   
  
  lowV = analogRead(LowPin);
  highV = analogRead(HighPin);

  // need to know time, angle for velocity calcs
  Serial.print(millis());
  Serial.print(" , ");
  Serial.print("Elbow: ");
    Serial.print(" , ");
  Serial.print(elbowAngle);
    Serial.print(" , ");
  Serial.print("Shoulder: ");
    Serial.print(" , ");
  Serial.print(shoulderAngle);
  Serial.print(" , ");
    Serial.print("Voltage:");
        Serial.print(" , ");
  Serial.print(highV);
      Serial.print(" , ");
      Serial.println(lowV);
}

Are you sure it's a software problem? My understanding is that analog pins are multiplexed, and connected to a single ADC internally when a reading is taken. It may be the act of /connecting/ to the pin electrically that is messing things up.

This part:
lowV = analogRead(LowPin);
highV = analogRead(HighPin);

You could do this as 4 reads:

lowV = analogRead(LowPin);
lowV = analogRead(LowPin);

highV = analogRead(HighPin);
highV = analogRead(HighPin);

Effectively throws the first read of each away, but lets the voltage settle for the 2nd read.

Does it work differently if you run this in IDE -0022 or -0023 vs 1.0? Serial port behavior was changed in 1.0.

You might also try a smaller shunt resistor, like 10 mOhm vs 1 ohm, so you have less voltage drop across it.
An example:
https://secure.dipmicro.com/store/PWR4412-2SBR010F

Okay so the initial weirdness was because I had wired AREF to 3.3V. After I commented out the external reference in the setup, everything worked out fine. However, the current sensing is still weird. According to my benchtop supply, the current both motors are drawing is about 1.5A. With the analog pins, 5V/1024 means each "unit" of analogRead should be about 0.0048828125V. If I use my R = 1 ohm, I = 1.5A, I should be seeing about a 1.5V drop across my current sense resistor. This should translate into 1.5/0.004 = 307 readings (give or take) from just printing the value I sense through analogRead. However, the data looks more like this:

0 0 0 0 10 0 0 0 0 14 0 0 0 0 0 10

It looks like from here http://letsmakerobots.com/node/18517 that, since I'm pwming the motors, the pwm might be too fast for the analogRead to pick up anything meaningful. But I wonder if I can just put a cap across the resistor? Otherwise, with a smaller shunt resistor, I need to build the op-amp circuit to amplify it. I probably have all the parts but want to keep this as simple as possible. Any thoughts? I'll post back here if just putting a cap across the resistor helps, but I don't have a good feeling about it.

Nope, a cap across the shunt resistor doesn't do anything. I tried a few different values.

You need to use an RC low pass circuit to filter the shunt voltage - if you just try a cap across the shunt it will need to be an enormously high value like 10,000uF or more.. Just take the shunt hi voltage through 10k to a 1uF to 10uF cap and read the voltage across the cap.