Для максимальной гибкости выходного формата, позиция десятичной точки указывается двоичным целым числом, называемым значением показателя степени. Если показатель степени равен нулю, то десятичная точка предполагается справа от самой правой цифры. Показатель степени больший нуля показывает, как много остаточных нулей не показывается. Для каждой единице ниже нуля десятичная точка в строке сдвигается налево.
Последний шаг перевода числа - это сохранить результат в виде BCD и указать, где находится десятичная точка. Строка типа BCD затем распаковывается в десятичную строку из ASCII символов. Знак устанавливается в соответствии со знаком оригинального значения.
Примеры тригонометрических вычислений
В этом примере моделируется кинематика руки робота по гомогенным матрицам преобразований размером 4 x 4, предложенным Денавитом и Хартенбергом (1,2). Передаточные и поворотные отношения между смежными звеньями руки описаны с помощью этих матриц, используя матричный метод D-H. Для каждого звена построена гомогенная матрица преобразований размером 4 x 4, которая представляет координатную систему звена (L(i)) в соединении (J(i)) в соответствии с предыдущей координатной системой (J(i-1),L(i-1)).
--------------------------------------- 1. J.Denavit and R.S.Hartenberg,"A Kinematic Notation for Lower-Pair Mechanisms Based on Matrices." J.Applied Mechanics, June 1955, pp. 215-221 2. C.S.George Lee, "Robert Arm Kinematics, Dinamics, and Control." IEEEComputer, Dec. 1982.
Следующие четыре геометрические величины полностью описывают движение любой пары соединение/звено (J(i),L(i)), как показано на Рисунке 20-7.
тета(i) = Угол между осью x(i) и осью x(i-1) при повороте вокруг оси z(i-1) против часовой стрелки. d(i) = Расстояние от (i-1)-ой координатной системы вдоль оси z(i-1) до оси x(i). a(i) = Расстояние до i-ой координатной системы от оси z(i-1) вдоль оси -x(i). альфа(i) = Угол между осью z(i) и осью z(i-1) при повороте вокруг оси x(i) против часовой стрелки.
Матрица преобразований A(i)(i-1) типа D-H для смежных координатных участков от соединения (i-1) до соединения (i) вычисляется как:
A(i)(i-1) = T(z,d) x T(z,тета) x T(x,a) x T(x,альфа)где:T(z,d) представляет передачу вдоль оси z(i-1)T(z,тета) представляет поворот угла тета вокруг оси z(i-1)T(x,a) представляет передачу вдоль оси x(i)T(x,альфа) представляет поворот угла альфа вокруг оси x(i) A(i)(i-1) = | COSтет(i) -COSалф(i)SINтет(i) SINалф(i)SINтет(i) COSтет(i) || || SINтет(i) COSалф(i)COSтет(i) -SINалф(i)COSтет(i) SINтет(i) || || 0 SINалф(i) COSалф(i) d(i) || || 0 0 0 1 |
Композитная гомогенная матрица T, которая представляет собой позицию и ориентацию пары соединение/звено по отношению к основной системе, получается последовательным умножением матриц преобразований типа D-H для смежных координатных участков.
T(i)(0) = A(1)(0) x A(2)(1) x ... x A(i)(i-1)
Пример программы на Рисунке 20-8 иллюстрирует, как можно выполнить передаточный процесс, используя возможности обработки операций с плавающей точкой процессора i486. Программа состоит из двух главных процедур. Первая процедура TRANS PROC используется для вычисления элементов в каждой матрице A(i)(i-1) типа D-H. Вторая процедура MATRIXMUL PROC находит произведение двух последовательных матриц типа D-H.
Рисунок 20-8. Пример Кинематики Руки Робота +-------------------------------------------------------------------------+| || || Name ROT_MATRIX_CAL || ; || ; Этот пример иллюстрирует использование команд с || ; плавающей точкой процессора i486 и частично || ; функцию FSINCOS, которая дает значения синуса и || ; косинуса одновременно. Программа вычисляет || ; композитные матрицы, по которым работает механизм || ; передачи. || ; || ; В этом примере рассмотрена только кинематика || ; робота. || ; || ; Если композитная матрица, упомянутая выше, || ; задается как: || ; T1n = A1 x A2 x ... x An, то || ; T1n находится вызовом процедур trans_proc и || ; matrixmul_proc, пока все матрицы не будут || ; перемножены. || ; || ; Процедура trans_proc вычисляет элементы каждой || ; матрицы A(A1,.....An), пока процедура || ; matrixmul_proc выполняет умножение матрицы Ai и || ; Ai+1. Для умножения процедура matrixmul_proc || ; использует процедуры matrix_row и matrix_elem. || ; || ; Определить стековое пространство || ; || trans_stack stackseg 400 || || ; Определение матричной структуры для передаточных || ; матриц размером 4x4 || || a_matrix struc || a11 dq ? || a12 dq ? || a13 dq ? || a14 dq ? || a21 dq ? || a22 dq ? || a23 dq ? || a24 dq ? || a31 dq 0h || a32 dq ? || a33 dq ? || a34 dq ? || a41 dq 0h || a42 dq 0h || a43 dq 0h || a44 dq 1h || a_matrix ends || ; || ; Предполагаем, что одно соединение хранится в || ; одой позиции и, следовательно, имеет два набора || ; параметров. Однако, возможно и большее количество || ; соединений. || ; || alp_deg struc || alpha_deg1 dd ? || alpha_deg2 dd ? || alp_deg ends || ; || tht_deg struc || theta_deg1 dd ? || theta_deg2 dd ? || tht_deg ends || ; || A_array struc || A1 dq ? || A2 dq ? || A_array ends || ; || D_array struc || D1 dq ? || D2 dq ? || D_array ends || ; || ; Сегмент данных - trans_data || ; || trans_data segment rw public || Amx a_matrix<> || Bmx a_matrix<> || Tmx a_matrix<> || ALPHA_DEG alp_deg<> || THETA_DEG tht_deg<> || A_VECTOR A_array<> || D_VECYOR D_array<> || ZERO dd 0 || d180 dd 180 || NUM_JOINT equ 1 || NUM_ROW equ 4 || NUM_COL equ 4 || REVERSE DB 1H || trans_data ends || || assume ds:trans_data, es:trans_data || ; || ; Сегмент trans_code содержит процедуры для || ; вычисления элементов матрицы и умножения матриц. || ; || trans_code segment er public || truns_proc proc far || ; || ; Вычислить углы альфа и тета в радианах из их значений || ; в градусах || ; || fldpi || fdiv d180 || ; || ; Умножить на Пи/180 || ; || fld st || fmul qword ptr ALPHA_DEG[ecx*8] || fxch st(1) || fmul qword ptr THETA_DEG[ecx*8] || ; || ; Тета (в радианах) в ST || ; и альфа (в радианах) в ST(1) || ; || ; Вычисление элементов матрицы || ; || ; a11 = COS тета || ; a12 = -COS альфа* SIN тета || ; a13 = SIN альфа * SIN тета || ; a14 = A * COS тета || ; a21 = SIN тета || ; a22 = COS альфа * COS тета || ; a23 = -SIN альфа * COS тета || ; a24 = A * SIN тета || ; a32 = SIN альфа || ; a33 = COS альфа || ; a34 = D || ; a31 = a41= a42 = a43 = 0.0 || ; a44 = 1 || ; || ; Регистр EBX содержит смещение для матрицы || ; || fsincos ; COS тета в ST || ; SIN тета в ST(1) || fld st ; удвоить COS тета || fst [ebx].a11 ; COS тета в a11 || fmul qword ptr A_VECTOR[ecx*8] || fstp [ebx].a14 ; A * COS тета в a14 || fxch st(1) ; SIN тета в ST || fst [ebx].a21 ; SIN тета в a21 || fld st ; удвоить SIN тета || fmul qword ptr A_VECTOR[ecx*8] || fstp [ebx].a24 ; A * SIN тета в a24 || fld st(2) ; альфа в ST || fsincos ; COS альфа в ST || ; SIN альфа в ST(1) || ; SIN тета в ST(2) || ; COS тета в ST(3) || fst [ebx].a33 ; COS альфа в a33 || fxch st(1) ; SIN альфа в ST || fst [ebx].a32 ; SIN альфа в a32 || fld st(2) ; SIN тета в ST || ; SIN альфа в ST(1) || fmul st,st(1) ; SIN альфа * SIN тета || fchs st(2) ; -COS тета * SIN альфа || fstp [ebx].a23 ; хранится в a23 || fld st(2) ; COS тета в ST || ; COS альфа в ST(1) || ; SIN тета в ST(2) || ; COS тета в ST(3) || fmul st,st(1) ; COS тета * COS альфа || fstp [ebx].a22 ; хранится в A22 || fmul st,st(1) ; COS альфа * SIN тета || ; || ; Для того, чтобы получить преимущества || ; параллельной работы IU и FPU || ; || push eax ; сохранить EAX || ; || ; Для ускорения поместить D в A34 || ; || mov eax, dword ptr D_VECTOR[ecx*8] || mov dword ptr [ebx + 88],eax || mov eax, dword ptr D_VECTOR[ecx*8 + 4] || mov dword ptr [ebx + 92],eax || pop eax ; восстановить EAX || fchs ; COS альфа * SIN тета || fstp [ebx].a12 ; хранится в a12 || ; Вычисляются все || ; ненулевые элементы || || ret || || trans_proc endp || || matrix_elem proc far || ; || ; Эта процедура вычисляет произведение i-той строки || ; первой матрицы и j-того столбца второй матрицы: || ; || ; Tij, где Tij = сумме произведений Aik x Bkj по k || ; || ; Параметры, передаваемые из вызвавшей процедуры || ; matrix_row: || ; || ; ESI = (i-1)*8 || ; EDI = (j-1)*8 || ; || ; локальный регистр, EBP = (k-1)*8 || ; || push ebp ; сохранить EBP || push ecx ; для того, чтобы использовать || ; ESX как временный регистр || mov ecx,esi ; сохранить для дальнейшего || ; индексирования || ; || ; Взять элемент в первой матрице, A || ; || imul ecx,NUM_COL ; ECX содержит смещение предыдущих || ; строк; смещение берется от начала || ; матрицы || || xor ebp,ebp ; Очистить EBP, который будет нужен || ; как временный регистр для индекса || ; (k) по i-той строке первой || ; матрицы и вниз по j-тому столбцу || ; второй матрицы. || ; || ; Очистить Tij для накапливания пар Aik*Bkj || ; || mov dword ptr [edx][edi],ebp || mov dword ptr [edx][edi+4],ebp || || push ecx ; сохранить в стеке: ESI * num_col || ; = смещению начала i-той строки от || ; начала матрицы A || || add ecx,ebp ; взять k-тый элемент i-той строки || ; матрицы A || ; || ; Загрузить Aik в FPU || ; || fld qword ptr [eax][ecx] || ; || ; Взять Bkj || ; || mov ecx,ebp || imul ecx,NUM_ROW ; ECX содержит смещение начала || ; k-той строки от начала матрицы B || add ecx,edi ; Взять j-тый элемент k-той строки || ; матрицы B || fmul qword ptr [ebx][ecx] ; Aik * Bkj || pop ecx ; ESI * num_col || ; в ECX снова || pop ecx ; также в вершине программного || ; стека || ; || ; Добавить к результату в выходной матрице, Tij || ; || add ecx,edi || ; || ; накопленные суммы пар Aik * Bkj || ; || fadd qword ptr [edx][ecx] || fstp qword ptr [edx][ecx] || ; || ; увеличить на единицу k, то есть || ; EBP на 8 || ; || add ebp,8 || ; || ; Достигло ли k ширины матрицы? || ; || cmp ebp, NUM_COL*8 || jl NXT_k || ; || ; Восстановить регистры || ; || pop ecx ; удалить из стека ESI * num_col || pop ecx ; восстановить ECX || pop ebp ; восстановить EBP || || matrix_elem endp || || matrix_row proc far || xor edi,edi || ; || ; Пройти по строке || ; || NXT_COL: || call matrix_ekem || add edi,8 || cmp edi, NUM_COL*8 || jl NXT_COL || ret || || matrix_row endp || || matrixmul_proc proc far || ; || ; Эта процедура перемножает матрицы, используя || ; процедуру matrix_row для вычисления элементов || ; каждой строки. || ; || ; Умножение матриц выполняется как: || ; || ; Tij = Aik x Bkj, || ; || ; где i и j указывают на номера строки и столбца || ; соответственно и k - индекс для прохода по i-той || ; строке первой матрицы и по j-тому столбцу второй || ; матрицы. || ; || mov ebp,esp ; использует базовый указатель || ; для индексирования || mov edx,dword ptr[ebp+4] ; смещение Tmx в EDX || mov ebx,dword ptr[ebp+8] ; смещение Bmx в EBX || mov eax, dword ptr[ebp+12] ; смещение Amx в EAX || ; || ; Установка ESI и EDI || ; EDI указывает столбец || ; ESI указывает строку || ; || xor esi,esi ; очистить ESI || || NXT_ROW: || call matrix_row || || add esi,8 || cmp esi,NUM_ROW*8 || jl NXT_ROW || ret 12 ; вытолкнуть указатели матрицы || || matrix_proc endp || || trans_code ends || || ; ********************************************** ; || ; ; || ; ; || ; ; || ; Главная программа ; || ; ; || ; ; || ; ; || ; ; || ; ********************************************** ; || || main_code segment er || || START: || || mov esp, stackstart trans_stack || ; || ; Сохранить все регистры || ; || pushad || ; || ; ECX указывает число соединений, где число матриц || ; равно NUM_JOINT + 1. || ; Найдем первую матрицу (от основания системы до || ; первого соединения) и назовем ее Bmx. || ; || xor ecx,ecx ; первой матрицей || mov ebx,offset Bmx ; || call trans_proc ; является Bmx || inc ecx || || NXT_MATRIX: || ; || ; Вторая матрица и выше будут сохраняться в Amx. || ; Результат умножения матриц будет сохраняться в || ; Tmx, но для следующего умножения будет доступен || ; через Bmx. Такое дублирование необходимо для || ; последовательного умножения матриц. Это || ; достигается заменой поррядка следования указателей || ; на Bmx и Tmx в программном стеке, хотя и является || ; невидимым для процедуры перемножения матриц. || ; REVERSE используется как индикатор; || ; если REVERSE = 0, то это значит, что результат || ; помещен в Tmx. || ; || mov ebx,offset Amx ; найти Amx || call trans_proc || inc ecx || xor REVERSE,1h || jnz Bmx_as_Tmx || ; || ; Не менять. Bmx используется как вторая входная || ; матрица, а Tmx - как выходная матрица. || ; || push offset Amx || push offset Bmx || push offset Tmx || jmp CONTINUE || ; || ; Поменять. Tmx используется как вторая входная || ; матрица, а Bmx - как выходная матрица. || ; || Bmx_as_Tmx: || || push offset Amx || push offset Tmx ; поменять переданные указатели || push offset Bmx || || CONTINUE: || || call matrixmul_proc || cmp ecx,NUM_JOINT || jle NXT_MATRIX || ; || ; Если REVERSE = 1, то конечный результат будет в || ; Bmx, а иначе в Tmx. || ; || popad || || main_code ends || || end START, ds:trans_data, ss:trans_stack || || |+-------------------------------------------------------------------------+
