Sonar implementation with an ultrasonic sensor, optionally mounted on a sweep motor. More...

#include "CaliberateMotors.nxc"
#include "Circle.nxc"
#include "Angle.nxc"
#include "Vector.nxc"
#include "PID.nxc"
#include "Motor.nxc"
#include "RobotPrima.nxc"

Include dependency graph for Sonar.nxc:

This graph shows which files directly or indirectly include this file:

Go to the source code of this file.

Data Structures
struct	Sonar
	Sonar class that represents an ultrasonic sensor mounted on a robot. More...

Functions
void	SonarInit (Sonar &sonar, int sensorPort, Vector &sensorLocation, int sensorOffset, int sensorCutOffMin, int sensorCutOffMax, int toleranceAngle, long toleranceDistance, int sensorMotor, int gearRatio, int motorMovePower, int motorScanPower, int sensorMinAngle, int sensorMaxAngle)
	Initializes sonar with an ultrasonic sensor connected at sensorPort and mounted on a motor sensorMotor. More...

int	SonarGetCurrentViewAngle (Sonar &sonar, Vector &robotLocation)
	Gets current absolute view angle in the world at given robot orientation.

int	SonarGetMinViewAngle (Sonar &sonar, Vector &robotLocation)
	Gets the absolute minimum view angle in the world at given robot orientation.

int	SonarGetMaxViewAngle (Sonar &sonar, Vector &robotLocation)
	Gets the absolute maximum view angle in the world at given robot orientation.

void	SonarGetPosition (Sonar &sonar, Vector &robotLocation, Vector &retVec)
	Gets the current sonar mount position and orientation in world coordinate.

bool	SonarSweep (Sonar &sonar, int angleFrom, int angleTo, long &rangeData[])
	Sweeps the sonar from angleFrom to angleTo and returns an array of angle/distance pairs. More...

void	SonarSweepPrint (Sonar &sonar, long &rangeData[], int waitTime)
	Prints the rangeData array on screen like a radar. Used for debugging. More...

bool	SonarGetObjectDistanceAtAngle (Sonar &sonar, Vector robotLocation, int viewAngle, long viewDistance, int &retBestAngle, long &retBestDistance)
	Returns the distance of an object expected to be found at the given viewAngle (relative to sonar neutral position) and viewDistance. More...

bool	SonarGetObjectDistanceAtLocation (Sonar &sonar, Vector robotLocation, Vector &viewLocation, int &retBestAngle, long &retBestDistance)
	Returns the distance of an object expected to be found at the given viewLocation (in world coordinate). More...

bool	SonarTriangulateLandmarks (Sonar &sonar, Vector &robotLocation, Vector &landmark1, Vector &landmark2, Vector &retLocation)
	Triangulates the position robot from two known landmarks. More...

bool	SonarFindObject (Sonar &sonar, Vector robotLocation, Vector &retObject)
	Returns the absolute location of an object in the world detected by the sonar at current angle, if any was detected within the range of sensorCutOffMin and sensorCutOffMax. More...

bool	SonarFindObjectAtAngle (Sonar &sonar, Vector robotLocation, int viewAngle, long viewDistance, Vector &retObject)
	Returns the absolute location of an object found at given view angle. More...

bool	SonarFindObjectAtLocation (Sonar &sonar, Vector robotLocation, Vector viewLocation, Vector &retObject)
	Returns the absolute location of an object found around the reference view location. More...

bool	SonarFindObjectRelative (Sonar &sonar, Vector &retObject)
	Returns the relative (to robot reference) location of an object detected by the sonar, if any was detected within the range of sensorCutOffMin and sensorCutOffMax. More...

bool	SonarFindObjectAtAngleRelative (Sonar &sonar, int viewAngle, Vector &retObject)
	Returns the relative (to robot's reference point) location of an object found at given view angle. More...

Detailed Description

Sonar implementation with an ultrasonic sensor, optionally mounted on a sweep motor.

Definition in file Sonar.nxc.

Function Documentation

bool SonarFindObject	(	Sonar &	sonar,
		Vector	robotLocation,
		Vector &	retObject
	)

Returns the absolute location of an object in the world detected by the sonar at current angle, if any was detected within the range of sensorCutOffMin and sensorCutOffMax.

The object is located in robot world coordinate. retObject is the reference to Vector where object's coordinates will be be stored. It returns true if an object was found within the cutoff and refObject is updated, otherwise returns false and refObject is untouched.

robotLocation is the location of robot's point of reference in in the robot world coordinate.

Definition at line 496 of file Sonar.nxc.

References Sonar::currentViewAngle, Sonar::sensorCutOffMax, Sonar::sensorCutOffMin, Sonar::sensorLocation, Sonar::sensorOffset, Sonar::sensorPort, Vector::theta, VectorAdd(), and VectorTranslate().

bool SonarFindObjectAtAngle	(	Sonar &	sonar,
		Vector	robotLocation,
		int	viewAngle,
		long	viewDistance,
		Vector &	retObject
	)

Returns the absolute location of an object found at given view angle.

This is exactly same as SonarObjectLocation() except this function first sets the angle of sonar motor to given view angle first. The angle is relative to the neutral position of the sonar (not the robot). Left (anticlockwwise) is positive and Right (clockwise) is negative.

Definition at line 536 of file Sonar.nxc.

References Sonar::sensorLocation, Sonar::sensorMaxAngle, Sonar::sensorMinAngle, SonarSweep(), Vector::theta, Sonar::toleranceAngle, VectorAdd(), and VectorTranslate().

Referenced by SonarFindObjectAtLocation().

bool SonarFindObjectAtAngleRelative	(	Sonar &	sonar,
		int	viewAngle,
		Vector &	retObject
	)

Returns the relative (to robot's reference point) location of an object found at given view angle.

This is exactly same as SonarObjectLocationRelative() except this function first sets the angle of sonar motor to given view angle first. The angle is relative to the neutral position of the sonar. Left is (anticlockwise) is positive and Right (clockwise) is negative.

Definition at line 685 of file Sonar.nxc.

References Sonar::currentViewAngle, Sonar::gearRatio, Sonar::motorMovePower, Sonar::sensorMaxAngle, Sonar::sensorMinAngle, Sonar::sensorMotor, and SonarFindObjectRelative().

bool SonarFindObjectAtLocation	(	Sonar &	sonar,
		Vector	robotLocation,
		Vector	viewLocation,
		Vector &	retObject
	)

Returns the absolute location of an object found around the reference view location.

This is exactly same as SonarObjectLocationAtAngle() except this function determines the view angle from the reference view object and sets the angle of sonar motor to it. The reference view object is located in world coordinate. The viewLocation vector is only used for its x-y components.

Definition at line 602 of file Sonar.nxc.

References Sonar::sensorLocation, SonarFindObjectAtAngle(), Sonar::toleranceDistance, VectorAdd(), VectorGetAngle(), VectorGetDistance(), and VectorSubtract().

bool SonarFindObjectRelative	(	Sonar &	sonar,
		Vector &	retObject
	)

Returns the relative (to robot reference) location of an object detected by the sonar, if any was detected within the range of sensorCutOffMin and sensorCutOffMax.

The object is located in a world coordinate whose origin is the robot's referece point. retObject is the reference to Vector where object's coordinates will be stored. It returns true if an object was found within the cut-off and refObject is updated, otherwise returns false and refObject is untouched.

Definition at line 657 of file Sonar.nxc.

References Sonar::currentViewAngle, Sonar::sensorCutOffMax, Sonar::sensorLocation, Sonar::sensorOffset, Sonar::sensorPort, Vector::theta, Vector::x, and Vector::y.

Referenced by SonarFindObjectAtAngleRelative().

bool SonarGetObjectDistanceAtAngle	(	Sonar &	sonar,
		Vector	robotLocation,
		int	viewAngle,
		long	viewDistance,
		int &	retBestAngle,
		long &	retBestDistance
	)

Returns the distance of an object expected to be found at the given viewAngle (relative to sonar neutral position) and viewDistance.

Sonar will take a short sweep around the given viewAngle (+/- toleranceAngle) and if an object is found within +/- toleranceDisance, it's most likely distance from sonar's reference point will be returned. The angle is relative to the neutral position of the sonar (not the robot). Left (anticlockwwise) is positive and Right (clockwise) is negative.

Definition at line 336 of file Sonar.nxc.

References Sonar::sensorMaxAngle, Sonar::sensorMinAngle, SonarSweep(), and Sonar::toleranceAngle.

Referenced by SonarGetObjectDistanceAtLocation().

bool SonarGetObjectDistanceAtLocation	(	Sonar &	sonar,
		Vector	robotLocation,
		Vector &	viewLocation,
		int &	retBestAngle,
		long &	retBestDistance
	)

Returns the distance of an object expected to be found at the given viewLocation (in world coordinate).

Sonar will take a short sweep around the given viewLocation (+/- toleranceAngle) and if an object is found within +/- toleranceDisance, its most likely distance from the robot's reference point will be returned. The angle viewLocation is located in world coordinate system. Note also that the returned distance is distance of the object from the robot reference point, not sonar reference point as in SonarGetObjectDistanceAtAngle(). Left (anticlockwwise) is positive and Right (clockwise) is negative.

Definition at line 383 of file Sonar.nxc.

References AngleAbsSub(), AngleAbsToRel(), SonarGetObjectDistanceAtAngle(), SonarGetPosition(), Vector::theta, VectorGetAngleVec(), and VectorGetDistanceVec().

Referenced by SonarTriangulateLandmarks().

void SonarInit	(	Sonar &	sonar,
		int	sensorPort,
		Vector &	sensorLocation,
		int	sensorOffset,
		int	sensorCutOffMin,
		int	sensorCutOffMax,
		int	toleranceAngle,
		long	toleranceDistance,
		int	sensorMotor,
		int	gearRatio,
		int	motorMovePower,
		int	motorScanPower,
		int	sensorMinAngle,
		int	sensorMaxAngle
	)

Initializes sonar with an ultrasonic sensor connected at sensorPort and mounted on a motor sensorMotor.

sensorMotor can be 0, which means the sensor is assumed to be not mounted on a motor. The sensor mount is placed on the robot at a relative offset sensorLocation from the robot's reference point. sensorOffset is the offset distance of sensor 'reading zero' from the mount. gearRatio is the Input to Ouput gear ration of the sensor motor scaled by

i.e. For 1:2 ratio = 50, 1:1 = 100, -2:1 = -200 etc. sensorMinAngle and sensorMaxAngle are the min and max angles between which sensor motor is allowed to rotate (only applicable if sensorMotor is specifiec. Reference angle '0' is the current state of motor. sensorMinAngle sensorMaxAngle are therefore relative to this. So, make sure to align the sensor motor to what would be the neutral position of the radar before calling this init funtion. Use MotorCaliberate() function to auto align it, for example. sensorCutOffMin and sensorCutOffMax are the mininum and maximum distance for an object to be detected.

Definition at line 87 of file Sonar.nxc.

References Sonar::currentViewAngle, Sonar::gearRatio, Sonar::motorMovePower, Sonar::motorScanPower, Sonar::neutralTachoCount, Sonar::pidControl, PIDControlInit(), Sonar::sensorCutOffMax, Sonar::sensorCutOffMin, Sonar::sensorLocation, Sonar::sensorMaxAngle, Sonar::sensorMinAngle, Sonar::sensorMotor, Sonar::sensorOffset, Sonar::sensorPort, Sonar::toleranceAngle, and Sonar::toleranceDistance.

bool SonarSweep	(	Sonar &	sonar,
		int	angleFrom,
		int	angleTo,
		long &	rangeData[]
	)

Sweeps the sonar from angleFrom to angleTo and returns an array of angle/distance pairs.

The angle angleFrom and angleTo are relative to sonar neutral position, zero being the neutral position. Returned array rangeData is a one dimentional integer array with angle and sense distance alternatingly filled. Some angles may not have any reading at all, either because nothing was sensed within the range of sensorCutOffMin and sensorCutOffMax, or a reading was not performed at that angle (motor sweeping pass it before making a reading). In that case, the readings will contain -1. An example return rangeData for a sweep of -10 to 10 is: [-10, -1, -9 -1, -8, 43, -7, 43, -6, 42, -5, -1, -4, -1 ...]

Definition at line 189 of file Sonar.nxc.

References PIDControl::absMaxOutput, Sonar::currentViewAngle, Sonar::gearRatio, Sonar::motorMovePower, Sonar::motorScanPower, MotorSetPower(), MotorStart(), MotorStop(), Sonar::pidControl, PIDControlCheckEnd(), PIDControlSetPoint(), PIDControlStep(), Sonar::sensorCutOffMax, Sonar::sensorCutOffMin, Sonar::sensorMotor, Sonar::sensorOffset, and Sonar::sensorPort.

Referenced by SonarFindObjectAtAngle(), and SonarGetObjectDistanceAtAngle().

void SonarSweepPrint	(	Sonar &	sonar,
		long &	rangeData[],
		int	waitTime
	)

Prints the rangeData array on screen like a radar. Used for debugging.

This function prints the returned scan data from SonarSweep() on to screen. The center bottom of the screen is the sonar location and the vertical line is the neutral orientation. So it can display readings made from -90 to +90 degrees. Readings are indicated with small circles on the screen. Radial distances are appropriately scalled to fit on the screen.

Definition at line 300 of file Sonar.nxc.

bool SonarTriangulateLandmarks	(	Sonar &	sonar,
		Vector &	robotLocation,
		Vector &	landmark1,
		Vector &	landmark2,
		Vector &	retLocation
	)

Triangulates the position robot from two known landmarks.

Sonar makes distance and angle readings of given two landmarks and triangulates the position of robot. Triangulation is performed by intersecting the two circles formed by the two landmarks having radiuses of their respective distances measured by sonar. One of the two intersection points is the triangulated location of robot. The one closer to current location is considered more likly candidate and is returned.

Definition at line 428 of file Sonar.nxc.

References AngleAbsSub(), CircleIntersectCircle(), Sonar::sensorLocation, SonarGetObjectDistanceAtLocation(), SonarGetPosition(), Vector::theta, VectorGetAngleVec(), VectorGetDistanceVec(), VectorRotate(), and VectorSubtract().

Data Structures

Functions

Detailed Description

Function Documentation