A glitch in my pan / tilt program

Projects Programming
1

Hi All,

I have been working on the software side of a pan tilt device which is designed to take a number of photos with a DSLR camera moving left to right, or right to left, then up the Y axis based on input from the serial monitor.

So the program basically waits for serial input, in the form of Starting X position, Starting Y position, Finishing X position, Finishing Y position and the focal length of the lense.

It uses all of this to work out the field of view of the lense, and then to determine how many photos will need to be taken (accounting for overlap) and how many degrees between each photo on the X and Y axis.

The idea is that I will use this with some stepper motors (when they arrive), however I do have a strange glitch in the code that I can't quite work out.

The code moves left to right on the X axis, then moves up on the Y, then right to left on the X axis then up on the Y and repeat. This is fine however for some reason at the end of the first X loop, it increments beyond where it should on the Y axis moving up, for example:

Moving to position (X,Y): 175,15
Moving to position (X,Y): 186,15
Moving to position (X,Y): 186,23
Moving to position (X,Y): 186,31
Moving to position (X,Y): 175,31

In the above, it should not be going to position 23 but rather jumping to 31 then going back on the X axis, is someone able to take a quick peek at my code and see if something jumps out in terms of what may be causing this.

I must apologize for the poorly written sketch, but many thanks for taking a look at this!

2 
#include <multiCameraIrControl.h>
Nikon D5200(12);
byte byteRead;


int fLength = 50; //define the focal length of the camera
int fovOverlap = 30; //define the overlap between images
int sDelay = 200; //define the delay before taking the photo (e.g. for focus, stability)
int cDelay = 500; //define how long to wait after photo is taken before moving to the next position
String cAutofocus = "N"; //define if the Camera autofocus will be used
String cExBracket = "N"; //define if exposure bracketing is likely to be used
String mTest = "N"; //define if the device is in test / simulation mode
String inData;
String cLandscape = "Y"; //define if the camera is in landscape orentation
double sDiagional = 28.3; //Define the Diagional Sensor Length
double sHorizontal = 23.6; //Define the Horozintal Sensor Length
double sVertical = 15.6; //Define the Vertical Sensor Length

int TopX = 0; //Define the top left starting position (X)
int TopY = 0; //Define the top left starting position (Y)
int BottomX = 0; //Define the top left starting position (X)
int BottomY = 0; //Define the top left starting position (Y)
int PanX = 0; //Define the number of Degrees to travel across the X Axis
int PanY = 0; //Define the number of Degrees to travel across the Y Axis
int PhotosX = 0; //Define the number of photos to take across the X Axis
int PhotosY = 0; //Define the number of photos to take across the Y Axis
int DegStepsX = 0; //Define the number of degrees to move across the X Axis per photo
int DegStepsY = 0; //Define the number of degrees to move across the Y Axis per photo
float fDiag = 0; //Define the Diagional field of view based on a set focal length
float fHor = 0; //Define the Horizontal field of view based on a set focal length
float fVer = 0; //Define the Vertical field of view based on a set focal length
int pStepXPos = 0; //Variable for storing the currect X position
int pStepYPos = 0; //Variable for storing the currect Y position
char test[20];

void setup() {
 Serial.begin(9600);
 Serial.flush();
 Serial.println("Panorama Robot");
 Serial.println("To Begin, Send Start X, Stary Y (Bottom Left), Finish X, Finish Y (Top Right), Focal Length, e.g. 10,15,180,40,150");
  }

void loop() {
              
while (Serial.available() > 0)
    {
        char recieved = Serial.read();
        inData += recieved; 

        // Process message when new line character is recieved
        if (recieved == '\n')
        {
          Serial.println("");  
          Serial.print("Arduino Received: ");
            Serial.println(inData);
                        Serial.println("");
            
TopX=getValue(inData,',',0).toInt();
TopY=getValue(inData,',',1).toInt();
BottomX=getValue(inData,',',2).toInt();
BottomY=getValue(inData,',',3).toInt();
fLength=getValue(inData,',',4).toInt();

if (cLandscape="Y") {
            fDiag=(2*atan(sDiagional/(2*fLength)))*100;
            fHor=(2*atan(sHorizontal/(2*fLength)))*100;
            fVer=(2*atan(sVertical/(2*fLength)))*100;
} else {
            fDiag=(2*atan(sDiagional/(2*fLength)))*100;
            fVer=(2*atan(sHorizontal/(2*fLength)))*100;
            fHor=(2*atan(sVertical/(2*fLength)))*100;
}

PanX = BottomX-TopX;
PanY = BottomY-TopY;

PhotosX=(100*PanX/((100-fovOverlap)*fHor));
PhotosY=(100*PanY/((100-fovOverlap)*fVer));

DegStepsX = PanX / PhotosX;
DegStepsY = PanY / PhotosY;


Serial.print("Need to travel (X,Y): ");
Serial.print(PanX) & Serial.print(",") & Serial.println(PanY); 
Serial.print("Starting from position (X,Y): ");
Serial.print(TopX) & Serial.print(",") & Serial.println(TopY); 
Serial.print("Field of view (X,Y): ");
Serial.print(floatToString(test, fVer, 2, 0)) & Serial.print(",") & Serial.println(floatToString(test, fDiag, 2, 0)); 
Serial.print("Which is a total of (X,Y): ");
Serial.print(PhotosX) & Serial.print(",") & Serial.println(PhotosY); 
Serial.print("With this number of degrees between each photo (X,Y): ");
Serial.print(DegStepsX) & Serial.print(",") & Serial.println(DegStepsY); 
delay(5000);

Serial.println("");
Serial.println("Starting Panorama");
Serial.println("");
//Serial.print("Moving to position (X,Y): ") & Serial.print(TopX) & Serial.print(",") & Serial.println(BottomY); 

inData = ""; // Clear recieved buffer

int YCounter = 1;
pStepYPos = TopY;
pStepXPos = TopX;

   while(pStepXPos < BottomX){
Serial.print("Moving to position (X,Y): ") & Serial.print(pStepXPos) & Serial.print(",") & Serial.println(pStepYPos);
delay(sDelay);
D5200.shutterNow();
delay(cDelay);
pStepXPos = pStepXPos + DegStepsX;}

while(pStepYPos < BottomY){
Serial.print("Moving to position (X,Y): ") & Serial.print(pStepXPos) & Serial.print(",") & Serial.println(pStepYPos);
delay(sDelay);
D5200.shutterNow();
delay(cDelay);
pStepYPos = pStepYPos + DegStepsY;
  YCounter = YCounter + 1;
  
  if ( (YCounter % 2) == 0) {
    
   while(pStepXPos < BottomX){
Serial.print("Moving to position (X,Y): ") & Serial.print(pStepXPos) & Serial.print(",") & Serial.println(pStepYPos);
delay(sDelay);
D5200.shutterNow();
delay(cDelay);
pStepXPos = pStepXPos + DegStepsX;}

} else {
   while(pStepXPos > TopX){
Serial.print("Moving to position (X,Y): ") & Serial.print(pStepXPos) & Serial.print(",") & Serial.println(pStepYPos);
delay(sDelay);
D5200.shutterNow();
delay(cDelay);
pStepXPos = pStepXPos - DegStepsX;}
}
 }
 Serial.println("Panorama complete");
        }
           if(inData == "+++\n"){ // DON'T forget to add "\n" at the end of the string.
              //Serial.println("OK. Press h for help.");
           }   
          
        }
    }
       

   



char * floatToString(char * outstr, double val, byte precision, byte widthp){
 char temp[16];
 byte i;

 // compute the rounding factor and fractional multiplier
 double roundingFactor = 0.5;
 unsigned long mult = 1;
 for (i = 0; i < precision; i++)
 {
   roundingFactor /= 10.0;
   mult *= 10;
 }
 
 temp[0]='\0';
 outstr[0]='\0';

 if(val < 0.0){
   strcpy(outstr,"-\0");
   val = -val;
 }

 val += roundingFactor;

 strcat(outstr, itoa(int(val),temp,10));  //prints the int part
 if( precision > 0) {
   strcat(outstr, ".\0"); // print the decimal point
   unsigned long frac;
   unsigned long mult = 1;
   byte padding = precision -1;
   while(precision--)
     mult *=10;

   if(val >= 0)
     frac = (val - int(val)) * mult;
   else
     frac = (int(val)- val ) * mult;
   unsigned long frac1 = frac;

   while(frac1 /= 10)
     padding--;

   while(padding--)
     strcat(outstr,"0\0");

   strcat(outstr,itoa(frac,temp,10));
 }

 // generate space padding 
 if ((widthp != 0)&&(widthp >= strlen(outstr))){
   byte J=0;
   J = widthp - strlen(outstr);
   
   for (i=0; i< J; i++) {
     temp[i] = ' ';
   }

   temp[i++] = '\0';
   strcat(temp,outstr);
   strcpy(outstr,temp);
 }
 
 return outstr;
}
 

String getValue(String data, char separator, int index)
{
 int found = 0;
  int strIndex[] = {
0, -1  };
  int maxIndex = data.length()-1;
  for(int i=0; i<=maxIndex && found<=index; i++){
  if(data.charAt(i)==separator || i==maxIndex){
  found++;
  strIndex[0] = strIndex[1]+1;
  strIndex[1] = (i == maxIndex) ? i+1 : i;
  }
 }
  return found>index ? data.substring(strIndex[0], strIndex[1]) : "";
}
3 
Serial output:

Panorama Robot
To Begin, Send Start X, Stary Y (Bottom Left), Finish X, Finish Y (Top Right), Focal Length, e.g. 10,15,180,40,150

Arduino Received: 10,15,180,40,150


Need to travel (X,Y): 170,25
Starting from position (X,Y): 10,15
Field of view (X,Y): 10.39,18.81
Which is a total of (X,Y): 15,3
With this number of degrees between each photo (X,Y): 11,8

Starting Panorama

Moving to position (X,Y): 10,15
Moving to position (X,Y): 21,15
Moving to position (X,Y): 32,15
Moving to position (X,Y): 43,15
Moving to position (X,Y): 54,15
Moving to position (X,Y): 65,15
Moving to position (X,Y): 76,15
Moving to position (X,Y): 87,15
Moving to position (X,Y): 98,15
Moving to position (X,Y): 109,15
Moving to position (X,Y): 120,15
Moving to position (X,Y): 131,15
Moving to position (X,Y): 142,15
Moving to position (X,Y): 153,15
Moving to position (X,Y): 164,15
Moving to position (X,Y): 175,15
Moving to position (X,Y): 186,15
Moving to position (X,Y): 186,23
Moving to position (X,Y): 186,31
Moving to position (X,Y): 175,31
Moving to position (X,Y): 164,31
Moving to position (X,Y): 153,31
Moving to position (X,Y): 142,31
Moving to position (X,Y): 131,31
Moving to position (X,Y): 120,31
Moving to position (X,Y): 109,31
Moving to position (X,Y): 98,31
Moving to position (X,Y): 87,31
Moving to position (X,Y): 76,31
Moving to position (X,Y): 65,31
Moving to position (X,Y): 54,31
Moving to position (X,Y): 43,31
Moving to position (X,Y): 32,31
Moving to position (X,Y): 21,31
Moving to position (X,Y): 10,31
Moving to position (X,Y): 10,39
Moving to position (X,Y): 21,39
Moving to position (X,Y): 32,39
Moving to position (X,Y): 43,39
Moving to position (X,Y): 54,39
Moving to position (X,Y): 65,39
Moving to position (X,Y): 76,39
Moving to position (X,Y): 87,39
Moving to position (X,Y): 98,39
Moving to position (X,Y): 109,39
Moving to position (X,Y): 120,39
Moving to position (X,Y): 131,39
Moving to position (X,Y): 142,39
Moving to position (X,Y): 153,39
Moving to position (X,Y): 164,39
Moving to position (X,Y): 175,39
Moving to position (X,Y): 186,39
Moving to position (X,Y): 186,47
Moving to position (X,Y): 175,47
Moving to position (X,Y): 164,47
Moving to position (X,Y): 153,47
Moving to position (X,Y): 142,47
Moving to position (X,Y): 131,47
Moving to position (X,Y): 120,47
Moving to position (X,Y): 109,47
Moving to position (X,Y): 98,47
Moving to position (X,Y): 87,47
Moving to position (X,Y): 76,47
Moving to position (X,Y): 65,47
Moving to position (X,Y): 54,47
Moving to position (X,Y): 43,47
Moving to position (X,Y): 32,47
Moving to position (X,Y): 21,47
Panorama complete
4

There is too much program to figure out a quick answer to your problem

I suggest that you modify the lines

Serial.print("Moving to position (X,Y): ")

to make each one unique so you know which one is being printed. Maybe just add in an A, B etc.

It may help you to see where the error arises.

I can't help wondering if your maths needs to be so complicated. I would have thought that the pan between frames (and the tilt) would be a fixed value (P steps, or T steps).

...R

5

The maths is complicated because it will change the values if the focal length changes. This then changes the number of photos required per axis, as well as the field of view between the X and Y axis and so on.

The pan, and then the tilt would then change, and the start and end points may also change allowing more flexibility.

I think from the code the issue is somewhere between the first loop and the next piece, it starts off by moving left to right, and then it should move up and go right to left, but it jumps to far up, then comes back.

  while(pStepXPos < BottomX){
Serial.print("Moving to position (X,Y): ") & Serial.print(pStepXPos) & Serial.print(",") & Serial.println(pStepYPos);
delay(sDelay);
D5200.shutterNow();
delay(cDelay);
pStepXPos = pStepXPos + DegStepsX;}

while(pStepYPos < BottomY){
Serial.print("Moving to position (X,Y): ") & Serial.print(pStepXPos) & Serial.print(",") & Serial.println(pStepYPos);
delay(sDelay);
D5200.shutterNow();
delay(cDelay);
pStepYPos = pStepYPos + DegStepsY;
  YCounter = YCounter + 1;
  
  if ( (YCounter % 2) == 0) {
    
   while(pStepXPos < BottomX){
Serial.print("Moving to position (X,Y): ") & Serial.print(pStepXPos) & Serial.print(",") & Serial.println(pStepYPos);
delay(sDelay);
D5200.shutterNow();
delay(cDelay);
pStepXPos = pStepXPos + DegStepsX;}

} else {
   while(pStepXPos > TopX){
Serial.print("Moving to position (X,Y): ") & Serial.print(pStepXPos) & Serial.print(",") & Serial.println(pStepYPos);
delay(sDelay);
D5200.shutterNow();
delay(cDelay);
pStepXPos = pStepXPos - DegStepsX;}
}
 }
6

TravisH:
The maths is complicated because

That was not my main comment. Did you try my suggestion?

...R

7

Hi Robin,

I did try your suggestion, and can narrow it down to the Y axis movement. When I annotate which loop is running the serial output gives the below, I am a bit confused why it would jump as I only increment by 1 multiple step, and it does not continue to jump, i.e. the Y axis does not stay at 23, it goes to 23 then jumps to 31 where it should be:

A (1st X Pan): Moving to position (X,Y): 164,15
A (1st X Pan): Moving to position (X,Y): 175,15
B (Y Pan): Moving to position (X,Y): 186,15
B (Y Pan): Moving to position (X,Y): 186,23
D (X Pan R-->L): Moving to position (X,Y): 186,31
D (X Pan R-->L): Moving to position (X,Y): 175,31

This is based on the code:

int YCounter = 1;
pStepYPos = TopY;
pStepXPos = TopX;

   while(pStepXPos < BottomX){
Serial.print("A (1st X Pan): Moving to position (X,Y): ") & Serial.print(pStepXPos) & Serial.print(",") & Serial.println(pStepYPos);
delay(sDelay);
D5200.shutterNow();
delay(cDelay);
pStepXPos = pStepXPos + DegStepsX;}

while(pStepYPos < BottomY){
Serial.print("B (Y Pan): Moving to position (X,Y): ") & Serial.print(pStepXPos) & Serial.print(",") & Serial.println(pStepYPos);
delay(sDelay);
D5200.shutterNow();
delay(cDelay);
pStepYPos = pStepYPos + DegStepsY;
  YCounter = YCounter + 1;
  
  if ( (YCounter % 2) == 0) {
    
   while(pStepXPos < BottomX){
Serial.print("C (X Pan L-->R): Moving to position (X,Y): ") & Serial.print(pStepXPos) & Serial.print(",") & Serial.println(pStepYPos);
delay(sDelay);
D5200.shutterNow();
delay(cDelay);
pStepXPos = pStepXPos + DegStepsX;}

} else {
   while(pStepXPos > TopX){
Serial.print("D (X Pan R-->L): Moving to position (X,Y): ") & Serial.print(pStepXPos) & Serial.print(",") & Serial.println(pStepYPos);
delay(sDelay);
D5200.shutterNow();
delay(cDelay);
pStepXPos = pStepXPos - DegStepsX;}
}
 }
 Serial.println("Panorama complete");
8

Where is the closing } for this line

while(pStepYPos < BottomY){

...R

9

Sorry, should have mentioned that it is not the complete code, but the closing point is just before Panorama complete.

So the logic of the code at that point, which seems to work (minus the one error position) is:

Move Left to right from start to end point
YCounter=1
Loop Move up 1 (* x degrees) movement on the Y axis
If YCounter=Odd then
Loop Move Left to Right on the X Axis from start to finish, whilst keeping Y axis fixed
else if YCounter=Even then
Loop Move Right to Loop on the X Axis from finish to start, whilst keeping Y axis fixed
Close If
Increment YCounter+1
Continue Y Loop
End Loop & Say Complete.

10

It was very annoying to realize you have another Thread on the same subject so that not all of the info and advice is here.

TravisH:
Sorry, should have mentioned that it is not the complete code,

This is also frustrating as it seems it was a complete waste of time looking at the code.

The erroneous value is in a variable called pStepYpos. I suggest you arrange to print that value every time and place that it changes.

Your code seems to be very convoluted - and even more so if you think the } that I thought was missing is actually in the right place. You have two WHILEs and an IF within another WHILE. You also have the code for the actions that those tests control mixed within all of that. There must be a simpler way - which would also expose the problem.

...R

11

Hi Robin2,

The other post is not on the same subject, it is similar but the other post is asking the question how can I alternate between a loop ascending and a loop descending and was an earlier post than this one. I did get some resolution to that post and updated the code accordingly.

The info / advice in the other thread relates to alternating between incrementing and decreasing loops within another loop, whereas this post is trying to work out why there is a strange error in a loop whihc does not persist (e.g. it increases on the Y, then goes back to what it should be).

Thanks for taking a look, I will do a print of the Y step pos and see if I can work out what is happening.The code is convoluted, I am still new to all of this, and there are a number of loops that are designed to achieve different things, for example one while is designed to move incrementally up the Y axis, and within that while loop is an if statement which determines if the pan axis should move left to right or right to left.

Thanks for trying to work out what was happening.

12

TravisH:
Thanks for trying to work out what was happening.

You might get some useful ideas about organizing your code in the Thread planning and implementing a program

My idea is to separate the control logic from the action code so you can see the control stuff without being distracted by the action stuff.

And you can put a piece of the "action stuff" in a separate short sketch and work on it until it is correct without needing any of the control logic.

...R

13

Without trying to take it too much off topic, in terms of separating the control code from the action logic should I move some of it to functions and just pass the variable through the function. I am thinking about how best to avoid duplication. Would it be better to create a loop function and then pass I want to loop across the Y axis increasing, or X axis decreasing and so on which might make the code easier to read and understand.

I am still slightly puzzled, I do agree with your comment about the Y axis position variable (pStepYpos) being at least in part the issue, however where I am really confused is the fact that the position increases more than it should but then goes to where it actually should be.

In the below example it is highlighted, the Y pan moves up to 23 does not do any of the X axis panning and then jumps to 31 where it actually should be. The thing that confuses me even more is that in the code it moves up on the Y axis fine, but there is no real check in the loops so it should move up, do the X thing and move on.

The only thing I can possibly think is that maybe the issue lies with the X axis pan, for example it moves up on the Y axis, and since the X axis position is already at the far point it moves up again on the Y axis (would changing the odd to even possibly fix that?)

A (1st X Pan): Moving to position (X,Y): 164,15
A (1st X Pan): Moving to position (X,Y): 175,15
B (Y Pan): Moving to position (X,Y): 186,15
B (Y Pan): Moving to position (X,Y): 186,23
D (X Pan R-->L): Moving to position (X,Y): 186,31
D (X Pan R-->L): Moving to position (X,Y): 175,31

14

TravisH:
I am still slightly puzzled, I do agree with your comment about the Y axis position variable (pStepYpos) being at least in part the issue, however where I am really confused is the fact that the position increases more than it should but then goes to where it actually should be.

I don't have your hardware so I can't try your program myself.

When I run into a problem like this I write a very simple program that goes through the same process so I can monitor it more closely. I would probably just use byte values and do no maths if it could be avoided.

When I got that working I would start adding complexities a piece at a time - making sure each works before moving on.

I also find it important to think about debugging while I am designing the program - "how will I know this is working? and how will I find a problem if it is not?"

This is why I like to write my code in small functions. If I know a function works then I also know to look somewhere else for the problem.

It is surprisingly easy to get confused by complex or nested conditions because you may not have planned for all the possible cases.

...R

15

TravisH:
Sorry, should have mentioned that it is not the complete code, but the closing point is just before Panorama complete.

AAAAAAAAAAGGHHH. Do you know how annoying that is? To reverse engineer over 230 lines of code just to find you haven't got the complete code. It's worse than reading a novel with the last page ripped out.