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
//============================================================================
}
}