Help needed with Triangulation for robot to accurately move to location

Hello, I looked around and have found a few forum post about this, but I was having a little difficulty understanding how to implement this into my project. I’m very new at this so I apologize if this is painfully simple.

In my engineering computations class we are programming to have a robot locate a bottle and pick it up (tank drive, moving robot), most other students have the robot moving in a circle until it see the bottle, then moving forward. I noticed that this is fairly inaccurate as the gripper arm gets caught on the bottle cap half the time (y-axis movement).

I’m looking to have my code store the servo motor location for when it first detects the bottle, continue to turn, then store the motor location for when it loses the bottle, find the average between the two and move the robot to that position. Then the robot can move forward blah blah blah. My only problem is that I dont know how to store the motor location in the arduino’s volatile memory, or how to write the new position to the robot.

Would anyone be able to help me out with this? Thanks :slight_smile:

Here is code

#include "MeMegaPi.h"

//Sensor
MeUltrasonicSensor ultraSensor(PORT_6);

//Pins for reading and writing servo motor location
int pos = 0;
int servoL = PORT1A;      //Do not know, value is placeholder
int servoR = PORT2A;      //Do not know, value is placeholder

//Motor 1 (Left)
MeMegaPiDCMotor motor1(PORT1A);

MeMegaPiDCMotor motor2(PORT1B);

//Motor 2 (Right)
MeMegaPiDCMotor motor3(PORT2A);

MeMegaPiDCMotor motor4(PORT2B);

//Gantry (Y-axis)
MeMegaPiDCMotor motor5(PORT3A);

MeMegaPiDCMotor motor6(PORT3B);

//Gripper (X-axis)
MeMegaPiDCMotor motor8(PORT4B); //first slot is A second is B

uint8_t motorSpeed = 50;

//Create a servo object
Servo posL;
Servo posR;

void setup()
{ 
  Serial.begin(9600);
  posL.attach(servoL);
  posR.attach(servoR);
}

void loop()
{
  //lifting gripper up so it does not knock into bottle
        motor5.run(100);
        motor6.run(100);
        delay(1600);
        motor5.stop();
        motor6.stop();
        delay(40);

//Turning to locate bottle
        while(ultraSensor.distanceCm() > 80)
        {
          motor1.run(motorSpeed);
          motor2.run(motorSpeed);
          motor3.run(motorSpeed);
          motor4.run(motorSpeed); 
        }

//Located side one of bottle
        if(ultraSensor.distanceCm() < 80 && ultraSensor.distanceCm() > 13)
        {
          //I want to store the 1st position of both servos here
          posL = //Don't know how to save servo location. Will save it to variable PosL1
          posR = //Don't know how to save servo location. Will save it to variable PosR1
          
        }
        
        while(ultraSensor.distanceCm() < 80 && ultraSensor.distanceCm() > 13)
        {
           motor1.run(50);
           motor2.run(50);
           motor3.run(50);
           motor4.run(50);
        }
        
//Sight of bottle waws broken, 1st while loop will not start until whole program has run
//Locating 2nd side of bottle
         motor1.stop();
         motor2.stop();
         motor3.stop();
         motor4.stop();
         //I want to store the 2st position of both servos here
         posL = //Don't know how to save servo location. Will save it to variable PosL2
         posR = //Don't know how to save servo location. Will save it to variable PosR2
                 
//Robot centering itself against bottle
         pos.write((pos1 + pos2) / 2.0) //Probably not right

//Moving towards the bottle
         while(ultraSensor.distanceCm() > 18)
         {
           motor1.run(motorSpeed);
           motor2.run(motorSpeed);
           motor3.run(-motorSpeed);
           motor4.run(-motorSpeed);
         }

//Dropping gripper down onto bottle
         motor5.run(-50);
         motor6.run(-50);
         delay(5000);
         motor5.stop();
         motor6.stop();

//Closing gripper 
         motor8.run(25);
         delay(5000);
         motor8.stop();

//Lifting bottle in triumph
         motor5.run(50);
         motor6.run(50);
         delay(5000);
         motor5.stop();
         motor6.stop();

//Do a little dance
        for(int x = 0; x < 3; x++)
         {
         motor1.run(-motorSpeed);
         motor2.run(-motorSpeed);
         motor3.run(-motorSpeed);
         motor4.run(-motorSpeed);
         delay(1000);
         motor1.run(motorSpeed);
         motor2.run(motorSpeed);
         motor3.run(motorSpeed);
         motor4.run(motorSpeed);
         delay(1000);
         }

//Dropin the mic
         motor5.run(-50);
         motor6.run(-50);
         delay(5000);
         motor5.stop();
         motor6.stop();

//Opening gripper 
         motor8.run(-25);
         delay(5000);
         motor8.stop();

         return 0;
}

Using 4 DC motors is questionable, 2 were easier and more precise to handle.

Does your robot really turn in place? How many encoder ticks for a full turn?

It is 2 motors to turn the robot, my professor told me why I needed to code for four, but it didn't really make much sense. They are continuous servo motors if that helps.

I'm running them continuous and not in a run(x), delay(x). I assume that's what you mean by the ticks

Does your servo library include position feedback? If not, you only can assume that motor movement is proportional to the given value, what most probably does not take into account start/stop ramps. Test yourself, whether a move or turn of double time will yield double distance or angle.

For proper position reading you need a rotary encoder for each motor.

NoobBaby:
My only problem is that I dont know how to store the motor location in the arduino's volatile memory,

Unless I am missing something (which is quite possible) that seems like the simplest of programmng tasks. Something like

motorLocation = NNN;

where NNN is the number you want to store

or how to write the new position to the robot.

I suspect you mean "how to calculate the new direction"

When you say "store the servo motor location for when it first detects the bottle, continue to turn, then store the motor location for when it loses the bottle, find the average between the two and move the robot to that position." it seems to me what you are identifying is the angle of the bottle relative to the robot rather than the position of the bottle.

You have not told us what sensor you are using to detect the bottle.

I think you need to know what servo position corresponds to straight-ahead - let's suppose it is 90° and that higher values are to the right (clockwise). And suppose the servo sweep detects the bottle at 110° and loses it at °118°. The average of those two is 114°. That means that robot needs to turn 114 - 90 = 24° clockwise to be facing the bottle.

Is that the sort of thing you want to figure out?

...R

What you describe applies to rotational servos, which are controlled by angle. Continuous servos instead are controlled by speed.