ПРИЛОЖЕНИЯ Приложение А. Принципиальная схема контроллера
Приложение Б. Пример фрагмента mdf файла ;6 НОГИЙ РОБОТ КОМАНДЫ
;-----------------------------------------------------------
; Команды
; имя макроса
; командная строка 1
; командная строка 2
; ...
; командная строка N
; #
;-----------------------------------------------------------
pause
; wait 1 0
; wait 2 0
sleep 2000
#
;-----------------------------------------------------------
;
;-----------------------------------------------------------
;
pip1
1 0 p 0
#
pip2
2 0 p 0
#
;-----------------------------------------------------------
;
;-----------------------------------------------------------
geta1
1 0 a 0
#
getall
geta1
#
;-----------------------------------------------------------
;
;-----------------------------------------------------------
ВсеИсх
1 0 w 6 0 0 0 0 0 0
2 0 w 6 0 0 0 0 0 0
#
;-----------------------------------------------------------
;ЛЕВАЯ ПЕРЕДНЯЯ НОГА
;-----------------------------------------------------------
ЛПИсх
1 0 t 2 0 24
1 0 t 2 1 0
#
ЛПИсхВстать
1 0 t 2 0 24
1 0 t 2 1 21
#
ЛПИсхПоднять
1 0 t 2 0 24
1 0 t 2 1 -60
#
ЛПИсхПодъем
1 0 t 2 0 24
1 0 t 2 1 33
Приложение В. Листинг консольной версии программы #include "stdafx.h"
#include
#include
#include
#include
#include "SETTINGS.h"
#include
#include
#include "Correction.h"
#include "Movements.h"
#include "xmpop.h"
#include "rs232lib.h"
#include "hxpdlib.h"
#include "COMPortLib.h" #pragma hdrstop
#pragma w char *Title = "\nHexapod_Console_2.0 SKB RoboTech\n";
String PortName = "COMX";
int ComPortSpeed = 57600; // Скорость
char *IniFile = "hxctl.ini";
char *MacroFile = "hxctl.mdf";
char *AdaptiveModeString = "Current mode is NON-Adaptive";
char *CorrectiveModeString = "Current mode is NON-Corrective";
char *AnalisesFileName;
String s; DCB dcb; //-----------------------------------------------------
char* pcComPort;
void TUsrComPort::Open()
// Разбираемся с COM-портом
{
int res;
Port = new TComPort(PortName, BaudRate, res);
Ready = (Port!=NULL && res!=0);
if(!Ready)
{
printf("\nres=",res);
if (Port==NULL) {printf("Port=NULL\n");}
if (res==0) { printf("\nres=%i",res);}
printf("\nUnable to open '%s' at %d",PortName, BaudRate);
}
else
printf("OK");
}
//----------------------------------------------------------
// Основная программа
//----------------------------------------------------------
void main(void)
{
printf("\n%s\n",Title);
//AnalisesFileName=GetCurrentDateAndTime();
AnalisesFileName="log.csv";
AnalisesFileHeaderWrite();
//--------------------------------------------------------
// Чтение файла конфигурации
//--------------------------------------------------------
FILE *f;
if((f=fopen(IniFile,"r"))==NULL)
Error("Open inifile '%s' error",IniFile); if(!SkipRemarkLine(f, PortName))
Error("Read PortName error"); if(!SkipRemarkLine(f, s))
Error("Read ComPort speed error");
ComPortSpeed = atoi(s); fclose(f); ReadMacroFile(MacroFile); printf("\nTry init port '%s' at %d... ",PortName, ComPortSpeed);
ComPort = new TUsrComPort(PortName,ComPortSpeed);
ComPort->Open();
printf("\nStart program\n"); //--------------------------------------------------------
// Основной цикл
//--------------------------------------------------------
int eoj = 0;
bool autoregime = false;
int sensors = 0;
int position = 0;
int sensors_leg = 0;
bool adaptive=false;
bool corrective=false;
int test_legs=0;
SetServoSpeed(8);
FirstPosition();
while(!eoj)
{
//------------------------------------------------------
// Автоматический режим
//------------------------------------------------------
if(autoregime)
{
// Sleep(1000);
ReadSensors();
CreateCorrection();
ReadSensors();
#define DIST 50
if(RDPACKAGE[SharpLeft]<=DIST && RDPACKAGE[SharpRight]<=DIST)
{
if(adaptive==0)
{
printf("StepFwd\n");
if (corrective==1)
{
CreateCorrection();
}
ExecCommand("StepFwd");
FirstPosition();
}
if(adaptive==1)
{
printf("StepFwdAn");
if (corrective==1)
{
CreateCorrection();
}
ExecCommand("StepFwdA");
}
}
else if(RDPACKAGE[SharpLeft]>=DIST && RDPACKAGE[SharpRight]>=DIST)
{
if(adaptive==0)
{
printf("StepBack2\n");
if (corrective==1)
{
CreateCorrection();
}
ExecCommand("StepBack2s");
FirstPosition();
}
if(adaptive==1)
{
printf("StepBackA\n");
if (corrective==1)
{
CreateCorrection();
}
ExecCommand("StepBackA");
}
}
else if(RDPACKAGE[SharpLeft]>=DIST)
{
if(adaptive==0)
{
printf("TurnRight\n");
if (corrective==1)
{
CreateCorrection();
}
ExecCommand("TurnRight");
FirstPosition();
}
if(adaptive==1)
{
printf("TurnRightA\n");
if (corrective==1)
{
CreateCorrection();
}
ExecCommand("TurnRightA");
}
}
else if(RDPACKAGE[SharpRight]>=DIST)
{
if(adaptive==0)
{
printf("TurnLeft\n");
if (corrective==1)
{
CreateCorrection();
}
ExecCommand("TurnLeft");
FirstPosition();
}
if(adaptive==1)
{
printf("TurnLeftA\n");
if (corrective==1)
{
CreateCorrection();
}
ExecCommand("TurnLeftA");
}
}
}
if(sensors)
{
Correction_Clear();
do
{
Sleep(1000);
ReadSensors();
cprintf("%3d", (int)RDPACKAGE[SharpRight]);
printf(" ");
cprintf("%3d", (int)RDPACKAGE[SharpLeft]);
printf(" ");
cprintf("%3d", (int)RDPACKAGE[AccelX]);
printf(" ");
cprintf("%3d", (int)RDPACKAGE[AccelY]);
printf(" ");
cprintf("%3d", (int)RDPACKAGE[AccelZ]);
printf(" ");
printf("\n");
sensors=0;
}
while (!kbhit());
}
if(sensors_leg)
{
Correction_Clear();
do
{
Sleep(1000);
ReadSensorsLegs();
cprintf("%3d", (int)RDPACKAGE[lp_leg]);
printf(" ");
cprintf("%3d", (int)RDPACKAGE[ls_leg]);
printf(" ");
cprintf("%3d", (int)RDPACKAGE[lz_leg]);
printf(" ");
cprintf("%3d", (int)RDPACKAGE[pp_leg]);
printf(" ");
cprintf("%3d", (int)RDPACKAGE[ps_leg]);
printf(" ");
cprintf("%3d", (int)RDPACKAGE[pz_leg]);
printf(" \n");
sensors_leg=0;
}
while (!kbhit());
}
if (test_legs)
{
ExecCommand("ЛПИсхПоднять");
ExecCommand("ЛЗИсхПоднять");
ExecCommand("ППИсхПоднять");
ExecCommand("ПЗИсхПоднять");
ExecCommand("ПСИсхПоднять");
ExecCommand("ЛСИсхПоднять");
Sleep(1400);
down(PUT_LP);
Sleep(1400);
down(PUT_LS);
Sleep(1400);
down(PUT_LZ);
Sleep(1400);
down(PUT_PP);
Sleep(1400);
down(PUT_PS);
Sleep(1400);
down(PUT_PZ);
test_legs=0;
} if(position)
{
Correction_Test();
}
//------------------------------------------------------
// Ручное управление
//------------------------------------------------------
char c;
if(kbhit())
{
Correction_Clear();
while(kbhit()) getch();
autoregime = 0;
printf("\nManual regime\n");
printf("~~~~~~~~~~~~~~~~~\n");
printf("%s \n",AdaptiveModeString);
printf("%s \n",CorrectiveModeString);
printf(" ESC - exit\n");
printf(" space - stop\n");
printf(" w,s,a,d - ctl\n");
printf(" w - movement forward\n");
printf(" s - movement back\n");
printf(" a - turn left\n");
printf(" d - turn right\n");
printf(" 1 - start autoregime\n");
printf(" 2 - sensors view\n");
printf(" 3 - correction\n");
printf(" 4 - tuch sensoes view\n");
printf(" 6 - set adaptive mode\n");
printf(" 7 - set non-adaptive mode\n");
printf(" 8 - set corrective mode\n");
printf(" 9 - set non-corrective mode\n");
printf(" 0 - set Servo Speed\n");
c = getch ();
switch(c)
{
case 27: eoj = 1; break;
case 'w':
if (corrective==1)
{
CreateCorrection();
}
if (adaptive==0)
{
if(ExecCommand("StepFwd")!=E_OK)
Warning("\n*** Exec command error\n");
FirstPosition();
break;
}
if (adaptive==1)
{
if(ExecCommand("StepFwdA")!=E_OK)
Warning("\n*** Exec command error\n");
if (corrective==1)
{
CreateCorrection();
}
break;
}
case 'a':
if (corrective==1)
{
CreateCorrection();
}
if (adaptive==0)
{
if(ExecCommand("TurnLeft")!=E_OK)
Warning("\n*** Exec command error\n");
FirstPosition();
break;
}
if (adaptive==1)
{
if(ExecCommand("TurnLeftA")!=E_OK)
Warning("\n*** Exec command error\n");
if(corrective==1)
{
CreateCorrection();
}
break;
}
case 'd':
if (corrective==1)
{
CreateCorrection();
}
if (adaptive==0)
{
if(ExecCommand("TurnRight")!=E_OK)
Warning("\n*** Exec command error\n");
FirstPosition();
break;
}
if (adaptive==1)
{
if(ExecCommand("TurnRightA")!=E_OK)
Warning("\n*** Exec command error\n");
if(corrective==1)
{
CreateCorrection();
}
break;
}
case 's':
if (corrective==1)
{
CreateCorrection();
}
if (adaptive==0)
{
if(ExecCommand("StepBack2")!=E_OK)
Warning("\n*** Exec command error\n");
FirstPosition();
break;
}
if (adaptive==1)
{
if(ExecCommand("StepBackA")!=E_OK)
Warning("\n*** Exec command error\n");
if (corrective==1)
{
CreateCorrection();
}
break;
}
case 'j':
if(ExecCommand("GOUP")!=E_OK)
Warning("\n*** Exec command error\n");
break;
case ' ':
FirstPosition();
break;
case '1':
printf("\nStart auto regime\n");
autoregime = 1;
break;
case '2':
printf("\nSensors view\n");
sensors=1;
break;
case '3':
printf("\nPosition\n");
position=1;
break;
case '4':
printf("\nSensors view\n");
sensors_leg=1;
break;
case '6':
adaptive=1;
printf("\nAdaptive mode set");
AdaptiveModeString = "Current mode is Adaptive";
break;
case '7':
adaptive=0;
printf("\nNON-Adaptive mode set");
AdaptiveModeString = "Current mode is NON-Adaptive";
break;
case '8':
corrective=1;
printf("\nCorrective mode set");
CorrectiveModeString = "Current mode is Corrective";
break;
case '9':
corrective=0;
printf("\nNON-Corrective mode set");
CorrectiveModeString = "Current mode is NON-Corrective";
break;
case '0':
printf("\nEnter servo speed between 2 and 10: ");
int speed;
scanf("%i",&speed);
SetServoSpeed(speed);
printf("%s %i","Servo speed setted",speed);
break;
}
}
}
}
|