Vehicle with proximity detection.

I'm working on a project that involves making a four wheeled vehicle that utilizes proximity detection to navigate. I'm utilizing an Arduino Uno and some infrared proximity sensors(Infrared Proximity Sensor Long Range - Sharp GP2Y0A02YK0F - SEN-08958 - SparkFun Electronics). I have two sensors mounted on the front left and right side and my goal is to adjust the speed of the motors depending on whether there is some in front of me or not. For example, with nothing in front of me the motors would be going at 100%, but if an object was detected on the right side the left motors would gradually slow down until the vehicle has turned enough so that the object is no longer in range.

I have tested out my sensors to ensure they work and get a good feel for how they work. Individually they work pretty well and put together in a simple sketch they work about the same. Where I run into trouble is when they are put together and trying to adjust the motor speeds. The code I utilize is below:

void loop(){
  int L_proxim = 0;
  int R_proxim = 0;
  for(int i = 0; i < 5; i ++){
    L_proxim += analogRead(Lprox);
  }
  L_proxim /= 5;
  for(int i = 0; i < 5; i ++){
    R_proxim += analogRead(Rprox);
  }
  R_proxim /= 5;
  L_speed = leftMotorSpeed(R_proxim);
  R_speed = rightMotorSpeed(L_proxim);
  move(L_speed, R_speed);
}

int leftMotorSpeed(int proxL){
  float motorPer = (theta_prox_L - proxL)/delta_prox_L;
  Serial.print("Left perc: ");
  Serial.println(motorPer);
  Serial.print("Left reading: ");
  Serial.println(proxL);
  return MAX_SPEED * motorPer;  //MAX_SPEED is constant between 0 and 255
}

int rightMotorSpeed(int proxR){
  Serial.print("Right reading: ");
  Serial.println(proxR);
  return MAX_SPEED * (theta_prox_R - proxR)/delta_prox_R; //theta is the middle of the sensor voltage range
                                                           //delta is the sensor voltage range
}

void move(int Lspeed, int Rspeed){
  boolean Ldirect = HIGH?(Lspeed >= 0):LOW;
  boolean Rdirect = HIGH?(Rspeed >= 0):LOW;
  
  digitalWrite(AIN1, Ldirect);
  digitalWrite(AIN2, !Ldirect);
  digitalWrite(BIN1, Rdirect);
  digitalWrite(BIN2, !Rdirect);
  analogWrite(PWMA, abs(Lspeed));
  analogWrite(PWMB, abs(Rspeed));
  return;
}

When I do get some legitimate values I can see that this code works pretty well. The problem is that only about 1/100 values are legitimate. When everything is changing so fast, I figure each loop iteration is around 1ms, give or take. The values coming from the ADC can be a little sporadic, though I have calmed them down a bit utilizing some filtering caps, every once in a while I'll see a value that is way off mark, on the order of +/- 50. This doesn't seem to be the entire problem though, most of the time the math being performed is either way off or doesn't seem to be performed at all, i.e. I can see the sensor was read with nothing in front of it and therefore should make the motor go 100% but instead is sets it to 0.

I can't really see where I'm going wrong with this, I have checked my formula for determining speed over and over and that is good, and from what I've read about the ADC it should be able to handle these conversions with no problem. I've looked all over trying to find anything about doing multiple "simultaneous" conversions on the arduino and haven't come across anything that's helpful. I dug through the wiring_analog.c file and determined that an analogRead(int) will indeed wait for a conversion to finish before it returns so there shouldn't be any issue doing multiple conversions back to back.

Anyone out there have any ideas or have run into similar issues? Is the Uno even capable of performing these kind of calculations fast enough to be viable?

That code is incomplete and the missing parts prevents me from understanding what it does.

I would tackle the problem in three stages.

Taking the motor control out of the problem (just move the sensors around manually), do the sensors give you the inputs you expect in the scenario that causes the problem?

Given those sensor inputs which we now know are correct, is the control logic producing the correct motor demands?

Are the motor demands being implemented accurately?

Presumably at the moment the answer to one of those questions is 'no'.

What are the data types of theta_prox_L and delta_prox_L?

I gave enough of the code to get a jist of what is happening, the rest of it has no bearing on this problem. As I stated previously I have tested the sensors on their own and, with the exception of the occasional miss, the values are as expected. The problem, as I already stated, is indeed in the motor control, the function for controlling them is not getting the proper values. The real problem doesn't lie in my code, at least the logic of it, it is most likely in the arduino itself. It doesn't seem to be able to perform the necessary calculations fast enough or accurately.

I have another sketch where the sensors are polled on a regular interval while the vehicle moves forward until something comes within range, at which point the vehicle stops and turns until it is clear again. This works fine, but now I want to utilize the sensors as a sort of feedback loop to constantly change the speed of the motors as necessary.

THETA and DELTA are ints.

automan:
The real problem doesn't lie in my code,
...
THETA and DELTA are ints.

I am not sure if I can agree with you on where your problem lies. At least you gave some answers to my question. So what are the values of these two integers and what is the range of values that proxL gets, from nothing in sight to object very close?

Not posting the complete code is frowned upon. You need help. The least you should do is to provide all you can on a post. Leave the worry of wasted space and long reading to the rest of us.

automan:
I gave enough of the code to get a jist of what is happening, the rest of it has no bearing on this problem

If large parts of your sketch are irrelevant then produce a simpler sketch which demonstrates the problem without them. As well as validating your assumptions about where the problem lies, it will reduce the amount of code we need to understand to help you. But whichever way you go, please post a complete sketch which demonstrates the problem.

  L_speed = leftMotorSpeed(R_proxim);

int leftMotorSpeed(int proxL){

The function definition implies that it expects the argument to be a value from the left sensor, but you're giving it the value from the right sensor. Which is correct? At the very least, this means that the values are being printed out with the wrong label, which would make it very easy for you to have validated the sensor data wrongly.

Well since you asked here is the rest of the code, though you will notice that the only problem area is what I posted earlier, the rest is just setup.

I will post code below, my laptop hates me right now.

the range I am seeing is normally from 39 - 1023 (integer value from analogRead), there is a 2.5V input to analog reference since the sensor range is 1.8 - 2.3v. The problem is that the input being read is unstable, occasionally jumps from lets say 193 to 39 and back to 193 which causes the motors to not operate properly. At the same time when I am getting a bunch of valid values, like 39 which should give me a motor input of 255 (max in this case), i instead get a 0, or possibly a negative number. This indicates that the hardware is not able to keep up with the calculations, maybe some overflow errors that aren't being caught.

L_speed = leftMotorSpeed(R_proxim);

int leftMotorSpeed(int proxL){

Means that i am feeding data from the right sensor to the left motors, that way if there is an object on the right side the vehicle it will turn left and avoid the object rather then turn right and run right into it.

#include <LSM303.h>
#include <Wire.h>

//Data

const int MAX_PROX = 1023;  //max reading from prox sensors
const int MAX_SPEED = 255;
int prox_cal_L = 0;        //min reading from left sensor after cal
int prox_cal_R = 0;        //min reading from right sensor after cal
int delta_prox_L = 0;      //delta of max and min reading from left sensor
int delta_prox_R = 0;      //delta of max and min reading from right sensor
int theta_prox_L = 0;
int theta_prox_R = 0;

int L_speed = 0;
int R_speed = 0;

LSM303::vector<int16_t> compass_cal_min = {32767, 32767, 32767}
, compass_cal_max = {-32768, -32768, -32768}; //cal data for compass

LSM303 compass;

//pins
int Lprox = 0;  //left prox sensor
int Rprox = 1;  //right prox sensor
int STBY = 10; //standby

//Motor A (Left)
int PWMA = 3; //Speed control 
int AIN1 = 12; //Direction
int AIN2 = 11; //Direction

//Motor B (Right)
int PWMB = 5; //Speed control
int BIN1 = 9; //Direction
int BIN2 = 8; //Direction

char report[80];

void setup(){

  Wire.begin();
  compass.init();
  compass.enableDefault();
  
  pinMode(STBY, OUTPUT);

  pinMode(PWMA, OUTPUT);
  pinMode(AIN1, OUTPUT);
  pinMode(AIN2, OUTPUT);

  pinMode(PWMB, OUTPUT);
  pinMode(BIN1, OUTPUT);
  pinMode(BIN2, OUTPUT);
  
  analogReference();
  digitalWrite(STBY, HIGH);
  Serial.begin(9600);
  calibration();
}

void loop(){
  int L_proxim = 0;
  int R_proxim = 0;
  for(int i = 0; i < 5; i ++){
    L_proxim += analogRead(Lprox);
  }
  L_proxim /= 5;
  for(int i = 0; i < 5; i ++){
    R_proxim += analogRead(Rprox);
  }
  R_proxim /= 5;
  L_speed = leftMotorSpeed(R_proxim);

  R_speed = rightMotorSpeed(L_proxim);

  move(L_speed, R_speed);
  
}


void calibration(){
  Serial.println("Calibrating prox sensors... ");
  for(int i=0; i<5; i++)
  {
    prox_cal_L += analogRead(Lprox);
    prox_cal_R += analogRead(Rprox);
  } 

  Serial.println("Writing cal data... ");
  prox_cal_L = prox_cal_L / 5;
  prox_cal_R = prox_cal_R / 5;
  
  delta_prox_L = (MAX_PROX - prox_cal_L)/2;
  delta_prox_R = (MAX_PROX - prox_cal_R)/2;
  theta_prox_L = delta_prox_L + prox_cal_L;
  theta_prox_R = delta_prox_R + prox_cal_R;
  
  Serial.println("Calibrating compass... ");
  move(100, -100);
  Serial.println("Calibrating compass... ");
  unsigned long timer = 0;
  unsigned long initial = millis();
  Serial.println(timer);
  do{
    Serial.println(timer);
    timer = millis();
    compass.read();
    compass_cal_min.x = min(compass_cal_min.x, compass.m.x);
    compass_cal_min.y = min(compass_cal_min.y, compass.m.y);
    compass_cal_min.z = min(compass_cal_min.z, compass.m.z);
    compass_cal_max.x = max(compass_cal_max.x, compass.m.x);
    compass_cal_max.y = max(compass_cal_max.y, compass.m.y);
    compass_cal_max.z = max(compass_cal_max.z, compass.m.z);
  }while(timer < (initial + 5000));
  stop();
  
  Serial.println("Writing compass data... ");
  compass.m_min = (LSM303::vector<int16_t>){
    compass_cal_min.x, compass_cal_min.y, compass_cal_min.z  };
  compass.m_max = (LSM303::vector<int16_t>){
    compass_cal_max.x, compass_cal_max.y, compass_cal_max.z  };  
   
   snprintf(report, sizeof(report), "min: {%+6d, %+6d, %+6d}    max: {%+6d, %+6d, %+6d}",
    compass_cal_min.x, compass_cal_min.y, compass_cal_min.z,
    compass_cal_max.x, compass_cal_max.y, compass_cal_max.z);
  Serial.println("Calibration data: ");
  Serial.print("Left prox: ");
  Serial.print(prox_cal_L);
  Serial.println();
  Serial.print("Right prox: ");
  Serial.print(prox_cal_R);
  Serial.println();
  Serial.print("Left theta: ");
  Serial.print(theta_prox_L);
  Serial.println();
  Serial.print("Right theta: ");
  Serial.print(theta_prox_R);
  Serial.println();
  Serial.print("Left delta: ");
  Serial.print(delta_prox_L);
  Serial.println();
  Serial.print("Right delta: ");
  Serial.print(delta_prox_R);
  Serial.println();
  Serial.print(report);
   
}

int leftMotorSpeed(int proxL){
  float motorPer = (theta_prox_L - proxL)/delta_prox_L;
  Serial.print("Left perc: ");
  Serial.println(motorPer);
  Serial.print("Left reading: ");
  Serial.println(proxL);
  return MAX_SPEED * motorPer;
}


int rightMotorSpeed(int proxR){
  Serial.print("Right reading: ");
  Serial.println(proxR);
  return MAX_SPEED * (theta_prox_R - proxR)/delta_prox_R;
}

void move(int Lspeed, int Rspeed){
  boolean Ldirect = HIGH?(Lspeed >= 0):LOW;
  boolean Rdirect = HIGH?(Rspeed >= 0):LOW;
  Serial.print("Moving: ");
  Serial.print(Lspeed);
  Serial.print(" ");
  Serial.print(Ldirect);
  Serial.print(" ");
  Serial.print(Rspeed);
  Serial.print(" ");
  Serial.println(Rdirect);
  digitalWrite(AIN1, Ldirect);
  digitalWrite(AIN2, !Ldirect);
  digitalWrite(BIN1, Rdirect);
  digitalWrite(BIN2, !Rdirect);
  analogWrite(PWMA, abs(Lspeed));
  analogWrite(PWMB, abs(Rspeed));
  return;
}

void stop(){
  digitalWrite(AIN1, LOW);
  digitalWrite(AIN2, LOW);
  digitalWrite(BIN1, LOW);
  digitalWrite(BIN2, LOW);  
}

I don't get what your calibration function is trying to do or what you're doing with the theta and delta values afterwards when you calculate the speed, but as well as printing out the wrong labels for left/right sensor readings, you're applying the wrong 'calibration' data for each input since leftMotorSpeed() uses the calibration data from the left sensor but is actually getting the right sensor data, and vice versa.

If this doesn't point you to the problem I suggest you supply a log of the serial output demonstrating the problem and perhaps that will reveal where abouts the error is being introduced, but from what I can see so far there is nothing to suggest an arithmetic error and the evidence suggests that it's simply a flaw in the algorithm you're using.

You mention that you're getting erratic readings from the sensors. It wouldn't surprise me if that's just a characteristic of those sensors and not anything you're doing wrong. In that case you could mitigate the problem by smoothing the value read from the sensors so that isolated glitches didn't throw your steering logic out. A simple decaying average ought to be good enough:

smoothed = ((weight * smoothed) + newValue) / (weight + 1);

I suggest you start with a value for weight of around 5, but you can try bigger values if you are still getting a lot of glitches. Note that this technique will make the sensing slower to detect approaching objects, but from what I can see of the code you're reading the sensors at a pretty high frequency so that probably won't matter to you - you can increase the frequency if necessary.

PeterH:
you're applying the wrong 'calibration' data for each input since leftMotorSpeed() uses the calibration data from the left sensor but is actually getting the right sensor data, and vice versa.

You're right, thanks for pointing that out, though the calibration data is fairly close between the left and right side so this isn't the main problem, it would only make the vehicle turn at the wrong time or speed.

PeterH:
In that case you could mitigate the problem by smoothing the value read from the sensors so that isolated glitches didn't throw your steering logic out. A simple decaying average ought to be good enough:

I was thinking of doing something similar, though this approach would be a lot simpler and faster to implement, I'll give it a shot.

I'm also considering slowing down the algorithm a bit, that may help mitigate the problem as well. After thinking about it I have it running around 1kHz right now, which at the velocity this vehicle will be moving is probably way more then it needs to be.

As for the calibration, it takes a reading with nothing in front of the vehicle to get a value for the lower limit of the sensor, this can change slightly due to the environment, at least in theory. Then using that lower limit and the known upper limit (1023 with a 10-bit ADC) it finds the middle point between the two (THETA) and the total span between them (DELTA). These are used in my equation to determine how fast the motor should turn, (THETA-reading)/DELTA is a number from -1 to 1, then multiplied by the MAX_SPEED gives me a new number for the speed. That way as an object approaches the vehicle the motors start to slow down and eventually move in reverse to avoid the object.

If both theta and delta are integers and you may lose a lot of accuracy when you do the division (THETA-reading)/DELTA

Instead, do this
(THETA-reading)/(float)DELTA

Since you are assigning the result to a float already. This prevents unnecessary loss of accuracy. You should try this.

How about decoupling all that logic from the motor control logic by implementing a function that uses a given sensor to return a distance value, doing whatever smoothing and offsetting and scaling you need. Test that and get it working accurately and reliably before you try to use the returned value to control the speed.