P13201
"TigerBot"
Software Review Packet
Enter
TigerBot Software Flowchart
Power on Roboard
Power on servos
Check and initialize
hardware &
software
subsystems
Set servos to
standing position
Start program
control loop
Main program control loop
Launches handler programs
Fall Detection
Yes
Cut power to any
moving servos
Determine fall
direction
Yes
Freeze any moving
servos
Run a
predefined
function to reorient away
from obstacle
Yes
Process command
Dispatch command
to the appropriate
handler
Roll to
predetermined
position
No
Collision /
Proximity
Detection
No
WiFi command
received
No
Servo action to
perform
No
Balancing
algorithm
Yes
Use Inverse kinematics and
zero moment point
equations to compute new
servo position and
acceleration vectors
Move servos
Stand up from
fall
Servo Movement
Tigerbot will utilize inverse kinematics to move its servos. Inverse kinematics, given a position to reach,
will give you the rotations of the joints of the robot needed to reach that position.
Inverse kinematics has been implemented in Matlab, and will be ported to C so Tigerbot can utilize it.
In C, servos are moved by utilizing Roboard libraries so the PWM signals don't have to be written
manually.
Control Flow
Application Control
(External)
Apache Web
Server / C-based
server
(Dispatcher)
Monolithic
program that runs
specialized
classes for certain
functions
Software Stack
Controller
iPhone Application
Network Layer
Keeps track of state
Apache Web
Server / C-based
server
(Dispatcher)
Which program is running is
corresponds with current
state
Monolithic
program that runs
specialized
classes for certain
functions
Custom / Provided Libraries
Zero moment point
Tigerbot Libraries
Roboard Library
(RoBoIO)
Roboard Library
(RoBoIO)
Inverse kinematics
RC Servo
(PWM)
I2C
Servos
Foot Sensor
Hardware Layer
Magnetometer /
Accelerometer /
Gyroscope
Software Layers
Control / Dispatch
Layer
(External)
This layer, external to the Roboard, is the mechanism which provides user input. An
iPhone application will serve this function and allow for remote control and testing of
all of TigerBot's functions.
Program Layer
Programs stored on the Roboard handle the external input and determine the proper
actions to take. These programs are also responsible for monitoring input from the
sensors and compensating for any problems detected.
Custom / Roboard
Libraries
These libraries will provide lower-level functionality to the layer above. Some libraries
are part of the RoBoIO software provided by Roboard, and additional libraries will be
written to provide additional layers of abstraction. Inverse kinematics will be a library
in this layer.
PWM / I2C /
Networking Drivers
Below the RoBoIO libraries are the underlying PWM / I2C drivers. These drivers are all
prewritten. The Roboard also makes use of a networking card, which uses generic
drivers that need to be configured manually to connect properly.
Lubuntu Kernel
Hardware
(Roboard /
Servos / Sensors /
Network Card)
Lubuntu is a lightweight Linux distribution closely based on the popular Ubuntu
distribution. Lubuntu was chosen because it is "lighter, less resource hungry, and
more energy efficient" than similar Linux distributions.
The hardware includes the Roboard itself, the servos hooked up to the I2C pins, and
the sensors hooked up to the I2C pins, the network card in the Roboard's Mini-PCI
slot. Lubuntu and RoBoIO libraries provide a mechanism for the hardware to be
interacted with.
Inverse Kinematics (MATLAB)
%Inverse Kinematics Function. Computes 6 output angles given foot position
%and orientation.
%written by Alexander Yevstifeev
%Based on work in the paper Closed-form Inverse Kinematic Joint Solution for Humanoid Robots by Muhammad
A. Ali, Andy Park, and C.S. George Lee.
function [T] = Leg_Inv_Kin(rx,ry,rz,p)
%outputs joint angles for T(1)-T(6) of a humanoid leg given the x,y,z axis
%rx ry rz are rotation angles for orientation and a position vector.
%order of angle joints from T(1) to T(6) are leg twist, leg side, leg lift, knee, foot lift, and foot tilt.
%Link length definitions
L5 = 0.05 ; % Link between ankle and bottom of foot
L4 = 0.25 ; % Link between knee and ankle
L3 = 0.25 ; % Link between hip and knee
%compute rotation matrix given rotation angles rx, ry, and rz
Cx = cos(rx) ;
Sx = sin(rx) ;
Cy = cos(ry) ;
Sy = sin(ry) ;
Cz = cos(rz) ;
Sz = sin(rz) ;
Rmat = [ Cy*Cz , -Cx*Sz+Sx*Sy*Cz , Sx*Sz+Cx*Sy*Cz ; ...
Cy*Sz , Cx*Cz+Sx*Sy*Sz ,-Sx*Cz+Cx*Sy*Sz ; ...
-Sy , Sx*Cy
, Cx*Cy
;];
%build location matrix, find inverse, assign inverse vectors
M = cat(2, Rmat , p') ;
M = cat(1,M,[0 0 0 1]) ;
Mi = inv(M) ;
s = 1:3 ;
ni = Mi(s,1)' ;
si = Mi(s,2)' ;
ai = Mi(s,3)' ;
pi = Mi(s,4)' ;
C4 = ( (pi(1)+L5)^2 + pi(2)^2 + pi(3)^2 - L3^2 - L4^2 ) / (2 * L3 * L4 ) ;
T(4) = atan2(sqrt(1 - C4^2),C4) ;
S4 = sin(T(4)) ;
psi = atan2( S4 * L3 , ( C4 * L3 ) + L4 ) ;
T(5) = atan2( -pi(3) , sqrt( (pi(1) + L5)^2 + pi(2)^2 ) ) - psi ;
C5 = cos(T(5)) ;
T(6) = atan2(pi(2) , -pi(1) - L5 );
if (cos(T(4)+T(5))*L3 + C5*L4) < 0
T(6) = T(6) + pi ;
end
S6 = sin(T(6)) ;
C6 = cos(T(6)) ;
T(2) = atan2( -S6*si(1) - C6*si(2) , -S6*ni(1) - C6*ni(2) ) ;
T(1) = atan2( sqrt(1 - (S6*ai(1) + C6*ai(2))^2) , S6*ai(1) + C6*ai(2));
if sin(T(2)) < 0
T(1) = T(1)+3.14159 ;
end
T(3) = atan2(ai(3) , C6*ai(1) - S6*ai(2) ) - T(4) - T(5) ;
%Adjustment for reference frames on model
T(1) = -(T(1)+1.5707);
T(2) = -(T(2) + 1.5707) ;
T(3) = T(3) - 1.5707 ;
T(6) = -T(6) ;
%correction for position value over 2*pi
for i = 1:6 ;
T(i) = rem(T(i),6.283) ;
end
end
Moving a Servo (C++)
if (rcservo_Init(RCSERVO_USEPINS1 + RCSERVO_USEPINS2)) {
motion_frame[0] = 1500L; //servo on pin S1, position 1500us
motion_frame[2] = 500L; //servo on pin S2, position 500us
rcservo_MoveTo(motion_frame, 500); //move both servos to their new position in 500ms
rcservo_Close();
}