Hello all,
I am working on a return to origin function for my elegoo smart car robot v1.0. The project is getting the robot from some location that I would not know back to the origin. In order to keep track of the location, I have implemented an optical sensor from a mouse to return x and y coordinates.
I am struggling with the logic of how to tell the robot it is moving in the correct/incorrect direction. I am currently using a ratio of the x/y position to the total x/y ratio and adjusting the speed on one wheel accordingly. Is there a better way to go about this?
Here is the code I currently have for the goHome function:
void goHome()
{
Serial.println("Going Home!");
delay(200);
int newX = 0;
int newY = 0;
int distX = 0;
int distY = 0;
int myRatio = 1;
int x,y;
uint8_t stat;
while (((totalX + newX) > 100) && ((totalY + newY) > 100))
{
mouse.getPosition(stat,x,y);
// Case 1: Robot moving in correct direction
// X value is negative, Y value is negative
if ((x < 0) && (y < 0))
{
distX = (newX + x) + totalX;
distY = (newY + y) + totalY;
myRatio = (distY/distX);
int lMotor = myRatio * ABS;
int rMotor = ABS;
digitalWrite(ENA,rMotor);
digitalWrite(ENB,lMotor);
digitalWrite(in1,LOW);//digital output
digitalWrite(in2,HIGH);
digitalWrite(in3,LOW);
digitalWrite(in4,HIGH);
delay(mDelay);
}
// Case 2: X is correct but Y is not correct
// X value is negative and Y value is positive
if ((x < 0) && (y > 0))
{
distX = (newX + x) + totalX;
distY = (newY + y) + totalY;
myRatio = (distY/distX);
int lMotor = myRatio * ABS;
int rMotor = ABS;
digitalWrite(ENA,rMotor);
digitalWrite(ENB,lMotor);
digitalWrite(in1,LOW);//digital output
digitalWrite(in2,HIGH);
digitalWrite(in3,LOW);
digitalWrite(in4,HIGH);
delay(mDelay);
}
// Case 3: X is incorrect but Y is correct
// X value is positive and Y value is negative
if ((x > 0) && (y < 0))
{
distX = (newX + x) + totalX;
distY = (newY + y) + totalY;
myRatio = (distY/distX);
int lMotor = myRatio * ABS;
int rMotor = ABS;
digitalWrite(ENA,rMotor);
digitalWrite(ENB,lMotor);
digitalWrite(in1,LOW);//digital output
digitalWrite(in2,HIGH);
digitalWrite(in3,LOW);
digitalWrite(in4,HIGH);
delay(mDelay);
}
// Case 4: X is incorrect and Y is incorrect
// X value is positive
if ((x > 0) && (y > 0))
{
distX = (newX + x) + totalX;
distY = (newY + y) + totalY;
myRatio = (distY/distX);
int lMotor = myRatio * ABS;
int rMotor = ABS;
digitalWrite(ENA,rMotor);
digitalWrite(ENB,lMotor);
digitalWrite(in1,LOW);//digital output
digitalWrite(in2,HIGH);
digitalWrite(in3,LOW);
digitalWrite(in4,HIGH);
delay(mDelay);
}
}
}
Any help would be much appreciated. Thanks for your time.