WPI Lidar explanation

VEX V5 General Discussion Technical Discussion
21 
task positionTracking() {
	x_Pos=startLocation.x;
	y_Pos=startLocation.y;
	theta=startLocation.theta;
	SensorValue[leftDrive]=0;
	SensorValue[rightDrive]=0;

	int lastLeft, lastRight, leftTicks, rightTicks;

	float leftMM, rightMM, mm;

	while (true) {
		int leftSample = SensorValue[leftDrive];
		int rightSample = SensorValue[rightDrive];

		leftTicks = leftSample - lastLeft;
		rightTicks = rightSample - lastRight;

		lastLeft = leftSample;
		lastRight = rightSample;

		leftMM = (float)leftTicks / LEFT_CLICKS_PER_MM;
		rightMM = (float)rightTicks / RIGHT_CLICKS_PER_MM;

		mm = (leftMM + rightMM) / 2.0;

		theta += (rightTicks - leftTicks) / 9.2345;

		if(theta > 180)
			theta = theta - 360;
		if(theta < -180)
			theta = 360 + theta;

		x_Pos += mm * cosDegrees(theta);
		y_Pos += mm * sinDegrees(theta);

		sleep(5);
	}
}

I also had this function that was called whenever the robot had rammed a wall. For the most part this was done automatically. If robot was driving towards a wall the ultrasonics sensors would trigger and begin turning until the values were the same to perfectly face the wall (square with wall) and then drive to bump the wall. Upon bumping the wall we could reset 2 of the 3 values involved in odometry keeping the values up to date.

void odomReset(){
	float wallOffsetFromMiddle=1500;
	float prevTheta;
	if(theta >-45 && theta < 45)
	{
		if(abs(theta) < 10)
		{
			prevTheta = theta;
			theta = 0.0;
			x_Pos=wallOffsetFromMiddle;

		}
	}
	else if(theta > 45 && theta < 135)
	{
		if(abs(theta - 90) < 10)
		{
			prevTheta = theta;
			theta = 90.0;
			y_Pos=wallOffsetFromMiddle;
		}
	}
	else if((theta > 135 && theta <= 180) || (theta < -135 && theta > -180))
	{
		if(abs(theta - 180) < 10||abs(-theta-180)<10)//both cases
		{
			prevTheta = theta;
			theta = 180.0;
			x_Pos=-1*wallOffsetFromMiddle;
		}
	}
	else if(theta < -45 && theta > -135)
	{
		if(abs(theta + 90) < 10)
		{
			prevTheta = theta;
			theta = -90.0;
			y_Pos=-1*wallOffsetFromMiddle;
		}
	}
	writeDebugStreamLine("Fixed odometry for you... There was an %1.5f difference in theta",abs(theta - prevTheta));
}
22

Why 9.2345? Was that experimentally determined, or is it mathematically derived?

23

All constants were derived for initial values and then tested over extended motions for minute percentage differences. Usually something like 10 full rotations to determine accuracy for turns. 4 inch omni wheels are bumpy and average to be not 4 inches and angle is off mathematically because center of rotation isnt perfect.

24

Thanks for that :slight_smile:

25

This is the simplest way to track position. Just treat movement as lots of tiny discrete turns and line segments.

Just like in calculus when you find the area under a curve using a single rectangle it is incredibly inaccurate. You want to have as many rectangles as possible. So making the time step between iterations of the loop as small as possible is incredibly important.

26

So after being asked somewhere else I took all the LIDAR code for parsing the data and object detection out and put it on its own repo. There is honestly a really good chance it would detect cones out of the box if mounted at the right height. I went through a big explanation of the pipeline at the top of the thread but rereading it has refreshed it in my mind if anyone has questions.

27

Cool can I get added to that private git repo. And do you know how to make ultrasonic sensors more accurate.

28

So recently after being asked about it I moved all the LIDAR code to a separate repository and made it public. (assuming thats the part you were looking for )

The ultrasonics sensors have pretty high accuracy in my experience. I am actually a big fan of them.

29

Thank you a lot. Can you also explain which were your global variables in the position tracking task. And what is “startLocation”…do I need a custom library for this code?

30

So the only 3 global variables were

x_Pos=startLocation.x;
y_Pos=startLocation.y;
theta=startLocation.theta;

You can use whatever way you want to set the values to the starting values. Or set them all to 0 if you want to have coordinate relative to where you started.

No extra libraries needed.

31

ohhh ok the how did you define your constants. Is it different for the gearing in your motor?

32

The size wheel would change it as would the gear ratio between the wheel and the encoder. Also the width of the chassis.