Приложение Г. Листинг библиотеки адаптивного опускания ног 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");

}