Embedded Files
The software was developed based on the block diagram shown in figure #6. The pseudo code below describes how the logic behind the software was developed. The entire code is displayed in the appendix.
Interrupt Service Routine
Interrupt Service Routine
The ISR is a simple state machine that is activated only when an interrupt occurs. It is used to send a precise control signal the R/C servos. It performs this by diving the timing waveform on the servos into different segments. The first segment is the setup time that the servo signal is high. This is the minimum requirement after which the angle of the servo is set by keeping the signal high. The second state is the time after the minimum time has elapsed. The duration that the signal remains high determine the servo angle. The final segment is just the remaining time in every 20 ms period when all servos have no signal being passed to them.
The servo signals are all controlled by one port and is designed to have only one pin go high at any time. This is done through time sharing and is facilitated by the SERVOCON register, which keeps track of the state number. Bit 0 of the SERVOCON register indicate whether the ISR should service a setup time or an angle time. Bits 3 to 1 mark the index number of the servo. Finally bit 4 indicates if the ISR is in the state when all pins are low. This register is incremented every interrupt and is reset to '11101000' upon reaching zero. In order of operation of the ISR is to sequentially serve the setup and angle time for each servo then a 16 cycle waiting time.
The ISR first increments the SERVOCON register and resets it if necessary. It then checks if the ISR should service a delay state or a servo. If a delay is to be serviced, a predefined delay time is loaded into the timer compare register. If not, the servo angle bit is checked. When this bit is clear, bits three, two and one are used as index numbers to set the correct bit on the signal waveform port. At the same time, the minimum setup time is loaded to the compare register. If the servo angle bit is set, the index bits are used to indirectly address the register containing the angle of the indexed servo. This values is stored into the compare register of the waveform timer.
SERVO CONTROLLER REGISTER (ADDRESS 20h)
bit 7-5 These bits are unused and read as 1
bit 4 DOWN: Delay state bit
0 = All the servos signals are low for the down time
1 = The servos are being controlled one at a time
bit 3-1 INX2 - INX1: Servo index number bits
000 = Servo 1 is being controlled
001 = Servo 2 is being controlled
010 = Servo 3 is being controlled
011 = Servo 4 is being controlled
100 = Servo 5 is being controlled
101 = Servo 6 is being controlled
110 = Servo 7 is being controlled
111 = Servo 8 is being controlled
bit 0 SETANG: Servo state bit
0 = The servo is set high for the miminum 1msec.
1 = The servo remains high to indicate its angle
Balancing Algorithm
Balancing Algorithm
For balancing, the A/D values are converted and saved. These values in conjunction with a threshold value are used to determine whether or not a leg is toughing the ground or not. One the number of legs that are touching the ground is determined, it is used to calculate the average of the sensor values. Only the legs that are touching the ground are used in this calculation.
The average sensor value is used to determine if a leg has too much or too little weight exerted on it. This determine if the leg is to extend or retract. If all the legs are toughing the ground, the legs with too little are not retraced. If any leg is not touching the ground it is extended.
Several registers are used to help indicate different aspects of each sensor and leg. The PADSTATE register is used to indicate if a sensor is touching the ground, and if that weight is above average. The LEGSTATE register is use to show the state of each leg. This register contains the flags used to transfer control of each leg to the balancing algorithm or to the user for walking. If the leg is active it is used for balancing. If the gait bits are set, it indicate that the user want permission of the leg. The balancing code looks at this bit to perform the necessary tasks to make sure it is safe to relinquish control of that leg.
Force Altering Algorithm Pseudo Code
Force Altering Algorithm Pseudo Code
Do for each active legs{
Get force value of the leg
Get force value of next active leg
Set both legs to the average force
Do for both of these legs{
if old value was greater than average
request extension of leg
otherwise{
if old value was zero
request extension of leg
else
request retraction of leg
}
}
}
Gait Algorithm Pseudo Code
Gait Algorithm Pseudo Code
If all legs active{
If time to move the next leg{
Activate all legs except leg-to-move
Set Gait direction to UP
}
}else{
If Leg height is reached
Set Gait direction to DOWN
Find Gait velocity
Move legs in opposite direction
If Gait direction is UP
Move leg up as defined by lift vector
else{
Move leg down as defined by lift vector
if leg is touching the ground{
Re-activate the leg
Increment leg-to-move counter
}
}
}
Servo Height Pseudo Code
Servo Height Pseudo Code
If in emergency state
retract all legs
else{
For all the active legs{
Get the servo angle
Temporarily add the change
Find the heighest servo
Find the lowest servo
}
Flag if the highest servo exceeds the max limit.
Flag if the lowest servo goes below the min limit.
If both flags are up
retract ALL legs
else{
Find the difference of the highest leg and the max limit
Subtract that difference from all the other servos
Set the highest servo to the max height limit.
}
Set inactive leg with height defined by Gait Algorithm
}
Resolving Design Issues
Resolving Design Issues
The original leg design was mechanical in nature. It would have improved the power conservation of the robot because it would redirect static forces to within the structural frame of the robot and not onto the motors that held up the frame. The mechanical design was to incorporate an elaborate system of pivots, screws and miniature jacks for this purpose. The design proved to be too complex and costly to build. Instead, the servo that directly controlled the movement of the robot were used to take on the weight of the robot. This proved to be a better choice because it had internal gearing that both resisted external mechanical forces and also increased the torque of the motors.
The new design made use of a "scissors-like" action to extend and retract the leg. One servo was dedicated to this job and was mounted on the leg at a strategic pivot point to perform this task. This servo had to be able to withstand forces up to the weight of the entire robot for certain moments within the robot's gait.
A second servo was mounted to the frame of the robot, and was attached to the leg to allow it to swing back and forth. That servo would have to work in conjunction with the first servo to share the workload of the robot's weight. Furthermore, these two servos would have to be strategically mounted onto the frame of the robot such that the two servos would:
Not make contact with each other
All the leg to move freely in its full range
Work in unison to determine the position of the leg.
Efficiently transfer their torque to the legs.
The servos should have enough torque to hold up the robot. 5.0 kg-cm is sufficient for this. The servos have an input square wave of frequency of 50 Hz. Its duty cycle determines the the angle of the shaft. A pulse width of 1ms will take it to -90 degrees and 2ms will take it to +90 degrees. Every angle in between is proportional to a pulse that is between these two widths.
Table of Content
Page updated
Google Sites
Report abuse