Приложение В. Листинг консольной версии программы

#include "stdafx.h"

#include <stdio.h>

#include <time.h>

#include <math.h>

#include <stdarg.h>

#include "SETTINGS.h"

#include <conio.h> 

#include <windows.h>

#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;

       }

}

}

 

}