RoboRealm + Arduino

Hello everyone,

I’m not very skillful when it comes to programming but I get my experience from reading online references. I am currently having an issue with my robot. I’m using roborealm to locate an object. The camera is mounted on the base and the wheels are programmed to rotate and find the object’s center mass. Once this is complete, I would like the robot to move forward or backwards to a designated distance. I have my code, everything works great except for moving forward or backwards. I know I’m doing something wrong but not sure what it is. I would appreciate any feedback, thank you.

//============================================================================
// Variable and Initial Conditions Setup
//============================================================================
#include <Servo.h> // Include the servo library
Servo LW;
Servo RW;
int tolerance = 40;                    // Tolerance range from x,y axies
int centerTolerance = 5;               // Tolerance range to help recenter servos
int imageX = 640;                      // Image width
int imageY = 480;                      // Image hieght

int count = 0;
int pos = 0;
int x, y, d;                           // x=[COG_X], y=[COG_Y], d=[COG_BOX_SIZE]
int var[3];                            // Stores x,y,d
char command[3];                       // Stores characters 'P'-pan, 'T'-tilt, 'D'-distance

void setup()
{
  Serial.begin(115200);                // Open the serial port
  Serial.println("Serial port ready"); // Print on screen
  RW.attach(7);
  LW.attach(8);
}


//============================================================================
// Main Loop
//============================================================================
void loop()
{
  if (Serial.available()){              // Do stuff if serial available
//============================================================================
// Data Parsing Algorithm
// RoboRealm serial send sequence:  P[COG_X];T[COG_Y];D[COG_BOX_SIZE];
//============================================================================
    char ch = Serial.read();
    if(ch >= '0' && ch <= '9')	{	// Check if character is a number
	 pos = pos * 10 + ch - '0';	// If yes, accumulate the value
    }
    else if(ch != ';'){                 // Check for delimiter
	 command[count] = ch;
	 pos = 0;
    }
    if( ch == ';' ){ 
        var[count] = pos;
	if(command[0] == 'P'){          //Pan variable
            x = var[0];
        }
        //if(command[1] == 'T'){          //Tilt variable
        //    y = var[1];
        //}
        if(command[2] == 'D'){          //Distance(BoxSize) variable
            d = var[2];
        }
        count++;
        pos = 0;   
     }      
//============================================================================
// Servo/Motor Contol Algorithms
//============================================================================
// Print back parsed data
       if(count == 3){     
            for (int i = 0; i < 3; i++) {
              Serial.print(command[i]);Serial.print(var[i]);Serial.print(' ');
              }
// Pan servo algorithm
        if(x < (imageX/2 - tolerance) && x != 0){        // If x left of y-axis move right
          LW.writeMicroseconds(1600);  
          RW.writeMicroseconds(1600);  
          delay(100);
          LW.writeMicroseconds(1500);  // Stop point
          RW.writeMicroseconds(1500);
        }
        
        else if(x > (imageX/2 + tolerance) && x != 0){   // If x right of y-axis move left
          LW.writeMicroseconds(1400);  
          RW.writeMicroseconds(1400);
          delay(100);
          LW.writeMicroseconds(1500);  // Stop point
          RW.writeMicroseconds(1500);
        }


//Here is where my issue starts, servos do not move        
        if (d > 100)
        {
          RW.writeMicroseconds(1000);
          LW.writeMicroseconds(2000);
        }
        
        else if(d < 100)
        {
          RW.writeMicroseconds(2000);
          LW.writeMicroseconds(1000);
        }
        
        else (d=100);
        {
          RW.writeMicroseconds(1500);
          LW.writeMicroseconds(1500);
        }
        count = 0;                                       // Reset count to retrieve next data packet
       }     
//============================================================================
// END
//============================================================================ 
  }
}

I have my code, everything works great except for moving forward or backwards. I know I’m doing something wrong but not sure what it is. I would appreciate any feedback, thank you.

What exactly is the problem? You have code to run the motors some amount, under various conditions. Is there something wrong with that code?

Yes there is. It finds the object but when the object is to far it should move forward and vice versa. However, it does not move forward or backwards. It turns left or right depending where my object is and it locks center to the object. I would like it to go forward or backwards when necessary. Here is the loop function I have.

//============================================================================
// Main Loop
//============================================================================
void loop()
{
  if (Serial.available()){              // Do stuff if serial available
//============================================================================
// Data Parsing Algorithm
// RoboRealm serial send sequence:  P[COG_X];T[COG_Y];D[COG_BOX_SIZE];
//============================================================================
    char ch = Serial.read();
    if(ch >= '0' && ch <= '9')	{	// Check if character is a number
	 pos = pos * 10 + ch - '0';	// If yes, accumulate the value
    }
    else if(ch != ';'){                 // Check for delimiter
	 command[count] = ch;
	 pos = 0;
    }
    if( ch == ';' ){ 
        var[count] = pos;
	if(command[0] == 'P'){          //Pan variable
            x = var[0];
        }
        //if(command[1] == 'T'){          //Tilt variable
        //    y = var[1];
        //}
        if(command[2] == 'D'){          //Distance(BoxSize) variable
            d = var[2];
        }
        count++;
        pos = 0;   
     }      
//============================================================================
// Servo/Motor Contol Algorithms
//============================================================================
// Print back parsed data
       if(count == 3){     
            for (int i = 0; i < 3; i++) {
              Serial.print(command[i]);Serial.print(var[i]);Serial.print(' ');
              }
// Pan servo algorithm
        if(x < (imageX/2 - tolerance) && x != 0){        // If x left of y-axis move right
          LW.writeMicroseconds(1600);  
          RW.writeMicroseconds(1600);  
          delay(100);
          LW.writeMicroseconds(1500);  // Stop point
          RW.writeMicroseconds(1500);
        }
        
        else if(x > (imageX/2 + tolerance) && x != 0){   // If x right of y-axis move left
          LW.writeMicroseconds(1400);  
          RW.writeMicroseconds(1400);
          delay(100);
          LW.writeMicroseconds(1500);  // Stop point
          RW.writeMicroseconds(1500);
        }


//Here is where my issue starts, servos do not move forward or backwards according to the distance      
        if (d > 100)
        {
          RW.writeMicroseconds(1000);
          LW.writeMicroseconds(2000);
        }
        
        else if(d < 100)
        {
          RW.writeMicroseconds(2000);
          LW.writeMicroseconds(1000);
        }
        
        else (d=100);
        {
          RW.writeMicroseconds(1500);
          LW.writeMicroseconds(1500);
        }
        count = 0;                                       // Reset count to retrieve next data packet
       }

There is no indication, in the code you posted, that var[2] is ever assigned a value, or what that value is. Since d may, or may not, ever be assigned that value, it is important to know that var[2] has a value, what that value is, and that conditions are ever right for that value to be assigned to d.

Thank you for the expedient response Paul. I understand where your coming from but my code also has a Var 0 which is working as it should although Var 2 is not. The values are coming from Roborealm. I just don't quite understand why Val 2 is not updating. I could attach anything that can be of any help. Once again,thank you for your expertise Paul.