robotmath.h

一个机器人的源代码.软件设计得超级好!是商业级代码.

// RobotMath.h
//
// Misc. math functions
#pragma once

#include <vector>
using namespace std;

// Constants:
const int ciNumAnalogInputs = 8;	// analog inputs per I/O board (per PMD Pilot chip)
const int ciNumDigitalInputs = 8;	// digital inputs per I/O board (per PMD Pilot chip)
const int ciNumDigitalOutputs = 8;  // digital outputs per I/O board (per PMD Pilot chip)
const int ciMicrostepsPerStep = 64;	// set PMD stepper to count 64 microsteps per step
const int ciMotorStepsPerRevolution = 200;
const int ciPMD3410CycleTime = 154;  // microseconds

const double cdMicrosecondsPerSecond = 1000000.0;
const double cdPMD3410CycleTime = (double)ciPMD3410CycleTime;  // microseconds
const double cdMicrostepsPerStep = (double)ciMicrostepsPerStep;
const double cdMotorStepsPerRevolution = (double)ciMotorStepsPerRevolution;
const double cdGearRatio = 4.8;
const double cdPi = 3.141592653589793;
const double cdWheelRadius = 62.5;  // mm
const double cdWheelCircumference = cdWheelRadius * 2.0 * cdPi;  // mm
const double cdWheelBaseRadius = 150.0;  // mm (half the measured wheel-to-wheel distance of @30 cm)
const double cdWheelBaseCircumference = cdWheelBaseRadius * 2.0 * cdPi;  // mm distance traced by each wheel as robot does a 360

// Measured 21 cm (210 mm) from axle to edge of forward sensor.
const int ciRobotFrontSensorRadius = 210;  // in mm

// Conversion factors (multiplicative)
const double cdCyclesToSeconds = cdPMD3410CycleTime / cdMicrosecondsPerSecond;
const double cdSecondsToCycles = 1.0 / cdCyclesToSeconds;
const double cdMillimetersToMicrosteps = (cdGearRatio * cdMotorStepsPerRevolution * cdMicrostepsPerStep) / cdWheelCircumference;
const double cdMicrostepsToMillimeters = 1.0 / cdMillimetersToMicrosteps;
const double cdHumanToPMDVelocity = cdMillimetersToMicrosteps / cdSecondsToCycles;  // mm/second to microsteps/cycle
const double cdPMDToHumanVelocity = 1.0 / cdHumanToPMDVelocity;						// microsteps/cycle to mm/second
const double cdHumanToPMDAcceleration = cdHumanToPMDVelocity / cdSecondsToCycles;   // mm/second^2 to microsteps/cycle^2
const double cdPMDToHumanAcceleration = 1.0 / cdHumanToPMDAcceleration;				// microsteps/cycle^2 to mm/second^2
const double cdHumanToPMDJerk = cdHumanToPMDAcceleration / cdSecondsToCycles;		// mm/second^3 to microsteps/cycle^3
const double cdPMDToHumanJerk = 1.0 / cdHumanToPMDJerk;								// microsteps/cycle^3 to mm/second^3
const double cdDegreesToRadians = cdPi / 180.0;
const double cdRadiansToDegrees = 180.0 / cdPi;

const double cdOneDegreeTurn = cdWheelBaseCircumference / 360.0;  // in mm

int ClipAndRoundToINT(double FloatValue);
unsigned long ClipAndRoundToULONG(double FloatValue);
short ClipAndRoundToSHORT(double FloatValue);

class IntMovingAverage
{
public:
	IntMovingAverage(int NumSamples);	// instantiate with number of samples to average over
	void AddValue(int NewValue);		// add a new sample
	int GetAverage();					// return current moving average
private:
	int m_NumSamples;
	int m_SamplePosition;
	vector<int> m_Samples;
};

// Input a set of sensor readings at various angles, and then query the dataset to answer
// questions like:  What angle is most open?  How wide is the opening?  How far can I travel in that direction?
class SensorArc
{
public:
	void Reset(int NumDegrees);  // throw out old data and reset to capture data from angle 0 to angle NumDegrees
	void AddReading(int DegreeAngle, int mmReading);  // input a new sensor reading measured at a particular angle
	void Process(int RequiredWidth, int DesiredRange);
	int GetBestRange();  // return 0 if there is no opening in the sensor arc greater than RequiredWidth
	int GetBestAngle();  // return -1 if there is no opening in the sensor arc greater than RequiredWidth
private:
	int Interpolate(int Angle);		// estimate a result for Angle from surrounding results
	bool GetBestArc(int Range, int *pStart, int *pEnd);  // For given range, set pStart and pEnd to the best arc possible or return false if none
	int ComputeChordLength(int Range, int ArcAngle);	// how wide is the arc at the specified range?
	vector<int> m_Readings;
	int m_Size;  // = NumDegrees + 1
	int m_BestRange;
	int m_BestAngle;
};