Hello Geniuses,
This code is for a Differential Drive Mobile Robot Class.
The robot should change each of its left and right wheel velocity from [-1.0 to 1.0] .
// I used nested for loop for this.
In each specific velocity value for left and right wheel, the robot will go in a circular trajectory.
I need to calculate the diameter of this circle by calculating the distance between the first point and current point of its trajectory (then stop at the longest distance).
I have wrote the code – but it doesn’t work.
class Robot {
float diameter;
float distance;
int index = 1; //index for number of outputs
int condition; //if (Nl == Nr or Nl = -Nr) = 0;
float [] velocityLeft = {1.0, 0.9, 0.8, 0.7, 0.6, 0.5, 0.4, 0.3, 0.2, 0.1, 0, -0.1, -0.2, -0.3, -0.4, -0.5, -0.6, -0.7, -0.8, -0.9, -1.0};
float [] velocityRight = {1.0, 0.9, 0.8, 0.7, 0.6, 0.5, 0.4, 0.3, 0.2, 0.1, 0, -0.1, -0.2, -0.3, -0.4, -0.5, -0.6, -0.7, -0.8, -0.9, -1.0};
float Nl; //Right wheel velocity
float Nr; //Left wheel velocity
float theta = radians(0); //Angle with -x-axis
ArrayList<Float> positionX = new ArrayList<Float>();
ArrayList<Float> positionY = new ArrayList<Float>();
float radius = 3; //DDMR wheel radius
float L = 6; //Distance between two wheels
float vx;
float vy;
PVector velocity = new PVector(vx, vy); //linear velocity
PVector location; //DDMR current location
float omega; //Angular velocity
float initialX;
float initialY;
Robot (){
initialX = 300;
initialY = 300;
location = new PVector (initialX, initialY); // Initialize the location.
}
void display(){
for (int iterationLeft = 0; iterationLeft < velocityLeft.length; iterationLeft ++)
{
for (int iterationRight = 0; iterationRight < velocityRight.length; iterationRight ++)
{
condition = 1;
diameter = 0;
distance = 0;
Nl = velocityLeft[iterationLeft];
Nr = velocityRight[iterationRight];
velocity.x = (radius/2)*cos(theta)*(Nr + Nl);
velocity.y = (radius/2)*sin(theta)*(Nr + Nl);
omega = radians((radius/(2*L))*(Nl-Nr));
if (positionX.size() > 0){
for (int iterationDelete = 0; iterationDelete < positionX.size(); iterationDelete ++)
{
positionX.remove(iterationDelete);
positionY.remove(iterationDelete);
}//end (for) iterationDelete
}//end (if) statement
if (Nl == Nr)
{
condition = 0;
print (index + ": Diameter = Infinity - Straight Movement - " + "Left Speed = " + Nl + " - Right Speed = " + Nr + "\n");
index ++;
} else if (Nl + Nr == 0)
{
condition = 0;
print (index + ": Diameter = 0 - In place turning " + "Left Speed = " + Nl + " - Right Speed = " + Nr + "\n");
index ++;
}
while (condition == 1)
{
//Save the all location values of x and y in an ArrayList
positionX.add(location.x);
positionY.add(location.y);
location.add(velocity);
theta = theta + omega;
//Calculate the distance between the current position and the first position
int lastIndex = positionX.size()-1;
distance = dist(positionX.get(0), positionY.get(0), positionX.get(lastIndex), positionY.get(lastIndex));
if (diameter <= distance)
{
diameter = distance;
}//end of (if) statement
if (diameter > distance)
{
print(index + ": Diameter= " + diameter + " - Left Speed = " + Nl + " - Right Speed = " + Nr + "\n");
condition = 0;
index++;
}//end of (if) Statement
}//end of (While) Loop
}//end (for) iterationRight loop
}//end (for) iterationLeft loop
}//end void Display()
}//end class Robot