Приложение Г. Листинг библиотеки адаптивного опускания ног Movemets.cpp
#include "StdAfx.h"
#include "Movements.h"
char* down(char mask, bool write)
{
int sens_lp=0;
int sens_ls=0;
int sens_lz=0;
int sens_pp=0;
int sens_ps=0;
int sens_pz=0;
int position_lp=Current_position.lp_vertical;
int position_ls=Current_position.ls_vertical;
int position_lz=Current_position.lz_vertical;
int position_pp=Current_position.pp_vertical;
int position_ps=Current_position.ps_vertical;
int position_pz=Current_position.pz_vertical;
bool DownLpComplete=true;
bool DownLsComplete=true;
bool DownLzComplete=true;
bool DownPpComplete=true;
bool DownPsComplete=true;
bool DownPzComplete=true;
if (mask & PUT_LP)
DownLpComplete=false;
if (mask & PUT_LS)
DownLsComplete=false;
if (mask & PUT_LZ)
DownLzComplete=false;
if (mask & PUT_PP)
DownPpComplete=false;
if (mask & PUT_PS)
DownPsComplete=false;
if (mask & PUT_PZ)
DownPzComplete=false;
bool Implementation_complete=false;
int ExCodeLp=0;
int ExCodeLs=0;
int ExCodeLz=0;
int ExCodePp=0;
int ExCodePs=0;
int ExCodePz=0;
while (Implementation_complete==false)
{
if(DownLpComplete==false)
{
ReadSensorsLegs();
sens_lp=RDPACKAGE[lp_leg];
if ((sens_lp!=255)&&(position_lp<50))
{
position_lp+=corr_step;
AddAgrumentToCommand(Lp_Leg,position_lp);
}
if ((sens_lp==255)&&(position_lp<=52))
{
ExCodeLp=1;
DownLpComplete=true;
}
if ((sens_lp!=255)&&(position_lp>=50))
{
ExCodeLp=2;
DownLpComplete=true;
}
}
if(DownLsComplete==false)
{
ReadSensorsLegs();
sens_ls=RDPACKAGE[ls_leg];
if ((sens_ls!=255)&&(position_ls<50))
{
position_ls+=corr_step;
AddAgrumentToCommand(Ls_Leg,position_ls);
}
if ((sens_ls==255)&&(position_ls<=52))
{
ExCodeLs=1;
DownLsComplete=true;
}
if ((sens_ls!=255)&&(position_ls>=50))
{
ExCodeLs=2;
DownLsComplete=true;
}
}
if(DownLzComplete==false)
{
ReadSensorsLegs();
sens_lz=RDPACKAGE[lz_leg];
if ((sens_lz!=255)&&(position_lz<50))
{
position_lz+=corr_step;
AddAgrumentToCommand(Lz_Leg,position_lz);
}
if ((sens_lz==255)&&(position_lz<=52))
{
ExCodeLz=1;
DownLzComplete=true;
}
if ((sens_lz!=255)&&(position_lz>=50))
{
ExCodeLz=2;
DownLzComplete=true;
}
}
if(DownPpComplete==false)
{
ReadSensorsLegs();
sens_pp=RDPACKAGE[pp_leg];
if ((sens_pp!=255)&&(position_pp>-50))
{
position_pp-=corr_step;
AddAgrumentToCommand(Pp_Leg,position_pp);
}
if ((sens_pp==255)&&(position_pp>=-52))
{
ExCodePp=1;
DownPpComplete=true;
}
if ((sens_pp!=255)&&(position_pp<=-50))
{
ExCodePp=2;
DownPpComplete=true;
}
}
if(DownPsComplete==false)
{
ReadSensorsLegs();
sens_ps=RDPACKAGE[ps_leg];
if ((sens_ps!=255)&&(position_ps>-50))
{
position_ps-=corr_step;
AddAgrumentToCommand(Ps_Leg,position_ps);
}
if ((sens_ps==255)&&(position_ps>=-52))
{
ExCodePs=1;
DownPsComplete=true;
}
if ((sens_ps!=255)&&(position_ps<=-50))
{
ExCodePs=2;
DownPsComplete=true;
}
}
if(DownPzComplete==false)
{
ReadSensorsLegs();
sens_pz=RDPACKAGE[pz_leg];
if ((sens_pz!=255)&&(position_pz>-50))
{
position_pz-=corr_step;
AddAgrumentToCommand(Pz_Leg,position_pz);
}
if ((sens_pz==255)&&(position_pz>=-52))
{
ExCodePz=1;
DownPzComplete=true;
}
if ((sens_pz!=255)&&(position_pz<=-50))
{
ExCodePz=2;
DownPzComplete=true;
}
}
Implementation_complete=DownLpComplete && DownLsComplete && DownLzComplete && DownPpComplete && DownPsComplete && DownPzComplete;
}
String ExCode ="";
sprintf(ExCode,"%i%i%i%i%i%i",ExCodeLp,ExCodeLs,ExCodeLz,ExCodePp,ExCodePs,ExCodePz);
if (write)
{ReadSensorsLegs();
sens_lp=RDPACKAGE[lp_leg];
sens_ls=RDPACKAGE[ls_leg];
sens_lz=RDPACKAGE[lz_leg];
sens_pp=RDPACKAGE[pp_leg];
sens_ps=RDPACKAGE[ps_leg];
sens_pz=RDPACKAGE[pz_leg];
ReadSensors();
int accelx=(int)RDPACKAGE[AccelX];
int accely=sensmedium();
Current_sensors.touch_lp=sens_lp;
Current_sensors.touch_ls=sens_ls;
Current_sensors.touch_lz=sens_lz;
Current_sensors.touch_pp=sens_pp;
Current_sensors.touch_ps=sens_ps;
Current_sensors.touch_pz=sens_pz;
Current_sensors.accel_x=accelx;
Current_sensors.accel_y=accely;
if ((Current_position.lp_vertical!=-60)&&(Current_position.ls_vertical!=-60)&&(Current_position.lz_vertical!=-60)&&(Current_position.pp_vertical!=60)&&(Current_position.ps_vertical!=60)&&(Current_position.pz_vertical!=60))
{
AnalisesFileWrite();
}
}
return ExCode;
}
void HorizontalCorrection(int LegNumber,int direction)
{
switch (LegNumber)
{
case 1:
{
int horizontal_position_lp=Current_position.lp_horizontal;
if (direction==0)
horizontal_position_lp+=15;
else
horizontal_position_lp-=15;
AddAgrumentToCommand(Lp_Leg_h,horizontal_position_lp);
}
case 2:
{
int horizontal_position_ls=Current_position.ls_horizontal;
if (direction==0)
horizontal_position_ls+=15;
else
horizontal_position_ls-=15;
AddAgrumentToCommand(Ls_Leg_h,horizontal_position_ls);
}
case 3:
{
int horizontal_position_lz=Current_position.lz_horizontal;
if (direction==0)
horizontal_position_lz-=15;
else
horizontal_position_lz+=15;
AddAgrumentToCommand(Lz_Leg_h,horizontal_position_lz);
}
case 4:
{
int horizontal_position_pp=Current_position.pp_horizontal;
if (direction==0)
horizontal_position_pp-=15;
else
horizontal_position_pp+=15;
AddAgrumentToCommand(Lp_Leg_h,horizontal_position_pp);
}
case 5:
{
int horizontal_position_ps=Current_position.ps_horizontal;
if (direction==0)
horizontal_position_ps+=15;
else
horizontal_position_ps-=15;
AddAgrumentToCommand(Lp_Leg_h,horizontal_position_ps);
}
case 6:
{
int horizontal_position_pz=Current_position.pz_horizontal;
if (direction==0)
horizontal_position_pz+=15;
else
horizontal_position_pz-=15;
AddAgrumentToCommand(Lp_Leg_h,horizontal_position_pz);
}
}
}
void Up(int LegNumber)
{
switch (LegNumber)
{
case 1:
{
AddAgrumentToCommand(Lp_Leg,-60);
break;
}
case 2:
{
AddAgrumentToCommand(Ls_Leg,-60);
break;
}
case 3:
{
AddAgrumentToCommand(Lz_Leg,-60);
break;
}
case 4:
{
AddAgrumentToCommand(Pp_Leg,60);
break;
}
case 5:
{
AddAgrumentToCommand(Ps_Leg,-60);
break;
}
case 6:
{
AddAgrumentToCommand(Pz_Leg,-60);
break;
}
}
}
void DownWithSearch(bool search,char mask)
{
if (search==false)
{
down(mask);
return;
}
else
{
char * ReturnCode=down(mask);
bool LpHorVal=(ReturnCode[0]==2);
bool LsHorVal=(ReturnCode[1]==2);
bool LzHorVal=(ReturnCode[2]==2);
bool PpHorVal=(ReturnCode[3]==2);
bool PsHorVal=(ReturnCode[4]==2);
bool PzHorVal=(ReturnCode[5]==2);
if (LpHorVal==true)
{
Up(1);
HorizontalCorrection(1,0);
down(PUT_LP);
}
if (LsHorVal==true)
{
Up(2);
HorizontalCorrection(2,0);
down(PUT_LS);
}
if (LzHorVal==true)
{
Up(3);
HorizontalCorrection(3,0);
down(PUT_LZ);
}
if (PpHorVal==true)
{
Up(4);
HorizontalCorrection(4,0);
down(PUT_PP);
}
if (PsHorVal==true)
{
Up(5);
HorizontalCorrection(5,0);
down(PUT_PS);
}
if (PzHorVal==true)
{
Up(6);
HorizontalCorrection(6,0);
down(PUT_PZ);
}
}
}
void PUT1()
{
down(PUT_PZ);
}
void FirstPosition()
{
ExecCommand("1 0 t 2 3 -27");
ExecCommand("2 0 t 2 3 -15");
ExecCommand("2 0 t 2 5 -20");
ExecCommand("1 0 t 2 5 26");
ExecCommand("2 0 t 2 1 26");
ExecCommand("1 0 t 2 1 21");
}
Приложение Д. Структура регистров и команд микроконтроллера
Таблица 1. Регистры микроконтроллера
Регистр
|
Название
|
Значение
|
Значение по умолчанию
|
R[0]
|
REG_ASTEP
|
Шаг угла поворота сервопривода или шаг изменения скорости. Определяет скорость вращения.
|
2 (0x02)
|
R[1]
|
REG_SBLIM
|
Пороговое значение блокирующего датчика
|
255 (0xFF)
|
R[2]
|
REG_SBMASK
|
Маска номеров блокирующих датчиков (АЦП 0 - 7)
|
0 (0x00)
|
R[3]
|
REG_STAT
|
Регистр статуса исполнительных устройств (i-й разряд: 0 - устройство i готово, 1 - в работе)
|
|
R[4]
|
REG_USR0
|
|
|
R[5]
|
REG_USR1
|
|
|
R[6]
|
REG_USR2
|
|
|
R[7]
|
REG_USR3
|
|
|
Таблица 2.Команды микроконтроллера
Команда (символ)
|
Обозначение
|
Аргументы
|
Описание
|
‘a’
|
CMD_READ_SENS
|
нет
|
Опрос АЦП 0..7.
Реакцией на эту команду является ответ в виде команды 's'
|
‘s’
|
CMD_SENS_COND
|
n V0 … V7
|
Ответ на запрос значений АЦП 0-7 (команда ‘a’) или на запрос значений регистров R0-R7 (команда ‘g’)
В этой команде n=8
V0 … V7 - передаваемые значения
|
‘p’
|
CMD_PIР
|
нет
|
Однократный звуковой сигнал (пищалка)
|
‘x’
|
CMD_ACK
|
нет
|
Подтверждение приема команды
|
'f'
|
CMD_STOP
|
нет
|
Останов всех сервоприводов
|
'i'
|
CMD_SET_INT
|
mask val
|
Прервать выполнение операции по состоянию датчика.
Аргументы: <�маска датчиков> <�значение>
|
Работа с отдельными приводами
|
‘z’
|
CMD_STARTPOS
|
нет
|
Установить приводы в исходное положение
|
‘n’
|
CMD_SET_FAST
|
num value
|
Быстрый поворот привода num на угол value.
value – значение угла в градусах: -60..+60.
num – номер сервопривода: 0..5
|
‘t’
|
CMD_SET_SLOW
|
num value
|
Медленный поворот привода num на угол value.
value – значение угла в градусах: -60..+60.
num – номер сервопривода: 0..5
Для ходового контроллера:
value – скорость в % (-100..100).
|
Векторные операции
|
‘w’
|
CMD_SET_VECT_SLOW
|
n A0 ... An-1
|
Медленный поворот сервоприводов
0..n-1 на углы A0…An-1.
|
‘v’
|
CMD_SET_VECT_FAST
|
n A0 ... An-1
|
Быстрый поворот сервоприводов
0..n-1 на углы A0…An-1.
|
Операции с регистрами
|
‘r’
|
CMD_SET_REG
|
num value
|
Установить значение регистра R[num]
num – номер регистра (0..7)
value – устанавливаемое значение.
|
‘g’
|
CMD_GET_REG
|
нет
|
Выдать значения регистров R0-R7
Реакцией на эту команду является ответ в виде команды 's' (CMD_SENS_COND)
|
Регистры REG_SBLIM и REG_SBMASK. Эта пара регистров определяет условия аварийного останова контроллера. Разряды регистра REG_SBMASK задают номера блокирующих датчиков (АЦП 0 - 7). На каждом такте работы система считывает значения сигналов с датчиков (АЦП), номера которых определяются разрядами REG_SBMASK и сравнивает со значением, находящимся в регистре REG_SBLIM. Если считанное значение превышает пороговое значение, то контроллер прекращает отработку управляющих сигналов.
Приложение Е. Краткое руководство пользователя визуальной версии программы
1. Предварительная подготовка
Перед запуском программы требуется ввести адрес COM-порта в файл конфигурации hxctl.ini. Настройка находится в секции “Параметры COM-порта”.
2.Запуск программы.
3. Установление связи робота с ПК.
Чтобы начать управление роботом требуется установить подключение. Для этого необходимо в секции COM_ port_connection нажать кнопку Connect.
4.Выбор режима передвижения.
Для управления роботом следует выбрать режим передвижения. Переключение режимов передвижения производится в секции Mode. В первой подсекции «Adaptive» можно с помощью переключателя выбрать режим адаптивного передвижения (Adaptive) и режим неадаптивного передвижения (No Adaptive) . Во второй подсекции «Correction» можно с помощью переключателя выбрать режим передвижения «с выравниванием по акселерометру» (Correction) или «без выравнивания по акселерометру» (No Correction) .
Если режим передвижения явным образом не выбран, то по умолчанию вбирается неадаптивный режим без выравнивая по акселерометру.
5.Запсук движения робота.
Робот можно запустить в автоматическом и ручном режимах. Для запуска автоматического режима необходимо нажать кнопку START в секции Automatic. В этом случае робот начнет двигаться , ориентируясь по показаниям дальномеров, бесконечно долго, до тех пор пока не будет нажата кнопка STOP. Для запуска ручного режима необходимо нажать оду из четырех кнопок в секции Manul. Стрелки на кнопках обозначают направление передвижения (движение вперед, движение назад, поворот направо, поворот налево). В этом режиме робот совершит ровно одно движение согласно выбранному направлению, а затем остановится. После этого можно заново выбрать режим передвижения робота и запустить его в ручном или автоматическом режимах.
7.Остановка движения
Для остановки робота в любой момент движения необходимо нажать копку STOP. После нажатия кнопки можно выбрать другой режим передвижения и заново запустить движение. Также можно завершить работу с программой, закрыв основное окно программы.
|