11. Appendix C - Software LibrariesHexotica - The Design and Implementation of a Small Walking Robot
11. Appendix C - Software Libraries
The following sections outline the libraries that have been developed to control the robot. Each library corresponds to one of the boxes shown in Figure 45. There is no sensor library since no sensors are available at this time. The error module doesn’t appear in the diagram since it has not yet been determined how this module will be used.
There are a number of applications that have been generated during the development of these libraries and which are used to control the robot. These are described in Appendix B - Application Files.
11.1 ADC.LIB
This library provides access to the ADC devices on the motor control cards.
Author
Stefan B. Williams
Services
The ADC functions provide access to the ADC devices on the motor control cards. During initialization, conversion factors are passed as parameters. These conversion factors are used to convert the value read from the ADC (0-255) into some meaningful unit. The return values from the read functions are therefore in the units expected by the calling routine.
Data Structure
The following structure contains the data used by the ADC access functions. Each adc_device structure is mapped to a single ADC device. The data structure stores the chip select line and device number used to access the physical device. It also stores the conversion factor and offset used to convert the value read from the device into the appropriate units.
typedef struct {
int CS;
int device;
float k;
float offset;
} adc_device;
Access Functions
adc_init(adc_device *adc, int CS, int device, float k, float offset)
adc_device *adc The ADC structure to be initialized
int CS The chip select to access this device
int device The device number on the motor control board
float k The conversion factor
float offset The conversion offset
This function initializes the ADC structure that is passed as a parameter using the device identifiers and conversion factors passed as the other parameters. The device number and chip select lines are used for the actual interfacing to the hardware. The conversion factors are used to convert the value read from the ADC (0-255) into some meaningful unit. This function also 'tickles' the ADC reset line to initialize conversions of the ADCs.
adc_read_value(adc_device *adc)
adc_device *adc The ADC device to read
This function reads the currently stored value of the ADC referenced by the passed ADC structure. The return value is modified by the conversion factors stored in the structure. These conversion values are used to transform the value read from the ADC, in the range 0-255, into some meaningful unit.
adc_read_smooth(adc_device *adc, int num_sample)
adc_device *adc The ADC device to read
int num_sample The number of samples to be smoothed
This function returns a smoothed value of the ADC referenced by the passed ADC structure. The value is read, using the adc_read_value function, num_sample times. This value is averaged and returned to the calling function. This effectively acts as a digital low-pass filter.
11.2 ERROR.LIB
This library is a preliminary implementation of an error reporting mechanism. This module will allow the various processes to report errors, either via the stdio interface or via some other mechanism.
Author
Stefan B. Williams
Services
The error functions provide access to a global error reporting mechanism. This mechanism is still in the development process and as more errors become available and when a final error reporting mechanism is decided upon, the functionality of this module will have to be examined.
Data Structure
The following structure contains the data used by the error library access functions. A single instance of the data structure is available to all functions executing on a single processor. In order to report an error, the appropriate error reporting function is called. This function then checks to determine if the error is more severe than the current error. If it is, the current error information is updated in the global error structure. The data structure stores the error number reported, the leg and angle for which the error was reported, the severity of the error and keeps track of whether or not this is a new error.
typedef struct {
char error_num;
char leg;
char angle;
char severity;
char new;
} error_struct;
error_struct error;
The following severity levels are recognized by the error reporting mechanism.
Severity Value Description ERROR_NONE 0 No error reported ERROR_WARNING 1 Warning error reported, motion continuing ERROR_STOP 2 Stop error reported, leg motion stopped ERROR_FATAL 3 Fatal error reported, leg motion stopped The following errors are reportable with the current mechanism. More errors will become available as the needs become apparent. This list is a preliminary attempt to describe some of the anticipated errors that may occur.
Error
Value Severity Description ERR_ANG_NO_CHG 0 STOP Motor signal sent but joint angle is not changing ERR_ANG_CHG_NO_CMD 1 STOP No motor signal but joint angle is changing ERR_ANG_OUT_OF_RNG 2 STOP Joint angle out of range ERR_MTR_SPD_LTD 10 WARNING Motor command speed limited ERR_POS_OUT_OF_RNG 20 STOP Leg position is out of range Access Functions
error_clear()
This function clears any currently existing errors. Since less serious errors are not reported if a more serious error exists, it is important that errors are acknowledged and cleared.
error_notify(int error_num, int leg, int angle, int severity)
int error_num The identifier of the error being reported
int leg The number of the leg reporting the error
int angle The joint angle at which the error occurred
int severity The severity of the error
This function provides other modules with a means by which to report errors. When an error is reported, it is stored if it is more severe than any currently existing errors.
error_report()
This function is used to report any existing errors. Its implementation has been delayed until the exact mechanism by which errors will be reported to the high-level controller has been determined.
error_display()
This function is used to display any existing errors as they occur. It uses stdio to print out error messages that can be examined by the developer. It is intented as a debugging tool and is not likely to be used by the actual robot control system.
11.3 LEG.LIB
This library implements all of the functionality needed by the low-level controller for control of the individual legs.
Author
Stefan B. Williams
Services
This library provides functionality to initialize the leg structures, calculate forward and inverse kinematics, calculate desired velocities, convert Cartesian velocities to joint velocities, compute via-points and desired positions, initialize movement of the leg and to check if the leg is in position. It also provides the real-time interrupt used to coordinate the motion of the leg.
Data Structure
The following structure contains the data used by the leg library access functions. Each leg structure maps to a physical leg and is used to store all of the information needed to control the legs. The data structure stores information needed to identify the leg, the position of the foot, the actual and desired joint angles of the leg, the cosine and sine of each angle (to avoid recalculating these CPU intensive functions), the velocities, the gains used for the PID control of the leg, the speed and direction of motion and the via indexes to which the leg is moving. It also contains a number of flags used to keep track of whether the leg is moving and if it is in position or not. Finally, the leg structure also holds a pointer to the adc_device and motor_device used to interface to the motor control hardware.
typedef struct {
int module; /*module type FRONT, MID, REAR*/
int side; /*side of body RIGHT or LEFT*/
float cart_pos[3]; /*cartesian pos of foot (m)*/
shared float jt_angle[3];/*des. angles of leg (rad)*/
float act_jt_angle[3]; /*act. jt angles of leg (rad)*/
shared float des_jvel[3];/*desired joint vel*/
float act_jvel[3]; /*'actual' joint vel*/
float prv_angle[3]; /*previous angles*/
float cos_[3]; /*cos of desired angles*/
float sin_[3]; /*sin of desired angles*/
float des_pos[3]; /*desired pos of foot*/
float des_vel[3]; /*desired vel of foot*/
float act_vel[3]; /*actual vel of foot*/
float Kp; /*proportional gain*/
float Kd; /*derivative gain*/
float speed; /*speed of foot*/
float alpha; /*direction of travel*/
float length_1, length_2;/*member lengths*/
float via_pts[4][3]; /*via pts for path*/
float adj_via_pts[4][3]; /*adjusted via pts*/
float offset[3]; /*offset for via pts*/
int via_ndx; /*index into via pts*/
bool in_pos; /*flag indicating if in pos*/
bool step_flag; /*flag to initiate stepping*/
bool move_flag; /*flag to initiate a move*/
adc_device pot[3]; /*pots measuring angles*/
motor_device motor[3]; /*motors controlling angles*/
} leg_struct;
shared leg_struct legs[NUM_LEGS];
Access Functions
leg_init(leg_struct *leg, int side, int *m_K, int *m_off, float *a_K, float *a_off)
leg_struct *leg The leg structure to be initialized
int side The side of the body
int *m_K The motor multipliers
int *m_off The motor offsets
float *a_K The ADC multipliers
float *a_off The ADC offsets
This function initializes the leg structure that is passed as an argument. This involves setting up the via-points, initializing all of the appropriate variables, setting up the motors and ADC structures which the leg uses to move, reading in the values of the pots and determining the initial position of the foot.
leg_kinematics(leg_struct *leg)
leg_struct *leg The leg structure to calculate kinematics
This function calculates the forward kinematics of the leg based on the current value of the joint variables. It also computes the approximate velocity of the leg.
leg_inv_kin(leg_struct *leg)
leg_struct *leg The leg structure to calculate inverse kin.
This function calculates the inverse kinematics of the leg based on the current value of the desired position. It currently isn’t used in the leg control scheme but is provided in case this functionality is ever required.
leg_calc_jt_vel(leg_struct *leg)
leg_struct *leg The leg structure to calculate joint velocities
This function calculates the joint velocities of the leg based on the current value of the desired foot velocity. It uses the inverse of the Jacobian to transform the Cartesian velocity of the foot into joint space.
leg_set_des_pos(leg_struct *leg, float *pos)
leg_struct *leg The leg structure to set desired position
float *pos The coordinates of the desired position
This function stores the passed position as the next desired position of the foot.
leg_chk_des_pos(leg_struct *leg)
leg_struct *leg The leg structure to check desired position
This function determines whether the foot is sufficiently close to the desired position. It returns TRUE if it is, FALSE otherwise.
leg_adj_des_pos(leg_struct *leg, float *delta)
leg_struct *leg The leg structure to adjust the desired pos.
float *delta The change in the desired position
This function changes the current value of the desired position by the passed deltas.
leg_calc_des_vel(leg_struct *leg)
leg_struct *leg The leg structure to compute desired velocities
This function calculates the desired foot velocity that will bring the leg to the desired position. It computes a unit vector pointing in the direction of the desired position, multiplies this by the magnitude of the current speed scalar and then uses a derivative component to minimize the deviation from the actual course.
Leg_updt_jt_angs(leg_struct *leg)
leg_struct *leg The leg structure to read joint angles
This function updates the current desired joint angles. It uses the desired joint velocities at each timestep to determine the next desired joint angles.
leg_read_jt_angs(leg_struct *leg)
leg_struct *leg The leg structure to read joint angles
This function reads the current joint angles. It uses the ADC structures stored in the leg structure to access the ADC devices. The values returned by the ADC functions are already in units of radians.
leg_cmd_jt_vel(leg_struct *leg)
leg_struct *leg The leg structure to command joint velocities
This function sends out joint velocity commands via the motor structures stored in the leg structure. Commands are sent as radians/sec and converted to command words by the motor functions. Checking is performed to determine if the legs are violating their workspace or if the desired joint velocities are too high.
leg_adj_via_pts(leg_struct *leg, float alpha)
leg_struct *leg The leg structure to adjust via-points
float alpha The current value of alpha
This function adjusts the value of the via-points based on the current value of alpha. It stores a copy of the adjusted via-points as well as the local coordinates of the via-points.
leg_start_move(leg_struct *leg, int position, int speed)
leg_struct *leg The leg structure to start moving
int position The via-point to move to
int speed The speed at which to move
This function initiates a movement in the leg. The desired position is set to the appropriate via-point and the speed parameter is stored. The move flag is also set to TRUE and the in position flag is set FALSE.
leg_stop(leg_struct *leg)
leg_struct *leg The leg structure to stop
This function commands all of the motors in the leg to stop. It also sets the velocity of the leg to zero and turns the movement flags to FALSE.
leg_move(leg_struct *leg)
leg_struct *leg The leg structure to move
The handler uses this function to actually move the leg. It updates the joint angles, computes the forward kinematics, calculates desired velocities, converts the desired velocity to joint velocities, commands the motors to the appropriate joint velocities and finally checks if the leg has achieved its desired position. If the leg is in position the function returns TRUE otherwise it returns FALSE.
leg_handler()
This function is the real-time interrupt responsible for updating the desired motion of the legs. Depending on which movement flags are set, it either takes care of stepping through the via-points or simply moves the legs to their desired position.
leg_handler_pid()
This function is the real-time interrupt responsible for motion of the legs. If any of the motion flags are set, it attempts to achieve the desired angular positions and velocities of the legs using a simple PD control approach.
11.4 LEG_INFO.LIB
This library contains local information needed by the high level controller for control of the legs. Since the low-level leg control is implemented on different processors, local copies of important information are kept in the leg_info structure.
Author
Stefan B. Williams
Services
This library provides functionality to initialize the leg_info structures and to send messages to the legs. These messages include instructions to move the leg, start and stop the leg, request information about the current leg status, change the direction of motion and request that the processor quit operation.
Data Structure
The following structure contains the data used by leg_info functions. Each leg_info structure stores information about the remote leg being controlled by the high level controller. Only data relevant to the high level control of the leg is maintained in this structure. The data structure stores the slave number, leg number and side of the body the leg is on. This information is used to send messages to the leg processors across the TIA/EIA-485 line. The structure also stores the via-index to which the leg is moving and the speed at which it has been commanded to reach the next via-point. This information is used to update the movement commands if the leg is stopped for some reason.
typedef struct {
char slave_num;
char leg_num;
char side;
char via_ndx;
char speed;
} leg_info;
shared leg_info legs[NUM_LEGS];
Access Functions
leg_info_init(leg_info *leg, int slave_num, int leg_num, int side)
leg_info *leg The leg to stop
int slave_num The slave number controlling the leg
int leg_num The local leg number
int side The side of the body of the leg
This function initializes the leg_info structure. It sets the values passed as parameters to identify which leg is being addressed by the structure.
leg_info_move(leg_info *leg, char position, int speed)
leg_info *leg The leg to move
char position The position to move to
int speed The speed at which to move the leg
This function sends a move leg message to the appropriate leg. It passes the position to which the leg should move and the desired speed. It expects to receive acknowledgment that the slave has received the message.
leg_info_start(leg_info *leg)
leg_info *leg The leg to move
This function sends a start leg message to the appropriate leg. It expects to receive acknowledgement that the slave has received the message.
leg_info_stop(leg_info *leg)
leg_info *leg The leg to move
This function sends a stop leg message to the appropriate leg. It expects to receive acknowledgement that the slave has received the message.
leg_info_rq_stat(leg_info *leg, msg *reply)
leg_info *leg The leg to move
msg *reply The reply received from the leg
This function sends a request for status message to the appropriate leg. It returns the actual message in the reply parameter. It expects to receive acknowledgement that the slave has received the message.
leg_info_chk_pos(leg_info *leg)
leg_info *leg The leg to move
This function uses the leg_info_rq_stat function to check if the leg is in position. If the leg reports that it is in the correct position, it returns TRUE. If the leg returns the wrong via-point or is not moving, a new move command is sent to initialize motion. This is to ensure that motion continues even if one of the slaves crashes. When they recover from a crash, the slave processors are no longer be able to tell whether they are supposed to be moving the legs.
leg_info_dirn(int slave_num, float alpha)
leg_info *leg The leg to move
float alpha The desired direction for the leg
This function sends a direction message to the appropriate slave. It expects to receive acknowledgement that the slave has received the message.
leg_info_quit(int slave_num)
leg_info *leg The leg to move
This function sends a quit message to the appropriate slave. It expects to receive acknowledgement that the slave has received the message.
11.5 MOTOR.LIB
This library provides access to the motor driver devices on the motor control cards.
Author
Stefan B. Williams
Services
The motor functions provide access to the motor driver devices on the motor control cards. During initialization, conversion factors are passed as parameters. These conversion factors are used to convert the value passed as a desired signal into an appropriate command word (0-255).
Data Structure
The following structure contains the data used by the motor access functions. Each motor_device structure is mapped to a single motor driver device. The data structure stores the chip select line and device number used to access the physical device. It also stores the conversion factor and offset used to convert the commanded value into an appropriate motor command word.
typedef struct {
int CS;
int device;
int k;
int offset;
} motor_device;
Access Functions
motor_init(motor_device *motor, int CS, int device, int k, int offset)
motor_device *motor The motor structure to be initialized
int CS The chip select to access this device
int device The device number on motor control board
int k The conversion factor
int offset The conversion offset
This function initializes the motor structure that is passed as a parameter using the device identifiers and conversion factors passed as the other parameters. The device number and chip select lines are used for the actual interfacing to the hardware. The conversion factors are used to convert the value passed to the motor driver into a motor command word (0-255).
motor_set_speed(motor_device *motor, float speed)
motor_device *motor The motor device to command
float speed The commanded motor speed/torque
This function takes the passed speed parameter and modifies it by the conversion factors stored in the structure. These conversion values are used to transform the passed motor parameter into a motor command word in the range 0-255.
motor_stop(motor_device *motor)
motor_device *motor The motor device to command
This function stops the motion of the motor pointed to by the supplied motor structure. The brake line is set and the speed command bits are set to 0.
11.6 MSG.LIB
This library contains the functions necessary for implementing the RS485 messaging. Both the master and slave services are implemented in this module.
Author
Stefan B. Williams
Services
This library provides functionality to initialize the communications protocols and send and receive messages. These messages include instructions to move the leg, start and stop the leg, request information about the current leg status, change the direction of motion and request that the processor quit operation. Each message is of a fixed length, with RS485 allowing messages of up 255 characters.
Data Structure
The following structure contains the data used by message functions. Each msg_struct structure stores messages that are sent messages and received across the TIA/EIA-485 line. The data structure stores the type of message, the slave number to which message is being sent, the length of the message the message itself. The message length is currently limited to 40 characters but this can be changed as necessary.
typedef struct {
char type;
char slave_num;
char length;
char message[40];
} msg_struct;
The following table shows the messages that are currently supported between the Master and Slave controllers.
Message Value Length Arguments Description MSG_MOVE_LEG 0x1 4 instruction, leg number, via-point, speed Move the leg to the via-point at the appropriate speed MSG_START_LEG 0x2 4 instruction, leg number, speed Start moving the leg through the via-points MSG_STOP_LEG 0x3 2 instruction, leg number Stop moving the leg MSG_REQ_STATUS 0x4 2 instruction, leg number Request status information from the leg MSG_SND_STATUS 0x5 10 instruction, leg number, via index, in position flag, move flag Send status information to the master about the leg MSG_DIRN 0x6 4 instruction, leg number, direction high byte, direction low byte Set the direction of motion of the leg MSG_QUIT 0x7 1 instruction Quit executing MSG_ACK_CMD N/A 1 instruction Return the instruction to verify the message arrived at the slave Table 12 - Messages supported for inter-process communication
Access Functions
off_485()
Turns off the RS485 driver for the Z180 port 1. Different boards have different methods of driving the RS485 serial interface. The current boards use the CS1 chip select line to turn the RS485 on or off. By writing a 0 to CS1, the line is turned off while writing a 1 to the line turns it on.
on_485()
Turns on the RS485 driver for the Z180 port 1. Different boards have different methods of driving the RS485 serial interface. The current boards use the CS1 chip select line to turn the RS485 on or off. By writing a 0 to CS1, the line is turned off while writing a 1 to the line turns it on.
msg_init(int slave_num);
int slave_num The slave number of the current processor
This function initializes the RS485 line to the appropriate communications rate and sets the current processor to the appropriate slave number. The master controller uses the slave number 0 while other boards take on values from 1-255. The processors then monitor the RS485 line waiting for messages directed to their particular slave ID number.
msg_send(msg_struct *msg, msg_struct *reply)
msg_struct *msg The msg to be sent
msg_struct *reply The reply received from the slave
This function sends a message over the RS485 line. It returns the reply in the reply structure.
msg_tlk_to_slave(msg_struct *msg, int ndelay, msg_struct *reply)
msg_struct *msg The msg to be sent
int ndelay The number of ticks to wait for a reply
msg_struct *reply The reply received from the slave
This function sends a message over the RS485 line using the sendOp22 command from the network library. It uses the passed parameter ndelay as the number of ticks to wait for a reply. It places the reply in the reply structure and returns the error code returned by the sendOp22 command.
msg_chk_query(msg_struc *msg)
msg_struct *msg The msg received from the master
This function checks to see if the slave has received a message from the master. It performst this checking using the check_opto_command function from the network library. If a message has been received, it is stored in the msg structure and TRUE is returned.
msg_reply(msg_struct *reply)
msg_struct *msg The msg received from the master
This function sends a reply from the slave to the master using the replyOpto22 function from the network library.
11.7 RIPPLE.LIB
This library contains the gait control code that implements the ripple gait.
Author
Stefan B. Williams
Services
This library provides functionality to initialize the ripple gait structures and to coordinate the motion of the legs.
Data Structure
The following structure contains the data used by the ripple gait access functions. The ripple_struct structure stores information about the status of the ripple gait on each side of the robot. The data structure stores pointers to the appropriate leg_info structures, the position and speed to which the current leg is being moved. It also stores the via-indexes that are commanded to move the legs forward.
typedef struct {
leg_info *leg[3];
char position;
char speed;
int seq[RIPPLE_NUM_PTS];
int via_ndx;
} ripple_struct;
Access Functions
ripple_init(ripple_struct *ripple, int side)
ripple_struct *ripple The ripple structure to be initialized
int side The tripod’s side
This function initializes the ripple structure based on which side of the body it is located. It sets pointers to the appropriate leg_info structures as well as setting up the stepping sequence.
ripple_move(ripple_struct *ripple, char position, char speed)
ripple_struct *ripple The ripple to be moved
char position The position to move to
char speed The speed at which to move the robot
This function uses the leg_info_move command to send move messages to each of the legs to coordinate the motion of the ripple gait.
ripple_chk_pos(ripple_struct *ripple)
ripple_struct *ripple The ripple structure to be checked
This function checks to see if the rippling leg is in position. Once the leg has moved through the via-points to arrive in contact with the ground, the function returns TRUE.
ripple_handler()
This function handles the coordination of the ripple gait motion. It is called from the main loop to monitor and update the position of each of the legs.
11.8 TRIPOD.LIB
This library contains the gait control code that implements the tripod gait.
Author
Stefan B. Williams
Services
This library provides functionality to initialize the tripod gait structures and to coordinate the motion of the legs.
Data Structure
The following structure contains the data used by the tripod gait access functions. The tripod_struct structure stores information about the status of the two tripods that the robot uses to implement the tripod gait. The data structure stores pointers to the appropriate leg_info structures, the position and speed to which the tripod is being moved and the sequence index of the step
typedef struct {
leg_info *leg[3];
char position;
char speed;
int seq[TRIPOD_NUM_PTS];
} tripod_struct;
Access Functions
tripod_init(tripod_struct *tripod, int side)
tripod_struct *tripod The tripod to be initialized
int side The tripod’s side
This function initializes the tripod structure based on which side of the body it is located. This is measured with respect to the front pair of legs. It sets pointers to the appropriate leg_info structures as well as setting up the stepping sequence.
tripod_move(tripod_struct *tripod, char position, char speed)
tripod_struct *tripod The tripod to be moved
char position The position to move to
char speed The speed at which to move the tripod
This function uses the leg_info_move command to send move messages to each of the legs in the tripod.
tripod_chk_pos(tripod_struct *tripod)
tripod_struct *tripod The tripod to be checked
This function checks to see if the tripod is in position. It checks each leg in the tripod using the leg_info_chk_pos command. As soon as it finds a leg that is not in position it returns FALSE otherwise it returns TRUE.
tripod_handler()
This function handles the implementation of the tripod walking gait. It is called from the main loop and is responsible for coordinating the motion of the legs.
copyright information
Back to home
page. Last updated: April 20th,
1997