PIMoveTo

Note

Looking for a detailed description? See PI Deep Dive.

void PIMoveTo(char *fName, int size, bool preload)

Uses a PI controller to move the robot using a trajectory profile as the reference data. The passed in trajectory profile should have 6 columns. The first 3 are the total angular displacements(rad) of motors 1, 2, and 3 repsectively, with the last 3 being the respective angular velocities(rad/s). The delta time is 0.1 seconds. The loop each iteration will calculate difference in encoder counts to convert it into displacement in radians using countsToRadDisp. This will then be added to the total angular displacement for each motor, and these calculated values will be subracted with the reference angular displacements to get the error for each motor. These errors are then summed to the motors individual total error counter, and based on the error values and P & I constants, angular velocities(rad/s) to set the motors to is calculated. Then the funtion setRadSToPercent is called to convert the angular speeds to percent and limit them within operating range, and then set the motors to that percent.

Parameters

• fName: the file name of trajectory profile.

• size: the number of lines/commands in trajectory profile.

• preload: If true, the file will be preloaded and won’t start until the start light turns on.

Examples

PIMoveTo("upRamp.txt", 31, false);

Note that while size may be any number equal to or larger to the number of lines, it is more effecient for memory to keep size equal to the number of lines. Additionally, the given file name must match the format shown in Trajectory profiles.

---

PIMoveTo("start.txt", 21, true);

In this example, preload is true. Thus, it will read the file and populate the arrays, but will not start moving until getCdsColor reports that light is detected.

Function Variables

Type	Name	Description
int	countNew1	The current iteration encoder counts for motor 1
int	countNew2	The current iteration encoder counts for motor 2
int	countNew3	The current iteration encoder counts for motor 3
int	countOld1	The previous iteration encoder counts for motor 1
int	countOld2	The previous iteration encoder counts for motor 2
int	countOld3	The previous iteration encoder counts for motor 3
float	displacement1	The total angular displacement for motor 1 in radians
float	displacement2	The total angular displacement for motor 2 in radians
float	displacement3	The total angular displacement for motor 3 in radians
float	refPos1	Temporary variable to read motor 1’s reference angular position from file
float	refPos2	Temporary variable to read motor 2’s reference angular position from file
float	refPos3	Temporary variable to read motor 3’s reference angular position from file
float	refSpeed1	Temporary variable to read motor 1’s reference angular velocity from file
float	refSpeed2	Temporary variable to read motor 2’s reference angular velocity from file
float	refSpeed3	Temporary variable to read motor 3’s reference angular velocity from file
float[][]	pos_ref	Reference angular positions - Columns are each motor, rows are each value
float[][]	vel_ref	Reference angular velocities - Columns are each motor, rows are each value
float	phi1	Stores the actual current total displacement for motor 1 in radians
float	phi2	Stores the actual current total displacement for motor 2 in radians
float	phi3	Stores the actual current total displacement for motor 3 in radians
float	motorSpeed1	Calculated motor 1’s speed in rad/s to correct for error from PI controller
float	motorSpeed2	Calculated motor 2’s speed in rad/s to correct for error from PI controller
float	motorSpeed3	Calculated motor 3’s speed in rad/s to correct for error from PI controller
float	errorTotal1	The total error error for motor 1 used by the integral term
float	errorTotal2	The total error error for motor 2 used by the integral term
float	errorTotal3	The total error error for motor 3 used by the integral term
float	Kp	The proportional constant
float	Ki	The integral constant