Help with optical tracking project. 99% done, just missing something easy....

I’ve built a program to use two lasers and LDR’s to control a stepper motor to track the location of a floating weight in a large roto-meter.

The project is fairly simple: A high and low limit switch are mapped during an initialization routine, then the lasers settle on their target. If the target drops lower, the upper sensor gets hit by the laser and commands a stepper to drive down. Inversely, if the target raises, the lower sensor gets hit and commands the stepper to drive up.

Everything works as long as both sensors are blocked at the end of the initialization routine, but if the target is below the top of the meter (which will almost be always), the initialization fails. Attached is my code. I’d appreciate it if somebody could lend a set of eyes. I believe I’m missing something in step 3 of the initialization routine.

///////////////////////////////////////// INITIALIZATION /////////////////////////////////////////

#include <Stepper.h>

#include <Wire.h> // Enable this line if using Arduino Uno, Mega, etc.
#include "Adafruit_LEDBackpack.h"
#include "Adafruit_GFX.h"

Adafruit_7segment matrix1 = Adafruit_7segment();
Adafruit_7segment matrix2 = Adafruit_7segment();

const int sensor_1 = A0; // LDR1.  Lower LDR.  Added const back to these "read-only" variables
const int sensor_2 = A1; // LDR2   Upper LDR
const int sensor_3 = A2; // Low Limit Switch
const int sensor_4 = A3; // High Limit Switch

int sensorValueA1 = 0;   // Lower LDR
int sensorValueA2 = 0;   // Upper LDR
int sensorValueA3 = 0;
int sensorValueA4 = 0;

int stepCount       = 0;
int LowLimitSwitch  = 0;
int HighLimitSwitch = 0;
int LowLimitCount   = 0;
int HighLimitCount  = 0;

int Percentage = 0;
float voltage = 0;

int LDRvoltage1 = 5;  // Set LDRs high so Step 3 of Initialization will track down to find sensor then start LOOP
int LDRvoltage2 = 5;

const int stepsPerRevolution = 200;  // change this to fit the number of steps per revolution

// initialize the stepper library on pins 9 through 12
Stepper myStepper (stepsPerRevolution, 9, 11, 10, 12); // Coil energizing sequence (A, A', B, B')

int PWM_out_pin = 3;
int PWM_out_level = 0;
int val = 0;

/////////////////////////////////////// SETUP ///////////////////////////////////////////////////

void setup() 
{ 

  matrix1.begin(0x71);
  matrix1.setBrightness(15);           // set display brightness (1-15)
  matrix2.begin(0x72);
  matrix2.setBrightness(15);           // set display brightness (1-15)  
  
  matrix1.println(0, DEC);             // initialize the display to show zero
  matrix1.writeDisplay();
  matrix2.println(0, DEC);             // initialize the display to show zero
  matrix2.writeDisplay();
  
  
  myStepper.setSpeed(50);      // setting the speed of the stepper
  
  Serial.begin (9600);         // initialize the serial port //changed from 57600 

  pinMode(PWM_out_pin, OUTPUT); // output will be a duty cycle of 0 to 255 with 255 equal to 5v

//  initializeSystem();           // Call the system initialization routing from the subrouting area below LOOP

}

///////////////////////////////////// BEGIN LOOP /////////////////////////////////////////////////

void loop() 
{
  
// ================================= Read the Sensors =======================================

  sensorValueA1     = analogRead(sensor_1); 
  sensorValueA2     = analogRead(sensor_2);
  LowLimitSwitch    = analogRead(sensor_3);
  HighLimitSwitch   = analogRead(sensor_4);
  float LDRvoltage1 = sensorValueA1*(5.0/1023.0); //
  float LDRvoltage2 = sensorValueA2*(5.0/1023.0); // This math is required to get divisions of 5v

/* 
  Serial.print("sensor_1 = ");    Serial.print(LDRvoltage1);
  Serial.print("  sensor_2 = ");  Serial.print(LDRvoltage2);
  Serial.print("  LL Switch = "); Serial.print(LowLimitSwitch);
  Serial.print("  HL Switch = "); Serial.print(HighLimitSwitch);  
  Serial.print("  Step Count: "); Serial.print(stepCount);
  Serial.print("  LL Count = ");  Serial.print(LowLimitCount);
  Serial.print("  HL Count = ");  Serial.print(HighLimitCount);
  Serial.print("  val = ");       Serial.print(val);
  Serial.print("  "); Serial.print(Percentage); Serial.print("%");
  Serial.print("  "); Serial.print(voltage); Serial.println("v");
*/  

// ==================================== Main Loop =========================================== 

       if (((LDRvoltage1)<1.0) && ((LDRvoltage2)>1.0) && ((HighLimitSwitch)<1000)) // Chassis is below target //changed HLSfrom 400 to 1000
         { 
         myStepper.step(1);         // Track up (moves 0.5mm per step)
         stepCount++;               // Increment step count (this is only reliable when moving and incrementing by 1)
         delay(10);                 // Wait for sensors to settle
         }  
       if (((LDRvoltage1)>1.0) && ((LDRvoltage2)<1.0) && ((LowLimitSwitch)<1000)) // Chassis is above target.  Stops if it hits the limit switch    //changed HLSfrom 400 to 1000
         { 
         myStepper.step(-1);        // Track down (moves 1mm per step)
         stepCount--;               // Decrement step count (this is only reliable when moving and decrementing by 1)  
         delay(10);                 // Wait for sensors to settle
         }
       if (((LDRvoltage1)<1.0) && ((LDRvoltage2)<1.0)) // Chassis is completely OFF target.  Run Initialization code to find target again
         { 
         initializeSystem();
         }    

// Percentage = ((val * 100)/ 255);
//  (stepCount * 50 ) / (HighLimitCount / 2)); // Calculate the range percentage.  Step count can't exceed 32,767


// ========================== 0 to 5 Volt PWM Output ===========================

int val = stepCount;
val = map(val,(LowLimitCount),(HighLimitCount),0,255);
      
PWM_out_level = val;
analogWrite(PWM_out_pin, val); // output 0-255 on pin 3 for a duty cycle of 0-5 volts

float voltage = val * (5.0 / 255.0);

Percentage = ((val * 100)/ 255);
   
// ======================= Print Debugging Information =========================


//  Serial.print("sensor_1 = ");    Serial.print(LDRvoltage1);
//  Serial.print("  sensor_2 = ");  Serial.print(LDRvoltage2);
//Serial.print("  LL Switch = "); Serial.print(LowLimitSwitch);
//  Serial.print("  HL Switch = "); Serial.print(HighLimitSwitch);  
//  Serial.print("  Step Count: "); Serial.print(stepCount);
//  Serial.print("  LL Count = ");  Serial.print(LowLimitCount);
//  Serial.print("  HL Count = ");  Serial.print(HighLimitCount);
//  Serial.print("  val = ");       Serial.print(val);
//  Serial.print("  "); Serial.print(Percentage); Serial.print("%");
//  Serial.print("  "); Serial.print(voltage); Serial.println("v");
  

    matrix1.println(voltage);           // display voltage on I2C LED display
    matrix1.writeDisplay();
    matrix2.println(Percentage);        // display percentage on I2C LED display
    matrix2.writeDisplay();

    delay(1);  // this is important for timing so display doesn't lock up
    
  
} ///////////////////////////////////// END LOOP /////////////////////////////////////////////////


///////////////////////// FUNCTION SUBROUTINE CALL AREA ///////////////////////////////////////////

void initializeSystem()  // Function that runs three steps of system initialization

{




int val = 0; // Set the PWM output to zero volts while the initialization routine is running
  
 // STEP ONE
while (LowLimitSwitch < 400) // Track to the LowLimitSwitch and reset stepCount
{
   LowLimitSwitch    = analogRead(sensor_3);
      
   myStepper.step(-1);   // Track down (moves 0.5mm per step) to find LowLimitSwitch
   stepCount--;          // Decrement step count (this is only reliable when moving 1 step at a time)
   delay(10);            // Wait for sensors to settle
}  

stepCount = 0;             // Reset stepCount
LowLimitCount = stepCount; // Reset LowLimitCount

// STEP TWO
while (HighLimitSwitch < 400) // Track to the HighLimitSwitch and write stepCount into HighLimitCount
{
   HighLimitSwitch   = analogRead(sensor_4);
      
   myStepper.step(1);   // Track up (moves 0.5mm per step) to find HighLimitSwitch
   stepCount++;         // Increment step count (this is only reliable when moving 1 step at a time) 
   delay(10);           // Wait for sensors to settle
}  

HighLimitCount = stepCount; // Write HighLimitCount for Map function to indicate a range position with a voltage 

// STEP THREE
while ((sensorValueA1) < 1.0) // Move chassis down until it finds the armature then start Loop   //changed from 600 to 1.0
{ 
   sensorValueA1     = analogRead(sensor_1); 
 
   myStepper.step(-1);   // Track down (moves 0.5mm per step)
   stepCount--;          // Decrement step count (this is only reliable when moving 1 step at a time) 
   delay(10);  
}
}

Why do you bother count steps in step 1? You immediately set stepCount to 0, without ever caring what stepCount was.

The code you posted actually does something. You did not explain what it actually does.

There are some things connected to the various analog pins. You've left us to guess what is connected to each pin. sensor_n doesn't say squat about what is connected, or what a reasonable value will be, when reading that pin. It doesn't even imply that it IS a pin number.

Here’s the image attached to the top post.

2988627d6ff94610667417be6886583ba7055849.jpg

How to insert uploaded images.

Why not add some debug statements to your initialization code so you can figure out which section of code causes the trouble?

I think you’re better off describing a variable with its name rather than in comments.

Instead of the line below:

int sensorValueA1 = 0;   // Lower LDR

You could use:

int lowerLdrSensorValue = 0;

Are your limit switches analog devices?

 // STEP ONE
while (LowLimitSwitch < 400) // Track to the LowLimitSwitch and reset stepCount
{
   LowLimitSwitch    = analogRead(sensor_3);

I’d modify such loops, at least the initial value of the limit switches should be correct before entering the loop. Something like

 // STEP ONE
while ((LowLimitSwitch = analogRead(sensor_3)) < 400) // Track to the LowLimitSwitch and reset stepCount
{

Or, easier to understand, get the values of the limit switches before any further adjustments. Then you also can find out when something is very wrong, like no target is present or the target is initially outside the expected range.

Thanks for the early tips. Still new at this, so I know it’s not very refined.

To answer some of the questions:

There are 4 sensors total. Sensors 1 and 2 are LDR’s mounted in a moving chassis driven up and down by a screw turned by a stepper motor. Sensors 3 and 4 are optical limit switches I have mounted at the top and bottom of the chassis travel.

The sensors are plugged into A0-A3 in order.
The stepper motor control chip is landed to D9-D12.

I’m using two Aidafruit I2C 7 segment displays. One is mapped to percent of travel, the other is mapped to voltage.

I assume all of my values are correct because as long as I have sensors 1 and 2 blocked at the end of the initialization routine, the entire project operates as intended. I can from that point forward move the target through the entire range and the chassis will track correctly, and the position is reported correctly.

The initialization routine begins by driving to motor towards the bottom limit switch (sensor 3). When it hits that, it reverses direction and drives upward until it hits the upper limit switch (sensor 4). After it hits the upper limit switch (sensor 4), the step count is mapped. If at that point the target is blocking the laser light to the LDR’s in the chassis (sensors 1 and 2), everything works perfectly from that time forward. However, if the target is not present at the very top of the range, the initialization routine begins again and it will continue this cycle until sensors 1, (chassis), and 4 (upper limit switch) are both blocked at the same time.

I’m pretty sure my problem is in the last few lines in the sketch, here:

// STEP THREE
while ((sensorValueA1) < 1.0) // Move chassis down until it finds the armature then start Loop //changed from 600 to 1.0

Does that make more sense? I’ll take a few more pictures and post them up in a bit. Thanks DuaneDegn for making my original picture easier to find. I can’t view them from my work laptop for some reason.

Here’s a couple more pictures. :slight_smile:

Hmmm… The last pictures violate forum security for some reason?? Nothing you haven’t seen before, just my proto boards and microcontroller setup.